From 0be43748de967d1d410385411baf45107145d24d Mon Sep 17 00:00:00 2001
From: "github-actions[bot]"
 <41898282+github-actions[bot]@users.noreply.github.com>
Date: Fri, 1 Nov 2024 09:50:10 +0100
Subject: [PATCH 1/9] Bump version of pre-commit hooks (#1831)

Co-authored-by: christophfroehlich <3367244+christophfroehlich@users.noreply.github.com>
---
 .pre-commit-config.yaml | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 63e7f08682..205e0f63ab 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -16,7 +16,7 @@
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.6.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
       - id: check-ast
@@ -37,7 +37,7 @@ repos:
 
   # Python hooks
   - repo: https://github.com/asottile/pyupgrade
-    rev: v3.17.0
+    rev: v3.19.0
     hooks:
     -   id: pyupgrade
         args: [--py36-plus]
@@ -50,7 +50,7 @@ repos:
       args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
 
   - repo: https://github.com/psf/black
-    rev: 24.8.0
+    rev: 24.10.0
     hooks:
       - id: black
         args: ["--line-length=99"]
@@ -63,7 +63,7 @@ repos:
 
   # CPP hooks
   - repo: https://github.com/pre-commit/mirrors-clang-format
-    rev: v19.1.0
+    rev: v19.1.3
     hooks:
       - id: clang-format
         args: ['-fallback-style=none', '-i']
@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.3
+    rev: 0.29.4
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

From 58805d35b75a19ff289c2dfd1582c8a353368980 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Sun, 3 Nov 2024 18:05:36 +0100
Subject: [PATCH 2/9] Update mergify.yml author entries (#1841)

---
 .github/mergify.yml | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/.github/mergify.yml b/.github/mergify.yml
index 39ee6b6bc0..fd185e02d0 100644
--- a/.github/mergify.yml
+++ b/.github/mergify.yml
@@ -32,7 +32,7 @@ pull_request_rules:
       - author=mergify[bot]
     actions:
         comment:
-          message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich?
+          message: This pull request is in conflict. Could you fix it @bmagyar @destogl @christophfroehlich @saikishor?
 
   - name: development targets master branch
     conditions:
@@ -40,6 +40,7 @@ pull_request_rules:
       - author!=bmagyar
       - author!=destogl
       - author!=christophfroehlich
+      - author!=saikishor
       - author!=mergify[bot]
       - author!=dependabot[bot]
     actions:

From be0a339ac447cf259b560f13f634dd0ced6028ff Mon Sep 17 00:00:00 2001
From: Baris Yazici <barisyazici@sabanciuniv.edu>
Date: Mon, 4 Nov 2024 17:18:19 +0100
Subject: [PATCH 3/9] fix: typo use thread_priority (#1844)

---
 controller_manager/src/ros2_control_node.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 13af864c2f..c0a61d6cda 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -79,7 +79,7 @@ int main(int argc, char ** argv)
       {
         RCLCPP_INFO(
           cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.",
-          kSchedPriority);
+          thread_priority);
       }
 
       // for calculating sleep time

From 434f7f54c91114028b8f73c927ecee41588f2bce Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?=
 <christophfroehlich@users.noreply.github.com>
Date: Mon, 4 Nov 2024 19:44:34 +0100
Subject: [PATCH 4/9] Fix CMP0115 (#1830)

---
 controller_manager/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index a1a98bc59a..6e004ffc4a 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -195,7 +195,7 @@ if(BUILD_TESTING)
   )
 
   ament_add_gmock(test_hardware_spawner
-    test/test_hardware_spawner
+    test/test_hardware_spawner.cpp
     TIMEOUT 120
   )
   target_link_libraries(test_hardware_spawner

From e6e69c1052b91d52de792f49d89c6c54bf4ec18a Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Mon, 4 Nov 2024 20:53:55 +0100
Subject: [PATCH 5/9] [ros2_control_node] Add the realtime_tools lock_memory
 method to prevent page faults (#1822)

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

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index c0a61d6cda..6de49166a9 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -57,6 +57,13 @@ int main(int argc, char ** argv)
   auto cm = std::make_shared<controller_manager::ControllerManager>(
     executor, manager_node_name, "", cm_node_options);
 
+  const bool lock_memory = cm->get_parameter_or<bool>("lock_memory", true);
+  std::string message;
+  if (lock_memory && !realtime_tools::lock_memory(message))
+  {
+    RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
+  }
+
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
   const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
   RCLCPP_INFO(
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index a7c1242ffc..9b70016184 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -77,6 +77,7 @@ controller_manager
 * 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>`_).
 * 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>`_).
 
 hardware_interface
 ******************

From 98f079598a6444d7043236c2e9fec04d1f7dae02 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 5 Nov 2024 11:08:17 +0100
Subject: [PATCH 6/9] [ros2_control_node] Add option to set the CPU affinity 
 (#1852)

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

diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
index 6de49166a9..51e7e83d23 100644
--- a/controller_manager/src/ros2_control_node.cpp
+++ b/controller_manager/src/ros2_control_node.cpp
@@ -64,6 +64,17 @@ int main(int argc, char ** argv)
     RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory : '%s'", message.c_str());
   }
 
+  const int cpu_affinity = cm->get_parameter_or<int>("cpu_affinity", -1);
+  if (cpu_affinity >= 0)
+  {
+    const auto affinity_result = realtime_tools::set_current_thread_affinity(cpu_affinity);
+    if (!affinity_result.first)
+    {
+      RCLCPP_WARN(
+        cm->get_logger(), "Unable to set the CPU affinity : '%s'", affinity_result.second.c_str());
+    }
+  }
+
   RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate());
   const int thread_priority = cm->get_parameter_or<int>("thread_priority", kSchedPriority);
   RCLCPP_INFO(
diff --git a/doc/release_notes.rst b/doc/release_notes.rst
index 9b70016184..12ded009dc 100644
--- a/doc/release_notes.rst
+++ b/doc/release_notes.rst
@@ -78,6 +78,7 @@ controller_manager
 * 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 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>`_).
 
 hardware_interface
 ******************

From 814137dd3a87ac21c70c84420c985e436e1f5f7c Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Tue, 5 Nov 2024 13:40:17 +0100
Subject: [PATCH 7/9] [CM] Fix controller missing update cycles in a real setup
 (#1774)

---
 .../controller_interface_base.hpp             |  2 +-
 controller_manager/CMakeLists.txt             |  1 +
 .../controller_manager/controller_manager.hpp |  2 +-
 .../controller_manager/controller_spec.hpp    |  2 +-
 controller_manager/src/controller_manager.cpp | 34 ++++----
 .../test/test_controller_manager.cpp          | 77 ++++++++++---------
 6 files changed, 66 insertions(+), 52 deletions(-)

diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp
index 211716f301..ae3804919c 100644
--- a/controller_interface/include/controller_interface/controller_interface_base.hpp
+++ b/controller_interface/include/controller_interface/controller_interface_base.hpp
@@ -160,7 +160,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    * **The method called in the (real-time) control loop.**
    *
    * \param[in] time The time at the start of this control loop iteration
-   * \param[in] period The measured time taken by the last control loop iteration
+   * \param[in] period The measured time since the last control loop iteration
    * \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
    */
   CONTROLLER_INTERFACE_PUBLIC
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
index 6e004ffc4a..1bb84eb32c 100644
--- a/controller_manager/CMakeLists.txt
+++ b/controller_manager/CMakeLists.txt
@@ -87,6 +87,7 @@ if(BUILD_TESTING)
 
   ament_add_gmock(test_controller_manager
     test/test_controller_manager.cpp
+    TIMEOUT 180
   )
   target_link_libraries(test_controller_manager
     controller_manager
diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp
index 6c0b4fde9b..068eefc1f9 100644
--- a/controller_manager/include/controller_manager/controller_manager.hpp
+++ b/controller_manager/include/controller_manager/controller_manager.hpp
@@ -123,7 +123,7 @@ class ControllerManager : public rclcpp::Node
     controller_spec.c = controller;
     controller_spec.info.name = controller_name;
     controller_spec.info.type = controller_type;
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     return add_controller_impl(controller_spec);
   }
 
diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp
index 9ce1aab8b6..0f44867814 100644
--- a/controller_manager/include/controller_manager/controller_spec.hpp
+++ b/controller_manager/include/controller_manager/controller_spec.hpp
@@ -37,7 +37,7 @@ struct ControllerSpec
 {
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
-  std::shared_ptr<rclcpp::Time> next_update_cycle_time;
+  std::shared_ptr<rclcpp::Time> last_update_cycle_time;
 };
 
 struct ControllerChainSpec
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index a8668f7f1b..d7e924a863 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -543,7 +543,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
   controller_spec.c = controller;
   controller_spec.info.name = controller_name;
   controller_spec.info.type = controller_type;
-  controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(
+  controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(
     0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
   // We have to fetch the parameters_file at the time of loading the controller, because this way we
@@ -1668,8 +1668,8 @@ void ControllerManager::activate_controllers(
       continue;
     }
     auto controller = found_it->c;
-    // reset the next update cycle time for newly activated controllers
-    *found_it->next_update_cycle_time =
+    // reset the last update cycle time for newly activated controllers
+    *found_it->last_update_cycle_time =
       rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type());
 
     bool assignment_successful = true;
@@ -2354,21 +2354,31 @@ controller_interface::return_type ControllerManager::update(
         run_controller_at_cm_rate ? period
                                   : rclcpp::Duration::from_seconds((1.0 / controller_update_rate));
 
+      const rclcpp::Time current_time = get_clock()->now();
       if (
-        *loaded_controller.next_update_cycle_time ==
+        *loaded_controller.last_update_cycle_time ==
         rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()))
       {
         // it is zero after activation
+        *loaded_controller.last_update_cycle_time = current_time - controller_period;
         RCLCPP_DEBUG(
-          get_logger(), "Setting next_update_cycle_time to %fs for the controller : %s",
-          time.seconds(), loaded_controller.info.name.c_str());
-        *loaded_controller.next_update_cycle_time = time;
+          get_logger(), "Setting last_update_cycle_time to %fs for the controller : %s",
+          loaded_controller.last_update_cycle_time->seconds(), loaded_controller.info.name.c_str());
       }
-
-      bool controller_go =
+      const auto controller_actual_period =
+        (current_time - *loaded_controller.last_update_cycle_time);
+
+      /// @note The factor 0.99 is used to avoid the controllers skipping update cycles due to the
+      /// jitter in the system sleep cycles.
+      // For instance, A controller running at 50 Hz and the CM running at 100Hz, then when we have
+      // an update cycle at 0.019s (ideally, the controller should only trigger >= 0.02s), if we
+      // wait for next cycle, then trigger will happen at ~0.029 sec and this is creating an issue
+      // to keep up with the controller update rate (see issue #1769).
+      const bool controller_go =
+        run_controller_at_cm_rate ||
         (time ==
          rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) ||
-        (time.seconds() >= loaded_controller.next_update_cycle_time->seconds());
+        (controller_actual_period.seconds() * controller_update_rate >= 0.99);
 
       RCLCPP_DEBUG(
         get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
@@ -2377,8 +2387,6 @@ controller_interface::return_type ControllerManager::update(
 
       if (controller_go)
       {
-        const auto controller_actual_period =
-          (time - *loaded_controller.next_update_cycle_time) + controller_period;
         auto controller_ret = controller_interface::return_type::OK;
         bool trigger_status = true;
         // Catch exceptions thrown by the controller update function
@@ -2402,7 +2410,7 @@ controller_interface::return_type ControllerManager::update(
           controller_ret = controller_interface::return_type::ERROR;
         }
 
-        *loaded_controller.next_update_cycle_time += controller_period;
+        *loaded_controller.last_update_cycle_time = current_time;
 
         if (controller_ret != controller_interface::return_type::OK)
         {
diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp
index 8ff844adc2..0c4e51985f 100644
--- a/controller_manager/test/test_controller_manager.cpp
+++ b/controller_manager/test/test_controller_manager.cpp
@@ -575,10 +575,8 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd
     // In case of a non perfect divisor, the update period should respect the rule
     // [cm_update_rate, 2*cm_update_rate)
     EXPECT_THAT(
-      test_controller->update_period_,
-      testing::AllOf(
-        testing::Ge(rclcpp::Duration::from_seconds(1.0 / cm_update_rate)),
-        testing::Lt(rclcpp::Duration::from_seconds(2.0 / cm_update_rate))));
+      test_controller->update_period_.seconds(),
+      testing::AllOf(testing::Ge(0.9 / cm_update_rate), testing::Lt((1.1 / cm_update_rate))));
     loop_rate.sleep();
   }
   // if we do 2 times of the controller_manager update rate, the internal counter should be
@@ -640,6 +638,9 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     ControllerManagerRunner cm_runner(this);
     cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
   }
+  time_ = test_controller->get_node()->now();  // set to something nonzero
+  cm_->get_clock()->sleep_until(time_ + PERIOD);
+  time_ = cm_->get_clock()->now();
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -650,7 +651,6 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
     test_controller->get_lifecycle_state().id());
 
   // Start controller, will take effect at the end of the update function
-  time_ = test_controller->get_node()->now();  // set to something nonzero
   const auto strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
   std::vector<std::string> start_controllers = {test_controller::TEST_CONTROLLER_NAME};
   std::vector<std::string> stop_controllers = {};
@@ -661,7 +661,8 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
     << "switch_controller should be blocking until next update cycle";
 
-  time_ += rclcpp::Duration::from_seconds(0.01);
+  cm_->get_clock()->sleep_until(time_ + PERIOD);
+  time_ = cm_->get_clock()->now();
   EXPECT_EQ(
     controller_interface::return_type::OK,
     cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
@@ -677,25 +678,29 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
   EXPECT_EQ(test_controller->get_update_rate(), ctrl_update_rate);
   const auto cm_update_rate = cm_->get_update_rate();
   const auto controller_update_rate = test_controller->get_update_rate();
+  const double controller_period = 1.0 / controller_update_rate;
 
   const auto initial_counter = test_controller->internal_counter;
   // don't start with zero to check if the period is correct if controller is activated anytime
   rclcpp::Time time = time_;
   for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter)
   {
+    rclcpp::Time old_time = time;
+    cm_->get_clock()->sleep_until(old_time + PERIOD);
+    time = cm_->get_clock()->now();
     EXPECT_EQ(
       controller_interface::return_type::OK,
       cm_->update(time, rclcpp::Duration::from_seconds(0.01)));
     // In case of a non perfect divisor, the update period should respect the rule
     // [controller_update_rate, 2*controller_update_rate)
     EXPECT_THAT(
-      test_controller->update_period_,
+      test_controller->update_period_.seconds(),
       testing::AllOf(
-        testing::Ge(rclcpp::Duration::from_seconds(1.0 / controller_update_rate)),
-        testing::Lt(rclcpp::Duration::from_seconds(2.0 / controller_update_rate))))
-      << "update_counter: " << update_counter;
+        testing::Gt(0.99 * controller_period),
+        testing::Lt((1.05 * controller_period) + PERIOD.seconds())))
+      << "update_counter: " << update_counter << " desired controller period: " << controller_period
+      << " actual controller period: " << test_controller->update_period_.seconds();
 
-    time += rclcpp::Duration::from_seconds(0.01);
     if (update_counter % cm_update_rate == 0)
     {
       const double no_of_secs_passed = static_cast<double>(update_counter) / cm_update_rate;
@@ -708,15 +713,15 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate)
       EXPECT_THAT(
         test_controller->internal_counter - initial_counter,
         testing::AnyOf(
-          testing::Eq(controller_update_rate * no_of_secs_passed),
-          testing::Eq((controller_update_rate * no_of_secs_passed) - 1)));
+          testing::Ge((controller_update_rate - 1) * no_of_secs_passed),
+          testing::Lt((controller_update_rate * no_of_secs_passed))));
     }
   }
 }
 
 INSTANTIATE_TEST_SUITE_P(
   per_controller_update_rate_check, TestControllerUpdateRates,
-  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 98));
+  testing::Values(10, 12, 16, 23, 37, 40, 50, 63, 71, 85, 90));
 
 class TestControllerManagerFallbackControllers
 : public ControllerManagerFixture<controller_manager::ControllerManager>,
@@ -764,7 +769,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro
     controller_spec.info.name = test_controller_1_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {test_controller_2_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -772,7 +777,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_failure_on_fallback_contro
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
   }
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -846,7 +851,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ
     controller_spec.info.name = test_controller_1_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {test_controller_2_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -854,7 +859,7 @@ TEST_F(TestControllerManagerFallbackControllers, test_fallback_controllers_activ
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
   }
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -944,7 +949,7 @@ TEST_F(
     controller_spec.info.name = test_controller_1_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {test_controller_2_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -952,7 +957,7 @@ TEST_F(
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
   }
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -1033,7 +1038,7 @@ TEST_F(
     controller_spec.info.name = test_controller_1_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {test_controller_2_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -1041,7 +1046,7 @@ TEST_F(
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
   }
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
@@ -1129,7 +1134,7 @@ TEST_F(
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {
       test_controller_2_name, test_controller_3_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -1137,14 +1142,14 @@ TEST_F(
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
 
     controller_spec.c = test_controller_3;
     controller_spec.info.name = test_controller_3_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_3
   }
 
@@ -1279,7 +1284,7 @@ TEST_F(
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {
       test_controller_2_name, test_controller_3_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -1287,21 +1292,21 @@ TEST_F(
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
 
     controller_spec.c = test_controller_3;
     controller_spec.info.name = test_controller_3_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_3
 
     controller_spec.c = test_controller_4;
     controller_spec.info.name = test_controller_4_name;
     controller_spec.info.type = "test_chainable_controller::TestChainableController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_4
   }
 
@@ -1381,7 +1386,7 @@ TEST_F(
     controller_spec.info.name = test_controller_1_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {test_controller_3_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_1
 
     EXPECT_EQ(
@@ -1413,7 +1418,7 @@ TEST_F(
     // available
     controller_spec.info.fallback_controllers_names = {
       test_controller_4_name, test_controller_3_name, test_controller_2_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_1
 
     EXPECT_EQ(
@@ -1509,7 +1514,7 @@ TEST_F(
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {
       test_controller_2_name, test_controller_4_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     ControllerManagerRunner cm_runner(this);
     cm_->add_controller(controller_spec);  // add controller_1
 
@@ -1517,21 +1522,21 @@ TEST_F(
     controller_spec.info.name = test_controller_2_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_2
 
     controller_spec.c = test_controller_3;
     controller_spec.info.name = test_controller_3_name;
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_3
 
     controller_spec.c = test_controller_4;
     controller_spec.info.name = test_controller_4_name;
     controller_spec.info.type = "test_chainable_controller::TestChainableController";
     controller_spec.info.fallback_controllers_names = {};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_4
   }
 
@@ -1617,7 +1622,7 @@ TEST_F(
     controller_spec.info.type = "test_controller::TestController";
     controller_spec.info.fallback_controllers_names = {
       test_controller_3_name, test_controller_4_name};
-    controller_spec.next_update_cycle_time = std::make_shared<rclcpp::Time>(0);
+    controller_spec.last_update_cycle_time = std::make_shared<rclcpp::Time>(0);
     cm_->add_controller(controller_spec);  // add controller_1
 
     EXPECT_EQ(

From 033a6bcd5ece6a34140106f2ebadeafa2e932376 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Wed, 6 Nov 2024 19:46:51 +0100
Subject: [PATCH 8/9] [HW_IF] Prepare the handles for async operations (#1750)

---
 .../include/hardware_interface/handle.hpp     | 85 +++++++++++++++++--
 .../loaned_command_interface.hpp              | 85 ++++++++++++++++++-
 .../loaned_state_interface.hpp                | 56 +++++++++++-
 3 files changed, 217 insertions(+), 9 deletions(-)

diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp
index 6fe4f25663..1dfd499c2c 100644
--- a/hardware_interface/include/hardware_interface/handle.hpp
+++ b/hardware_interface/include/hardware_interface/handle.hpp
@@ -17,7 +17,10 @@
 
 #include <limits>
 #include <memory>
+#include <mutex>
+#include <shared_mutex>
 #include <string>
+#include <utility>
 #include <variant>
 
 #include "hardware_interface/hardware_info.hpp"
@@ -69,13 +72,57 @@ class Handle
   {
   }
 
-  Handle(const Handle & other) = default;
+  Handle(const Handle & other) noexcept
+  {
+    std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
+    prefix_name_ = other.prefix_name_;
+    interface_name_ = other.interface_name_;
+    handle_name_ = other.handle_name_;
+    value_ = other.value_;
+    value_ptr_ = other.value_ptr_;
+  }
 
-  Handle(Handle && other) = default;
+  Handle(Handle && other) noexcept
+  {
+    std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
+    std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
+    prefix_name_ = std::move(other.prefix_name_);
+    interface_name_ = std::move(other.interface_name_);
+    handle_name_ = std::move(other.handle_name_);
+    value_ = std::move(other.value_);
+    value_ptr_ = std::move(other.value_ptr_);
+  }
 
-  Handle & operator=(const Handle & other) = default;
+  Handle & operator=(const Handle & other)
+  {
+    if (this != &other)
+    {
+      std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
+      prefix_name_ = other.prefix_name_;
+      interface_name_ = other.interface_name_;
+      handle_name_ = other.handle_name_;
+      value_ = other.value_;
+      value_ptr_ = other.value_ptr_;
+    }
+    return *this;
+  }
 
-  Handle & operator=(Handle && other) = default;
+  Handle & operator=(Handle && other)
+  {
+    if (this != &other)
+    {
+      std::unique_lock<std::shared_mutex> lock(other.handle_mutex_);
+      std::unique_lock<std::shared_mutex> lock_this(handle_mutex_);
+      prefix_name_ = std::move(other.prefix_name_);
+      interface_name_ = std::move(other.interface_name_);
+      handle_name_ = std::move(other.handle_name_);
+      value_ = std::move(other.value_);
+      value_ptr_ = std::move(other.value_ptr_);
+    }
+    return *this;
+  }
 
   virtual ~Handle() = default;
 
@@ -95,8 +142,14 @@ class Handle
 
   const std::string & get_prefix_name() const { return prefix_name_; }
 
+  [[deprecated("Use bool get_value(double & value) instead to retrieve the value.")]]
   double get_value() const
   {
+    std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
+    if (!lock.owns_lock())
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
     // BEGIN (Handle export change): for backward compatibility
     // TODO(Manuel) return value_ if old functionality is removed
     THROW_ON_NULLPTR(value_ptr_);
@@ -104,12 +157,33 @@ class Handle
     // END
   }
 
-  void set_value(double value)
+  [[nodiscard]] bool get_value(double & value) const
+  {
+    std::shared_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
+    if (!lock.owns_lock())
+    {
+      return false;
+    }
+    // BEGIN (Handle export change): for backward compatibility
+    // TODO(Manuel) set value directly if old functionality is removed
+    THROW_ON_NULLPTR(value_ptr_);
+    value = *value_ptr_;
+    return true;
+    // END
+  }
+
+  [[nodiscard]] bool set_value(double value)
   {
+    std::unique_lock<std::shared_mutex> lock(handle_mutex_, std::try_to_lock);
+    if (!lock.owns_lock())
+    {
+      return false;
+    }
     // BEGIN (Handle export change): for backward compatibility
     // TODO(Manuel) set value_ directly if old functionality is removed
     THROW_ON_NULLPTR(this->value_ptr_);
     *this->value_ptr_ = value;
+    return true;
     // END
   }
 
@@ -122,6 +196,7 @@ class Handle
   // TODO(Manuel) redeclare as HANDLE_DATATYPE * value_ptr_ if old functionality is removed
   double * value_ptr_;
   // END
+  mutable std::shared_mutex handle_mutex_;
 };
 
 class StateInterface : public Handle
diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
index aa306870a1..6013dea778 100644
--- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
+++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp
@@ -16,10 +16,13 @@
 #define HARDWARE_INTERFACE__LOANED_COMMAND_INTERFACE_HPP_
 
 #include <functional>
+#include <limits>
 #include <string>
+#include <thread>
 #include <utility>
 
 #include "hardware_interface/handle.hpp"
+#include "rclcpp/logging.hpp"
 
 namespace hardware_interface
 {
@@ -51,6 +54,27 @@ class LoanedCommandInterface
 
   virtual ~LoanedCommandInterface()
   {
+    auto logger = rclcpp::get_logger(command_interface_.get_name());
+    RCLCPP_WARN_EXPRESSION(
+      rclcpp::get_logger(get_name()),
+      (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+      "LoanedCommandInterface %s has %u (%.4f %%) timeouts and %u (~ %.4f %%) missed calls out of "
+      "%u get_value calls",
+      get_name().c_str(), get_value_statistics_.timeout_counter,
+      (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.failed_counter,
+      (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.total_counter);
+    RCLCPP_WARN_EXPRESSION(
+      rclcpp::get_logger(get_name()),
+      (set_value_statistics_.failed_counter > 0 || set_value_statistics_.timeout_counter > 0),
+      "LoanedCommandInterface %s has %u (%.4f %%) timeouts and  %u (~ %.4f %%) missed calls out of "
+      "%u set_value calls",
+      get_name().c_str(), set_value_statistics_.timeout_counter,
+      (set_value_statistics_.timeout_counter * 100.0) / set_value_statistics_.total_counter,
+      set_value_statistics_.failed_counter,
+      (set_value_statistics_.failed_counter * 10.0) / set_value_statistics_.total_counter,
+      set_value_statistics_.total_counter);
     if (deleter_)
     {
       deleter_();
@@ -70,13 +94,70 @@ class LoanedCommandInterface
 
   const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); }
 
-  void set_value(double val) { command_interface_.set_value(val); }
+  template <typename T>
+  [[nodiscard]] bool set_value(T value, unsigned int max_tries = 10)
+  {
+    unsigned int nr_tries = 0;
+    ++set_value_statistics_.total_counter;
+    while (!command_interface_.set_value(value))
+    {
+      ++set_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++set_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
-  double get_value() const { return command_interface_.get_value(); }
+  double get_value() const
+  {
+    double value;
+    if (get_value(value))
+    {
+      return value;
+    }
+    else
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+  }
+
+  template <typename T>
+  [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+  {
+    unsigned int nr_tries = 0;
+    ++get_value_statistics_.total_counter;
+    while (!command_interface_.get_value(value))
+    {
+      ++get_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++get_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
 protected:
   CommandInterface & command_interface_;
   Deleter deleter_;
+
+private:
+  struct HandleRTStatistics
+  {
+    unsigned int total_counter = 0;
+    unsigned int failed_counter = 0;
+    unsigned int timeout_counter = 0;
+  };
+  mutable HandleRTStatistics get_value_statistics_;
+  HandleRTStatistics set_value_statistics_;
 };
 
 }  // namespace hardware_interface
diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
index 96cc3e89df..3ebc8c7ca0 100644
--- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
+++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp
@@ -16,11 +16,13 @@
 #define HARDWARE_INTERFACE__LOANED_STATE_INTERFACE_HPP_
 
 #include <functional>
+#include <limits>
 #include <string>
+#include <thread>
 #include <utility>
 
 #include "hardware_interface/handle.hpp"
-
+#include "rclcpp/logging.hpp"
 namespace hardware_interface
 {
 class LoanedStateInterface
@@ -56,6 +58,17 @@ class LoanedStateInterface
 
   virtual ~LoanedStateInterface()
   {
+    auto logger = rclcpp::get_logger(state_interface_.get_name());
+    RCLCPP_WARN_EXPRESSION(
+      logger,
+      (get_value_statistics_.failed_counter > 0 || get_value_statistics_.timeout_counter > 0),
+      "LoanedStateInterface %s has %u (%.4f %%) timeouts and %u (%.4f %%) missed calls out of %u "
+      "get_value calls",
+      state_interface_.get_name().c_str(), get_value_statistics_.timeout_counter,
+      (get_value_statistics_.timeout_counter * 100.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.failed_counter,
+      (get_value_statistics_.failed_counter * 10.0) / get_value_statistics_.total_counter,
+      get_value_statistics_.total_counter);
     if (deleter_)
     {
       deleter_();
@@ -75,11 +88,50 @@ class LoanedStateInterface
 
   const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); }
 
-  double get_value() const { return state_interface_.get_value(); }
+  double get_value() const
+  {
+    double value;
+    if (get_value(value))
+    {
+      return value;
+    }
+    else
+    {
+      return std::numeric_limits<double>::quiet_NaN();
+    }
+  }
+
+  template <typename T>
+  [[nodiscard]] bool get_value(T & value, unsigned int max_tries = 10) const
+  {
+    unsigned int nr_tries = 0;
+    ++get_value_statistics_.total_counter;
+    while (!state_interface_.get_value(value))
+    {
+      ++get_value_statistics_.failed_counter;
+      ++nr_tries;
+      if (nr_tries == max_tries)
+      {
+        ++get_value_statistics_.timeout_counter;
+        return false;
+      }
+      std::this_thread::yield();
+    }
+    return true;
+  }
 
 protected:
   const StateInterface & state_interface_;
   Deleter deleter_;
+
+private:
+  struct HandleRTStatistics
+  {
+    unsigned int total_counter = 0;
+    unsigned int failed_counter = 0;
+    unsigned int timeout_counter = 0;
+  };
+  mutable HandleRTStatistics get_value_statistics_;
 };
 
 }  // namespace hardware_interface

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 9/9] [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>`_).