diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index e0094b7e01..953e26e5bb 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -57,10 +57,12 @@ int main(int argc, char ** argv) auto cm = std::make_shared( executor, manager_node_name, "", cm_node_options); + const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + rclcpp::Rate rate(cm->get_update_rate(), cm->get_clock()); RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); std::thread cm_thread( - [cm]() + [cm, use_sim_time, &rate]() { if (!realtime_tools::configure_sched_fifo(kSchedPriority)) { @@ -101,7 +103,14 @@ int main(int argc, char ** argv) // wait until we hit the end of the period next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); + if (use_sim_time) + { + rate.sleep(); + } + else + { + std::this_thread::sleep_until(next_iteration_time); + } } cm->shutdown_async_controllers_and_components();