From 246885f9aab4d6910110311f045768c4c8a7fca4 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Fri, 8 Nov 2024 20:28:05 +0100
Subject: [PATCH] [ros2_control_node] Handle simulation environment clocks
 (backport #1810) (#1862)

---
 controller_manager/src/ros2_control_node.cpp | 13 +++++++++++--
 doc/release_notes.rst                        |  1 +
 2 files changed, 12 insertions(+), 2 deletions(-)

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<controller_manager::ControllerManager>(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<int>("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 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1820>`_).
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/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 <https://github.com/ros-controls/ros2_control/pull/1822>`_).