From 1dcf13fa68835284e867a1780f5a9624925b7e04 Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Sun, 10 Nov 2024 20:24:46 +0100
Subject: [PATCH] [ros2_control_node] Handle simulation environment clocks
 (backport #1810) (#1863)

---
 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 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<controller_manager::ControllerManager>(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<bool>("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 <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>`_).