Add ControllerInterfaceParams to initialize the Controllers (#2390)

Author: saikishor

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/src/controller_manager.cpp

@@ -2024,10 +2024,15 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
   // Catch whatever exception the controller might throw
   try
   {
-    if (
-      controller.c->init(
-        controller.info.name, robot_description_, get_update_rate(), get_namespace(),
-        controller_node_options) == controller_interface::return_type::ERROR)
+    controller_interface::ControllerInterfaceParams controller_params;
+    controller_params.controller_name = controller.info.name;
+    controller_params.robot_description = robot_description_;
+    controller_params.update_rate = get_update_rate();
+    controller_params.node_namespace = get_namespace();
+    controller_params.node_options = controller_node_options;
+    controller_params.hard_joint_limits = resource_manager_->get_hard_joint_limits();
+    controller_params.soft_joint_limits = resource_manager_->get_soft_joint_limits();
+    if (controller.c->init(controller_params) == controller_interface::return_type::ERROR)
     {
       to.clear();
       RCLCPP_ERROR(

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -516,6 +516,19 @@ class ResourceManager
    */
   const std::unordered_map<std::string, HardwareComponentInfo> & get_components_status();
 
+  /// Return the unordered map of hard joint limits.
+  /**
+   * \return unordered map of hard joint limits.
+   */
+  const std::unordered_map<std::string, joint_limits::JointLimits> & get_hard_joint_limits() const;
+
+  /// Return the unordered map of soft joint limits.
+  /**
+   * \return unordered map of soft joint limits.
+   */
+  const std::unordered_map<std::string, joint_limits::SoftJointLimits> & get_soft_joint_limits()
+    const;
+
   /// Prepare the hardware components for a new command interface mode
   /**
    * Hardware components are asked to prepare a new command interface claim.

hardware_interface/src/resource_manager.cpp

@@ -739,6 +739,7 @@ class ResourceStorage
       for (const auto & [joint_name, limits] : hw_info.limits)
       {
         std::vector<joint_limits::SoftJointLimits> soft_limits;
+        hard_joint_limits_.insert({joint_name, limits});
         const std::vector<joint_limits::JointLimits> hard_limits{limits};
         joint_limits::JointInterfacesCommandLimiterData data;
         data.set_joint_name(joint_name);
@@ -747,6 +748,7 @@ class ResourceStorage
         if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
         {
           soft_limits = {hw_info.soft_limits.at(joint_name)};
+          soft_joint_limits_.insert({joint_name, hw_info.soft_limits.at(joint_name)});
           RCLCPP_INFO(
             get_logger(), "Using SoftJointLimiter for joint '%s' in hardware '%s' : '%s'",
             joint_name.c_str(), hw_info.name.c_str(), soft_limits[0].to_string().c_str());
@@ -1347,6 +1349,10 @@ class ResourceStorage
 
   std::string robot_description_;
 
+  // Unordered map of the hard and soft limits for the joints
+  std::unordered_map<std::string, joint_limits::JointLimits> hard_joint_limits_;
+  std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_joint_limits_;
+
   /// The callback to be called when a component state is switched
   std::function<void()> on_component_state_switch_callback_ = nullptr;
 
@@ -1957,6 +1963,18 @@ ResourceManager::get_components_status()
   return resource_storage_->hardware_info_map_;
 }
 
+const std::unordered_map<std::string, joint_limits::JointLimits> &
+ResourceManager::get_hard_joint_limits() const
+{
+  return resource_storage_->hard_joint_limits_;
+}
+
+const std::unordered_map<std::string, joint_limits::SoftJointLimits> &
+ResourceManager::get_soft_joint_limits() const
+{
+  return resource_storage_->soft_joint_limits_;
+}
+
 // CM API: Called in "callback/slow"-thread
 bool ResourceManager::prepare_command_mode_switch(
   const std::vector<std::string> & start_interfaces,