From d714e8bfe521c653d82e60ec2b47e5a0d5083863 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Wed, 6 Nov 2024 20:10:39 +0100
Subject: [PATCH] [ros2_control_node] Handle simulation environment clocks
 (#1810)

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

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 51e7e83d23..0ed68d9765 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -57,6 +57,9 @@ int main(int argc, char ** argv)
   auto cm = std::make_shared<controller_manager::ControllerManager>(
     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());
+
   const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
   std::string message;
   if (lock_memory && !realtime_tools::lock_memory(message))
@@ -82,7 +85,7 @@ int main(int argc, char ** argv)
     thread_priority);
 
   std::thread cm_thread(
-    [cm, thread_priority]()
+    [cm, thread_priority, use_sim_time, &rate]()
     {
       if (!realtime_tools::configure_sched_fifo(thread_priority))
       {
@@ -123,7 +126,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();
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 12ded009dc..b11a1351fe 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -76,6 +76,7 @@ controller_manager
 * The ``--controller-type`` or ``-t`` spawner arg is removed. Now the controller type is defined in the controller configuration file with ``type`` field (`#1639 <https://github.com/ros-controls/ros2_control/pull/1639>`_).
 * The ``--namespace`` or ``-n`` spawner arg is deprecated. Now the spawner namespace can be defined using the ROS 2 standard way (`#1640 <https://github.com/ros-controls/ros2_control/pull/1640>`_).
 * Added support for the wildcard entries for the controller configuration files (`#1724 <https://github.com/ros-controls/ros2_control/pull/1724>`_).
+* ``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>`_).
 * 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>`_).
 * The ``ros2_control_node`` node has a new ``cpu_affinity`` parameter to bind the process to a specific CPU core. By default, this is not enabled. (`#1852 <https://github.com/ros-controls/ros2_control/pull/1852>`_).