Merge branch 'master' into feat/jtc-jerk-limits

Author: sachinkum0009

.pre-commit-config.yaml

@@ -133,7 +133,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc|drawio)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.37.2
+    rev: 0.37.4
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/CHANGELOG.rst

@@ -2,6 +2,12 @@
 Changelog for package controller_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.8.0 (2026-07-01)
+------------------
+* Improve docstring of the semantic_components for consistency (`#3338 <https://github.com/ros-controls/ros2_control/issues/3338>`_)
+* Improve docstring of the controller_interface for consistency (`#3318 <https://github.com/ros-controls/ros2_control/issues/3318>`_)
+* Contributors: mcereda
+
 6.7.1 (2026-05-12)
 ------------------
 * Fix the out of bound access of std::vector in ChainableController and others (`#3287 <https://github.com/ros-controls/ros2_control/issues/3287>`_)

controller_interface/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_interface</name>
-  <version>6.7.1</version>
+  <version>6.8.0</version>
   <description>Base classes for controllers and syntax cookies for supporting common sensor types in controllers and broadcasters</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>

controller_manager/CHANGELOG.rst

@@ -2,6 +2,20 @@
 Changelog for package controller_manager
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.8.0 (2026-07-01)
+------------------
+* Unload controller upon sigterm (`#3406 <https://github.com/ros-controls/ros2_control/issues/3406>`_)
+* document Ubuntu realtime kernel on Raspberry Pi (`#3397 <https://github.com/ros-controls/ros2_control/issues/3397>`_)
+* Wait to terminate test until spawner exits (`#3373 <https://github.com/ros-controls/ros2_control/issues/3373>`_)
+* Increase timeout of test_spawner_unspawner (`#3381 <https://github.com/ros-controls/ros2_control/issues/3381>`_)
+* fix typo in state and reference export methods (`#3347 <https://github.com/ros-controls/ros2_control/issues/3347>`_)
+* Enforce cleanup_controller on more exit points of configure_controller (`#3192 <https://github.com/ros-controls/ros2_control/issues/3192>`_)
+* Fix the initialization of the parsed resource managers (`#3346 <https://github.com/ros-controls/ros2_control/issues/3346>`_)
+* Controller Manager recovery from invalid URDF errors (`#2775 <https://github.com/ros-controls/ros2_control/issues/2775>`_)
+* Fix duplicate fallback controllers from the controller chain (`#3108 <https://github.com/ros-controls/ros2_control/issues/3108>`_)
+* Added default value to the fallback param declarations (`#3270 <https://github.com/ros-controls/ros2_control/issues/3270>`_)
+* Contributors: Christian Rauch, Christoph Fröhlich, Kevin Gilliam, Lucas Alvarez, Sai Kishor Kothakota, VitezGabriela
+
 6.7.1 (2026-05-12)
 ------------------
 * Fix size assertion in test_chainable_controller (`#3284 <https://github.com/ros-controls/ros2_control/issues/3284>`_)

controller_manager/CMakeLists.txt

@@ -54,7 +54,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
 )
@@ -278,6 +281,17 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  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)

controller_manager/controller_manager/spawner.py

@@ -108,7 +108,7 @@ def parse_args_advanced(args):
         "-u",
         "--unload-on-kill",
         action="store_true",
-        help="Deactivate the active controllers and unload them on kill",
+        help="Deactivate the active controllers and unload them on SIGINT or SIGTERM",
     )
     global_parser.add_argument("-h", "--help", action="store_true", help="Show help")
 
@@ -239,7 +239,7 @@ def parse_native_args(args):
     parser.add_argument(
         "-u",
         "--unload-on-kill",
-        help="Wait until this application is interrupted and unload controller",
+        help="Wait until this application is interrupted (SIGINT or SIGTERM) and deactivate/unload controllers",
         action="store_true",
     )
     parser.add_argument(
@@ -358,6 +358,13 @@ def get_ros_params_files(argv):
     activate_as_group = global_args.activate_as_group
     unload_on_kill = global_args.unload_on_kill
     node = None
+    lock = None
+
+    def _on_shutdown_signal(signum, frame):
+        raise KeyboardInterrupt()
+
+    signal.signal(signal.SIGINT, _on_shutdown_signal)
+    signal.signal(signal.SIGTERM, _on_shutdown_signal)
 
     if spawner_ros_params_files:
         for controller in controllers:
@@ -574,6 +581,7 @@ def get_ros_params_files(argv):
         # second KeyboardInterrupt during the signal.signal() call itself.
         try:
             signal.signal(signal.SIGINT, signal.SIG_IGN)
+            signal.signal(signal.SIGTERM, signal.SIG_IGN)
         except (KeyboardInterrupt, Exception):
             pass
         if unload_on_kill:
@@ -632,7 +640,7 @@ def get_ros_params_files(argv):
     finally:
         if node:
             node.destroy_node()
-        if lock.is_locked:
+        if lock is not None and lock.is_locked:
             lock.release()
         rclpy.shutdown()
 

controller_manager/doc/userdoc.rst

@@ -182,7 +182,7 @@ There are two scripts to interact with controller manager from launch files:
                             Controller param file to be loaded into controller node before configure. Pass multiple times to load different files for different controllers or to override the parameters of the same controller.
       --load-only           Only load the controller and leave unconfigured.
       --inactive            Load and configure the controller, however do not activate them
-      -u, --unload-on-kill  Wait until this application is interrupted and unload controller
+      -u, --unload-on-kill  Wait until this application is interrupted (SIGINT or SIGTERM) and deactivate/unload controllers
       --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT
                             Time to wait for the controller manager service to be available
       --switch-timeout SWITCH_TIMEOUT
@@ -267,7 +267,7 @@ The ``spawner`` now supports per controller arguments, while parsing the argumen
       --activate-as-group   Activate controllers as a group
       --switch-asap, --no-switch-asap
                             Switch controllers as soon as possible
-      -u, --unload-on-kill  Deactivate the active controllers and unload them on kill
+      -u, --unload-on-kill  Deactivate the active controllers and unload them on SIGINT or SIGTERM
       -h, --help            Show help
 
     Controller Options:
@@ -484,6 +484,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
 -----------
 

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);

controller_manager/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>controller_manager</name>
-  <version>6.7.1</version>
+  <version>6.8.0</version>
   <description>The main runnable entrypoint of ros2_control and home of controller management and resource management.</description>
   <maintainer email="bence.magyar.robotics@gmail.com">Bence Magyar</maintainer>
   <maintainer email="denis@stoglrobotics.de">Denis Štogl</maintainer>
@@ -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>

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);
         }
       }
     });