diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 2fc5f55ec9..3200fc926e 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -44,6 +44,8 @@ int main(int argc, char ** argv) auto cm = std::make_shared(executor, manager_node_name); + const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); + const int cpu_affinity = cm->get_parameter_or("cpu_affinity", -1); if (cpu_affinity >= 0) { @@ -68,7 +70,7 @@ int main(int argc, char ** argv) thread_priority); std::thread cm_thread( - [cm, thread_priority]() + [cm, thread_priority, use_sim_time]() { if (realtime_tools::has_realtime_kernel()) { @@ -120,7 +122,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) + { + cm->get_clock()->sleep_until(current_time + period); + } + else + { + std::this_thread::sleep_until(next_iteration_time); + } } }); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index baf340336f..5ce3cda5fa 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -16,6 +16,7 @@ controller_interface controller_manager ****************** +* ``ros2_control_node`` can now handle the sim time used by different simulators, when ``use_sim_time`` is set to true (`#1810 `_). * The ``ros2_control_node`` node now accepts the ``thread_priority`` parameter to set the scheduler priority of the controller_manager's RT thread (`#1820 `_). * Added support for the wildcard entries for the controller configuration files (`#1724 `_). * The ``ros2_control_node`` node has a new ``lock_memory`` parameter to lock memory at startup to physical RAM in order to avoid page faults (`#1822 `_).