Make sleep in ros2_control node optional (backport #3213) (#3452)

mergify[bot] · web-flow · commit 0e5cccee22cc · 2026-07-13T13:16:14.000+02:00
diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt
@@ -55,7 +55,10 @@ target_link_libraries(controller_manager PUBLIC
                       ${std_msgs_TARGETS}
                       ${controller_manager_msgs_TARGETS})
 
-add_executable(ros2_control_node src/ros2_control_node.cpp)
+add_executable(ros2_control_node
+  src/ros2_control_node.cpp
+  src/sleeping_policies.cpp
+)
 target_link_libraries(ros2_control_node PRIVATE
   controller_manager
 )
@@ -270,6 +273,17 @@ if(BUILD_TESTING)
     ${controller_manager_msgs_TARGETS}
   )
 
+  ament_add_gmock(test_sleeping_policies
+    test/test_sleeping_policies.cpp
+    src/sleeping_policies.cpp
+    TIMEOUT 60
+    APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
+  )
+  target_link_libraries(test_sleeping_policies
+    controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   find_package(ament_cmake_pytest REQUIRED)
   install(FILES test/test_ros2_control_node.yaml
     DESTINATION test)
diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst
@@ -491,6 +491,18 @@ overruns.manage (optional; bool; default: true)
   If an overrun is detected, the controller manager will print a warning message to the console.
   When used with ``use_sim_time`` set to true, this parameter is ignored and the overrun handling is disabled.
 
+hardware_synchronization.expect_blocking_read_write (optional; bool; default: false)
+  If true, the controller manager will not sleep actively. Use this, when there is a hardware
+  interface running that will block during its read or write operation until receiving new data
+  from the hardware.
+
+hardware_synchronization.minimum_cycle_time (optional; double; default: 0.0001)
+  The minimum sleep time in seconds for the control node's real-time loop. This is used to
+  prevent the control node from running too fast, which can cause high CPU
+  usage if the hardware doesn't block in read / write. This is only used when
+  ``expect_blocking_read_write`` is set to true.
+  If the cycle is shorter than this, it will sleep for this period and print a warning.
+
 Concepts
 -----------
 
diff --git a/controller_manager/include/controller_manager/sleeping_policies.hpp b/controller_manager/include/controller_manager/sleeping_policies.hpp
@@ -0,0 +1,54 @@
+// Copyright 2026 ROS2-Control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <memory>
+
+#include <rclcpp/logging.hpp>
+
+#include "controller_manager/controller_manager.hpp"
+
+namespace controller_manager
+{
+struct ControlLoopTimingConfig
+{
+  bool use_sim_time{false};
+  bool manage_overruns{true};
+  bool expect_blocking_read_write{false};
+  double minimum_cycle_time{0.0001};
+};
+
+struct ControlLoopState
+{
+  rclcpp::Time previous_time;
+  std::chrono::steady_clock::time_point next_iteration_time;
+  std::chrono::nanoseconds period{0};
+  rclcpp::Time cycle_end_time;  //< The time when work was done in the current cycle.
+};
+}  // namespace controller_manager
+
+bool sleep_for_sim_time(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  controller_manager::ControlLoopState & state);
+
+void sleep_for_blocking_read_write(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  const controller_manager::ControlLoopTimingConfig & config,
+  controller_manager::ControlLoopState & state);
+
+void sleep_for_periodic_cycle(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  const controller_manager::ControlLoopTimingConfig & config,
+  controller_manager::ControlLoopState & state);
diff --git a/controller_manager/package.xml b/controller_manager/package.xml
@@ -40,6 +40,7 @@
   <exec_depend>sensor_msgs</exec_depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
+  <test_depend>diagnostic_msgs</test_depend>
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>example_interfaces</test_depend>
   <test_depend>hardware_interface_testing</test_depend>
diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp
@@ -19,6 +19,7 @@
 #include <thread>
 
 #include "controller_manager/controller_manager.hpp"
+#include "controller_manager/sleeping_policies.hpp"
 #include "rclcpp/executors.hpp"
 #include "realtime_tools/realtime_helpers.hpp"
 
@@ -79,8 +80,20 @@ int main(int argc, char ** argv)
     cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(),
     thread_priority);
 
+  const controller_manager::ControlLoopTimingConfig timing_config{
+    .use_sim_time = use_sim_time,
+    .manage_overruns = manage_overruns,
+    .expect_blocking_read_write =
+      cm->get_parameter_or<bool>("hardware_synchronization.expect_blocking_read_write", false),
+    .minimum_cycle_time =
+      cm->get_parameter_or<double>("hardware_synchronization.minimum_cycle_time", 0.0001),
+  };
+  RCLCPP_INFO_EXPRESSION(
+    cm->get_logger(), timing_config.expect_blocking_read_write,
+    "Synchronizing control loop with hardware.");
+
   std::thread cm_thread(
-    [cm, thread_priority, use_sim_time, manage_overruns]()
+    [cm, thread_priority, timing_config]()
     {
       rclcpp::Parameter cpu_affinity_param;
       if (cm->get_parameter("cpu_affinity", cpu_affinity_param))
@@ -124,63 +137,39 @@ int main(int argc, char ** argv)
       cm->get_clock()->wait_until_started();
       cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate()));
 
-      // for calculating sleep time
-      auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
-
-      // for calculating the measured period of the loop
-      rclcpp::Time previous_time = cm->get_trigger_clock()->now();
-      std::this_thread::sleep_for(period);
-
-      std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()};
-
+      controller_manager::ControlLoopState state;
+      state.period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate());
+      state.previous_time = cm->get_trigger_clock()->now();
+      std::this_thread::sleep_for(state.period);
+      state.next_iteration_time = std::chrono::steady_clock::now();
       while (rclcpp::ok())
       {
         // calculate measured period
         auto const current_time = cm->get_trigger_clock()->now();
-        auto const measured_period = current_time - previous_time;
-        previous_time = current_time;
+        auto const measured_period = current_time - state.previous_time;
+        state.previous_time = current_time;
 
         // execute update loop
         cm->read(cm->get_trigger_clock()->now(), measured_period);
         cm->update(cm->get_trigger_clock()->now(), measured_period);
         cm->write(cm->get_trigger_clock()->now(), measured_period);
+        state.cycle_end_time = cm->get_trigger_clock()->now();
 
         // wait until we hit the end of the period
-        if (use_sim_time)
+        if (timing_config.use_sim_time)
         {
-          try
-          {
-            cm->get_clock()->sleep_until(current_time + period);
-          }
-          catch (const std::runtime_error & e)
+          if (!sleep_for_sim_time(cm, state))
           {
-            RCLCPP_ERROR(
-              cm->get_logger(),
-              "sleep_until failed with error: %s. Exiting control loop and aborting....", e.what());
             break;
           }
         }
+        else if (timing_config.expect_blocking_read_write)
+        {
+          sleep_for_blocking_read_write(cm, timing_config, state);
+        }
         else
         {
-          next_iteration_time += period;
-          const auto time_now = std::chrono::steady_clock::now();
-          if (manage_overruns && next_iteration_time < time_now)
-          {
-            const double time_diff =
-              static_cast<double>(
-                std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - next_iteration_time)
-                  .count()) /
-              1.e6;
-            const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
-            const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
-            RCLCPP_WARN_THROTTLE(
-              cm->get_logger(), *cm->get_clock(), 1000,
-              "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop "
-              "took %f ms (missed cycles : %d).",
-              cm->get_update_rate(), time_diff + cm_period, overrun_count + 1);
-            next_iteration_time += (overrun_count * period);
-          }
-          std::this_thread::sleep_until(next_iteration_time);
+          sleep_for_periodic_cycle(cm, timing_config, state);
         }
       }
     });
diff --git a/controller_manager/src/sleeping_policies.cpp b/controller_manager/src/sleeping_policies.cpp
@@ -0,0 +1,80 @@
+// Copyright 2026 ROS2-Control Development Team
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "controller_manager/sleeping_policies.hpp"
+
+bool sleep_for_sim_time(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  controller_manager::ControlLoopState & state)
+{
+  // Implement sleep logic for simulated time
+  try
+  {
+    cm->get_clock()->sleep_until(state.previous_time + state.period);
+  }
+  catch (const std::runtime_error & e)
+  {
+    RCLCPP_ERROR(
+      cm->get_logger(), "sleep_until failed with error: %s. Exiting control loop and aborting....",
+      e.what());
+    return false;
+  }
+  return true;  // Placeholder return value
+}
+
+void sleep_for_blocking_read_write(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  const controller_manager::ControlLoopTimingConfig & config,
+  controller_manager::ControlLoopState & state)
+{
+  // Implement sleep logic for blocking read/write
+  if (
+    (state.cycle_end_time - state.previous_time).nanoseconds() <
+    static_cast<rcl_duration_value_t>(config.minimum_cycle_time * 1e9))
+  {
+    RCLCPP_WARN_THROTTLE(
+      cm->get_logger(), *cm->get_clock(), 1000,
+      "Last control cycle was shorter than the minimum cycle time while blocking read or "
+      "write is configured. Is the hardware interface not blocking correctly? Note: This "
+      "might happen when the hardware component that should block is not active.");
+    std::this_thread::sleep_for(
+      std::chrono::microseconds(static_cast<int>(config.minimum_cycle_time * 1e6)));
+  }
+}
+
+void sleep_for_periodic_cycle(
+  std::shared_ptr<controller_manager::ControllerManager> cm,
+  const controller_manager::ControlLoopTimingConfig & config,
+  controller_manager::ControlLoopState & state)
+{
+  state.next_iteration_time += state.period;
+  const auto time_now = std::chrono::steady_clock::now();
+  if (config.manage_overruns && state.next_iteration_time < time_now)
+  {
+    const double time_diff =
+      static_cast<double>(
+        std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - state.next_iteration_time)
+          .count()) /
+      1.e6;
+    const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
+    const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
+    RCLCPP_WARN_THROTTLE(
+      cm->get_logger(), *cm->get_clock(), 1000,
+      "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop "
+      "took %f ms (missed cycles : %d).",
+      cm->get_update_rate(), time_diff + cm_period, overrun_count + 1);
+    state.next_iteration_time += (overrun_count * state.period);
+  }
+  std::this_thread::sleep_until(state.next_iteration_time);
+}
diff --git a/controller_manager/test/test_sleeping_policies.cpp b/controller_manager/test/test_sleeping_policies.cpp
