1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00

added ros controller reset when robot program isn't running

This commit is contained in:
Tristan Schnell
2019-06-11 13:59:03 +02:00
parent 3f22a2479a
commit 6337d4a374

View File

@@ -139,6 +139,7 @@ int main(int argc, char** argv)
double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency())); double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
ROS_INFO("Starting control loop");
// Run as fast as possible // Run as fast as possible
while (ros::ok()) while (ros::ok())
{ {
@@ -151,7 +152,7 @@ int main(int argc, char** argv)
period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count()); period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
stopwatch_last = stopwatch_now; stopwatch_last = stopwatch_now;
cm.update(timestamp, period); cm.update(timestamp, period, !g_hw_interface->isRobotProgramRunning());
g_hw_interface->write(timestamp, period); g_hw_interface->write(timestamp, period);
// if (!control_rate.sleep()) // if (!control_rate.sleep())