diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 30332dafed..b17328dc54 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 bool lock_memory = cm->get_parameter_or("lock_memory", true); std::string message; if (lock_memory && !realtime_tools::lock_memory(message)) @@ -69,7 +71,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()) { @@ -121,7 +123,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 f9e1dcc858..06e1d8543e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,6 +17,7 @@ For details see the controller_manager section. 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 `_).