Merge branch 'master' into add/overrun/parameters

Author: saikishor

.pre-commit-config.yaml

@@ -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: 25.1.0
+    rev: 25.9.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: v21.1.0
+    rev: v21.1.2
     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.33.3
+    rev: 0.34.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -17,11 +17,13 @@
 
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <utility>
 #include <vector>
 
 #include "realtime_tools/async_function_handler.hpp"
 
+#include "controller_interface/controller_interface_params.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/introspection.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
@@ -151,10 +153,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
    */
   virtual void release_interfaces();
 
+  [[deprecated(
+    "Use the init(const controller_interface::ControllerInterfaceParams & params) method instead. "
+    "This method will be removed in the future ROS 2 releases.")]]
   return_type init(
     const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
     const std::string & node_namespace, const rclcpp::NodeOptions & node_options);
 
+  return_type init(const controller_interface::ControllerInterfaceParams & params);
+
   /// Custom configure method to read additional parameters for controller-nodes
   /*
    * Override default implementation for configure of LifecycleNode to get parameters.
@@ -201,6 +208,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
   const std::string & get_robot_description() const;
 
+  /**
+   * Get the unordered map of joint limits that are defined in the robot description.
+   */
+  const std::unordered_map<std::string, joint_limits::JointLimits> & get_hard_joint_limits() const;
+
+  /**
+   * Get the unordered map of soft joint limits that are defined in the robot description.
+   */
+  const std::unordered_map<std::string, joint_limits::SoftJointLimits> & get_soft_joint_limits()
+    const;
+
   /**
    * Method used by the controller_manager for base NodeOptions to instantiate the Lifecycle node
    * of the controller upon loading the controller.
@@ -358,9 +376,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
 
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
-  unsigned int update_rate_ = 0;
   bool is_async_ = false;
-  std::string urdf_ = "";
+  controller_interface::ControllerInterfaceParams ctrl_itf_params_;
   std::atomic_bool skip_async_triggers_ = false;
   ControllerUpdateStats trigger_stats_;
 

controller_interface/include/controller_interface/controller_interface_params.hpp

@@ -0,0 +1,60 @@
+// Copyright 2025 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.
+
+#ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_
+#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_
+
+#include <string>
+#include <unordered_map>
+
+#include "joint_limits/joint_limits.hpp"
+#include "rclcpp/node_options.hpp"
+
+/**
+ * @brief Parameters for the Controller Interface
+ * This struct holds the parameters required to initialize a controller interface.
+ *
+ * @var controller_name Name of the controller.
+ * @var robot_description The URDF or SDF description of the robot.
+ * @var cm_update_rate The update rate of the controller manager in Hz.
+ * @var node_namespace The namespace for the controller node.
+ * @var node_options Options for the controller node.
+ * @var joint_limits A map of joint names to their limits.
+ * @var soft_joint_limits A map of joint names to their soft limits.
+ *
+ * This struct is used to pass parameters to the controller interface during initialization.
+ * It allows for easy configuration of the controller's behavior and interaction with the robot's
+ * joints.
+ */
+namespace controller_interface
+{
+
+struct ControllerInterfaceParams
+{
+  ControllerInterfaceParams() = default;
+
+  std::string controller_name = "";
+  std::string robot_description = "";
+  unsigned int update_rate = 0;
+
+  std::string node_namespace = "";
+  rclcpp::NodeOptions node_options = rclcpp::NodeOptions();
+
+  std::unordered_map<std::string, joint_limits::JointLimits> hard_joint_limits = {};
+  std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_joint_limits = {};
+};
+
+}  // namespace controller_interface
+
+#endif  // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_PARAMS_HPP_

controller_interface/src/controller_interface_base.cpp

@@ -43,16 +43,29 @@ return_type ControllerInterfaceBase::init(
   const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
   const std::string & node_namespace, const rclcpp::NodeOptions & node_options)
 {
-  urdf_ = urdf;
-  update_rate_ = cm_update_rate;
+  controller_interface::ControllerInterfaceParams params;
+  params.controller_name = controller_name;
+  params.robot_description = urdf;
+  params.update_rate = cm_update_rate;
+  params.node_namespace = node_namespace;
+  params.node_options = node_options;
+
+  return init(params);
+}
+
+return_type ControllerInterfaceBase::init(
+  const controller_interface::ControllerInterfaceParams & params)
+{
+  ctrl_itf_params_ = params;
   node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    controller_name, node_namespace, node_options,
+    ctrl_itf_params_.controller_name, ctrl_itf_params_.node_namespace,
+    ctrl_itf_params_.node_options,
     false);  // disable LifecycleNode service interfaces
 
   try
   {
     // no rclcpp::ParameterValue unsigned int specialization
-    auto_declare<int>("update_rate", static_cast<int>(update_rate_));
+    auto_declare<int>("update_rate", static_cast<int>(ctrl_itf_params_.update_rate));
 
     auto_declare<bool>("is_async", false);
     auto_declare<int>("thread_priority", 50);
@@ -149,17 +162,17 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
       RCLCPP_ERROR(get_node()->get_logger(), "Update rate cannot be a negative value!");
       return get_lifecycle_state();
     }
-    if (update_rate_ != 0u && update_rate > update_rate_)
+    if (ctrl_itf_params_.update_rate != 0u && update_rate > ctrl_itf_params_.update_rate)
     {
       RCLCPP_WARN(
         get_node()->get_logger(),
         "The update rate of the controller : '%ld Hz' cannot be higher than the update rate of the "
         "controller manager : '%d Hz'. Setting it to the update rate of the controller manager.",
-        update_rate, update_rate_);
+        update_rate, ctrl_itf_params_.update_rate);
     }
     else
     {
-      update_rate_ = static_cast<unsigned int>(update_rate);
+      ctrl_itf_params_.update_rate = static_cast<unsigned int>(update_rate);
     }
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }
@@ -273,11 +286,29 @@ std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> ControllerInterfaceBase::
   return node_;
 }
 
-unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; }
+unsigned int ControllerInterfaceBase::get_update_rate() const
+{
+  return ctrl_itf_params_.update_rate;
+}
 
 bool ControllerInterfaceBase::is_async() const { return is_async_; }
 
-const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }
+const std::string & ControllerInterfaceBase::get_robot_description() const
+{
+  return ctrl_itf_params_.robot_description;
+}
+
+const std::unordered_map<std::string, joint_limits::JointLimits> &
+ControllerInterfaceBase::get_hard_joint_limits() const
+{
+  return ctrl_itf_params_.hard_joint_limits;
+}
+
+const std::unordered_map<std::string, joint_limits::SoftJointLimits> &
+ControllerInterfaceBase::get_soft_joint_limits() const
+{
+  return ctrl_itf_params_.soft_joint_limits;
+}
 
 void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
 {

controller_interface/test/test_controller_interface.cpp

@@ -104,9 +104,21 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   node_options_arguments.push_back("-p");
   node_options_arguments.push_back("update_rate:=2812");
   node_options = node_options.arguments(node_options_arguments);
-  ASSERT_EQ(
-    controller.init(TEST_CONTROLLER_NAME, "", 5000.0, "", node_options),
-    controller_interface::return_type::OK);
+  controller_interface::ControllerInterfaceParams params;
+  params.controller_name = TEST_CONTROLLER_NAME;
+  params.robot_description = "";
+  params.update_rate = 5000;  // set a different update rate than the one in the node options
+  params.node_namespace = "";
+  params.node_options = node_options;
+  joint_limits::JointLimits joint_limits;
+  joint_limits.has_velocity_limits = true;
+  joint_limits.max_velocity = 1.0;
+  params.hard_joint_limits["joint1"] = joint_limits;
+  joint_limits::SoftJointLimits soft_joint_limits;
+  soft_joint_limits.min_position = -1.0;
+  soft_joint_limits.max_position = 1.0;
+  params.soft_joint_limits["joint1"] = soft_joint_limits;
+  ASSERT_EQ(controller.init(params), controller_interface::return_type::OK);
 
   // initialize executor to be able to get parameter update
   auto executor =
@@ -122,6 +134,21 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
 
   // update_rate is set to controller_manager's rate
   ASSERT_EQ(controller.get_update_rate(), 5000u);
+  const auto hard_limits = controller.get_hard_joint_limits();
+  const auto soft_limits = controller.get_soft_joint_limits();
+  ASSERT_THAT(hard_limits, testing::SizeIs(1));
+  ASSERT_TRUE(hard_limits.find("joint1") != hard_limits.end());
+  ASSERT_THAT(hard_limits.at("joint1").max_velocity, joint_limits.max_velocity);
+  ASSERT_TRUE(hard_limits.at("joint1").has_velocity_limits);
+  ASSERT_FALSE(hard_limits.at("joint1").has_position_limits);
+  ASSERT_FALSE(hard_limits.at("joint1").has_acceleration_limits);
+  ASSERT_FALSE(hard_limits.at("joint1").has_deceleration_limits);
+  ASSERT_FALSE(hard_limits.at("joint1").has_jerk_limits);
+  ASSERT_FALSE(hard_limits.at("joint1").has_effort_limits);
+  ASSERT_THAT(soft_limits, testing::SizeIs(1));
+  ASSERT_TRUE(soft_limits.find("joint1") != soft_limits.end());
+  ASSERT_THAT(soft_limits.at("joint1").min_position, soft_joint_limits.min_position);
+  ASSERT_THAT(soft_limits.at("joint1").max_position, soft_joint_limits.max_position);
 
   // Even after configure is 0
   controller.configure();
@@ -156,6 +183,23 @@ TEST(TestableControllerInterface, setting_update_rate_in_configure)
   controller.configure();
   ASSERT_EQ(controller.get_update_rate(), 623u);
 
+  // Should stay same after multiple cleanups as it is set during initialization
+  const auto hard_limits_final = controller.get_hard_joint_limits();
+  const auto soft_limits_final = controller.get_soft_joint_limits();
+  ASSERT_THAT(hard_limits_final, testing::SizeIs(1));
+  ASSERT_TRUE(hard_limits_final.find("joint1") != hard_limits_final.end());
+  ASSERT_THAT(hard_limits_final.at("joint1").max_velocity, joint_limits.max_velocity);
+  ASSERT_TRUE(hard_limits_final.at("joint1").has_velocity_limits);
+  ASSERT_FALSE(hard_limits_final.at("joint1").has_position_limits);
+  ASSERT_FALSE(hard_limits_final.at("joint1").has_acceleration_limits);
+  ASSERT_FALSE(hard_limits_final.at("joint1").has_deceleration_limits);
+  ASSERT_FALSE(hard_limits_final.at("joint1").has_jerk_limits);
+  ASSERT_FALSE(hard_limits_final.at("joint1").has_effort_limits);
+  ASSERT_THAT(soft_limits_final, testing::SizeIs(1));
+  ASSERT_TRUE(soft_limits_final.find("joint1") != soft_limits_final.end());
+  ASSERT_THAT(soft_limits_final.at("joint1").min_position, soft_joint_limits.min_position);
+  ASSERT_THAT(soft_limits_final.at("joint1").max_position, soft_joint_limits.max_position);
+
   executor->cancel();
   controller.get_node()->shutdown();
   rclcpp::shutdown();

controller_manager/CMakeLists.txt

@@ -184,6 +184,20 @@ if(BUILD_TESTING)
     DESTINATION lib
   )
 
+  add_library(test_controller_failed_activate SHARED
+  test/test_controller_failed_activate/test_controller_failed_activate.cpp
+  )
+  target_link_libraries(test_controller_failed_activate PUBLIC
+    controller_manager
+  )
+  target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL")
+  pluginlib_export_plugin_description_file(
+    controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml)
+  install(
+    TARGETS test_controller_failed_activate
+    DESTINATION lib
+  )
+
   ament_add_gmock(test_release_interfaces
     test/test_release_interfaces.cpp
     APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$<CONFIG>
@@ -192,6 +206,7 @@ if(BUILD_TESTING)
     controller_manager
     test_controller
     test_controller_with_interfaces
+    test_controller_failed_activate
     ros2_control_test_assets::ros2_control_test_assets
   )
 

controller_manager/controller_manager/spawner.py

@@ -326,6 +326,8 @@ def main(args=None):
         if not unload_controllers_upon_exit:
             return 0
 
+        # The lock has to be released to not block other spawner instances while waiting for the interrupt
+        lock.release()
         logger.info("Waiting until interrupt to unload controllers")
         while True:
             time.sleep(1)
@@ -381,7 +383,8 @@ def main(args=None):
     finally:
         if node:
             node.destroy_node()
-        lock.release()
+        if lock.is_locked:
+            lock.release()
         rclpy.shutdown()
 
 

controller_manager/doc/userdoc.rst

@@ -409,6 +409,12 @@ Hardware and Controller Errors
 If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces.
 Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available.
 
+Factors that affect Determinism
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+When run under the conditions determined in the above section, the determinism is assured up to the limitations of the hardware and the real-time kernel. However, there are some situations that can affect determinism:
+
+* When a controller fails to activate, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop.
+
 Support for Asynchronous Updates
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 For some applications, it is desirable to run a controller at a lower frequency than the controller manager's update rate. For instance, if the ``update_rate`` for the controller manager is 100Hz, the sum of the execution times of all controllers' ``update`` calls and hardware components ``read`` and ``write`` calls must be below 10ms. If one controller requires 15ms of execution time, it cannot be executed synchronously without affecting the overall system update rate. Running a controller asynchronously can be beneficial in this scenario.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -51,7 +51,7 @@
 namespace controller_manager
 {
 class ParamListener;
-class Params;
+struct Params;
 using ControllersListIterator = std::vector<controller_manager::ControllerSpec>::const_iterator;
 
 rclcpp::NodeOptions get_cm_node_options();