[Feature] Interfaces remapping for controllers

controller_interface/include/controller_interface/controller_interface_base.hpp

@@ -15,6 +15,7 @@
 #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
 #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -254,12 +255,32 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   virtual bool is_in_chained_mode() const = 0;
 
+  /// Get the remapping of state interfaces defined in the controller namespace
+  /**
+   * Get the remapping of state interfaces defined in the controller namespace
+   *
+   * \returns map of state interfaces remapping
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  const std::map<std::string, std::string> & get_state_interfaces_remap() const;
+
+  /// Get the remapping of command interfaces defined in the controller namespace
+  /**
+   * Get the remapping of command interfaces defined in the controller namespace
+   *
+   * \returns map of command interfaces remapping
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  const std::map<std::string, std::string> & get_command_interfaces_remap() const;
+
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
 
 private:
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+  std::map<std::string, std::string> state_interfaces_remap_;
+  std::map<std::string, std::string> command_interfaces_remap_;
   unsigned int update_rate_ = 0;
   bool is_async_ = false;
   std::string urdf_ = "";

controller_interface/src/controller_interface_base.cpp

@@ -89,7 +89,70 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
     is_async_ = get_node()->get_parameter("is_async").as_bool();
   }
 
-  return get_node()->configure();
+  const auto & state = get_node()->configure();
+
+  auto retrieve_remap_interface = [this](
+                                    const std::string & remap_namespace,
+                                    std::map<std::string, std::string> & remappings) -> bool
+  {
+    for (auto & [key, value] : remappings)
+    {
+      if (!node_->has_parameter(remap_namespace + "." + key))
+      {
+        node_->declare_parameter(remap_namespace + "." + key, value);
+      }
+    }
+    return node_->get_parameters(remap_namespace, remappings);
+  };
+
+  // set the map keys and values by iteratinf over the names
+  const auto & state_itf = state_interface_configuration();
+  const auto & cmd_itfs = command_interface_configuration();
+  for (const auto & interface : state_itf.names)
+  {
+    state_interfaces_remap_[interface] = interface;
+  }
+  for (const auto & interface : cmd_itfs.names)
+  {
+    command_interfaces_remap_[interface] = interface;
+  }
+
+  if (retrieve_remap_interface("remap.state_interfaces", state_interfaces_remap_))
+  {
+    if (std::any_of(
+          state_interfaces_remap_.begin(), state_interfaces_remap_.end(),
+          [](const auto & pair) { return pair.first != pair.second; }))
+    {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "The controller : %s will remap the following state interfaces:", get_node()->get_name());
+      for (const auto & [key, value] : state_interfaces_remap_)
+      {
+        RCLCPP_WARN_EXPRESSION(
+          node_->get_logger(), key != value, "\t'%s' to '%s'", key.c_str(), value.c_str());
+      }
+    }
+  }
+  if (
+    retrieve_remap_interface("remap.command_interfaces", command_interfaces_remap_) &&
+    !command_interfaces_remap_.empty())
+  {
+    if (std::any_of(
+          command_interfaces_remap_.begin(), command_interfaces_remap_.end(),
+          [](const auto & pair) { return pair.first != pair.second; }))
+    {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "The controller : %s will remap the following command interfaces:", get_node()->get_name());
+      for (const auto & [key, value] : command_interfaces_remap_)
+      {
+        RCLCPP_WARN_EXPRESSION(
+          node_->get_logger(), key != value, "\t'%s' to '%s'", key.c_str(), value.c_str());
+      }
+    }
+  }
+
+  return state;
 }
 
 void ControllerInterfaceBase::assign_interfaces(
@@ -135,4 +198,16 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; }
 
 const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; }
 
+const std::map<std::string, std::string> &
+controller_interface::ControllerInterfaceBase::get_state_interfaces_remap() const
+{
+  return state_interfaces_remap_;
+}
+
+const std::map<std::string, std::string> &
+controller_interface::ControllerInterfaceBase::get_command_interfaces_remap() const
+{
+  return command_interfaces_remap_;
+}
+
 }  // namespace controller_interface

controller_manager/include/controller_manager/controller_spec.hpp

@@ -19,6 +19,7 @@
 #ifndef CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 #define CONTROLLER_MANAGER__CONTROLLER_SPEC_HPP_
 
+#include <map>
 #include <memory>
 #include <string>
 #include <vector>
@@ -38,6 +39,43 @@ struct ControllerSpec
   hardware_interface::ControllerInfo info;
   controller_interface::ControllerInterfaceBaseSharedPtr c;
   std::shared_ptr<rclcpp::Time> next_update_cycle_time;
+
+  controller_interface::InterfaceConfiguration get_command_interface_configuration() const
+  {
+    return get_remapped_interface_configuration(
+      c->command_interface_configuration(), c->get_command_interfaces_remap());
+  }
+
+  controller_interface::InterfaceConfiguration get_state_interface_configuration() const
+  {
+    return get_remapped_interface_configuration(
+      c->state_interface_configuration(), c->get_state_interfaces_remap());
+  }
+
+private:
+  controller_interface::InterfaceConfiguration get_remapped_interface_configuration(
+    const controller_interface::InterfaceConfiguration & interface_cfg,
+    const std::map<std::string, std::string> & remap) const
+  {
+    if (interface_cfg.type != controller_interface::interface_configuration_type::INDIVIDUAL)
+    {
+      return interface_cfg;
+    }
+    else
+    {
+      controller_interface::InterfaceConfiguration remapped_cmd_itf_cfg = interface_cfg;
+      for (auto & [key, value] : remap)
+      {
+        auto it =
+          std::find(remapped_cmd_itf_cfg.names.begin(), remapped_cmd_itf_cfg.names.end(), key);
+        if (it != remapped_cmd_itf_cfg.names.end())
+        {
+          *it = value;
+        }
+      }
+      return remapped_cmd_itf_cfg;
+    }
+  }
 };
 
 struct ControllerChainSpec

controller_manager/src/controller_manager.cpp

@@ -700,6 +700,18 @@ controller_interface::return_type ControllerManager::configure_controller(
 
   try
   {
+    const auto avilable_state_interfaces = resource_manager_->available_state_interfaces();
+    std::for_each(
+      avilable_state_interfaces.begin(), avilable_state_interfaces.end(),
+      [&controller](const auto & state_interface)
+      {
+        const auto ctrl_node = controller->get_node();
+        if (!ctrl_node->has_parameter("remap.state_interfaces." + state_interface))
+        {
+          ctrl_node->declare_parameter(
+            "remap.state_interfaces." + state_interface, state_interface);
+        }
+      });
     new_state = controller->configure();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
     {
@@ -778,8 +790,8 @@ controller_interface::return_type ControllerManager::configure_controller(
   }
 
   // let's update the list of following and preceding controllers
-  const auto cmd_itfs = controller->command_interface_configuration().names;
-  const auto state_itfs = controller->state_interface_configuration().names;
+  const auto cmd_itfs = found_it->get_command_interface_configuration().names;
+  const auto state_itfs = found_it->get_state_interface_configuration().names;
   for (const auto & cmd_itf : cmd_itfs)
   {
     controller_manager::ControllersListIterator ctrl_it;
@@ -1217,7 +1229,7 @@ controller_interface::return_type ControllerManager::switch_controller(
     const auto extract_interfaces_for_controller =
       [this](const ControllerSpec ctrl, std::vector<std::string> & request_interface_list)
     {
-      auto command_interface_config = ctrl.c->command_interface_configuration();
+      auto command_interface_config = ctrl.get_command_interface_configuration();
       std::vector<std::string> command_interface_names = {};
       if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
@@ -1252,7 +1264,7 @@ controller_interface::return_type ControllerManager::switch_controller(
     {
       std::vector<std::string> interface_names = {};
 
-      auto command_interface_config = controller.c->command_interface_configuration();
+      auto command_interface_config = controller.get_command_interface_configuration();
       if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
         interface_names = resource_manager_->available_command_interfaces();
@@ -1265,7 +1277,7 @@ controller_interface::return_type ControllerManager::switch_controller(
       }
 
       std::vector<std::string> interfaces = {};
-      auto state_interface_config = controller.c->state_interface_configuration();
+      auto state_interface_config = controller.get_state_interface_configuration();
       if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
         interfaces = resource_manager_->available_state_interfaces();
@@ -1350,7 +1362,7 @@ controller_interface::return_type ControllerManager::switch_controller(
   {
     if (is_controller_active(controller.c))
     {
-      auto command_interface_config = controller.c->command_interface_configuration();
+      auto command_interface_config = controller.get_command_interface_configuration();
       if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
         controller.info.claimed_interfaces = resource_manager_->available_command_interfaces();
@@ -1585,7 +1597,7 @@ void ControllerManager::activate_controllers(
 
     bool assignment_successful = true;
     // assign command interfaces to the controller
-    auto command_interface_config = controller->command_interface_configuration();
+    auto command_interface_config = found_it->get_command_interface_configuration();
     // default to controller_interface::configuration_type::NONE
     std::vector<std::string> command_interface_names = {};
     if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
@@ -1630,7 +1642,7 @@ void ControllerManager::activate_controllers(
     }
 
     // assign state interfaces to the controller
-    auto state_interface_config = controller->state_interface_configuration();
+    auto state_interface_config = found_it->get_state_interface_configuration();
     // default to controller_interface::configuration_type::NONE
     std::vector<std::string> state_interface_names = {};
     if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
@@ -1752,7 +1764,7 @@ void ControllerManager::list_controllers_srv_cb(
     // Get information about interfaces if controller are in 'inactive' or 'active' state
     if (is_controller_active(controllers[i].c) || is_controller_inactive(controllers[i].c))
     {
-      auto command_interface_config = controllers[i].c->command_interface_configuration();
+      auto command_interface_config = controllers[i].get_command_interface_configuration();
       if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
         controller_state.required_command_interfaces = resource_manager_->command_interface_keys();
@@ -1764,7 +1776,7 @@ void ControllerManager::list_controllers_srv_cb(
         controller_state.required_command_interfaces = command_interface_config.names;
       }
 
-      auto state_interface_config = controllers[i].c->state_interface_configuration();
+      auto state_interface_config = controllers[i].get_state_interface_configuration();
       if (state_interface_config.type == controller_interface::interface_configuration_type::ALL)
       {
         controller_state.required_state_interfaces = resource_manager_->state_interface_keys();
@@ -2480,8 +2492,8 @@ void ControllerManager::propagate_deactivation_of_chained_mode(
         break;
       }
 
-      const auto ctrl_cmd_itf_names = controller.c->command_interface_configuration().names;
-      const auto ctrl_state_itf_names = controller.c->state_interface_configuration().names;
+      const auto ctrl_cmd_itf_names = controller.get_command_interface_configuration().names;
+      const auto ctrl_state_itf_names = controller.get_state_interface_configuration().names;
       auto ctrl_itf_names = ctrl_cmd_itf_names;
       ctrl_itf_names.insert(
         ctrl_itf_names.end(), ctrl_state_itf_names.begin(), ctrl_state_itf_names.end());
@@ -2518,8 +2530,8 @@ controller_interface::return_type ControllerManager::check_following_controllers
     get_logger(), "Checking following controllers of preceding controller with name '%s'.",
     controller_it->info.name.c_str());
 
-  const auto controller_cmd_interfaces = controller_it->c->command_interface_configuration().names;
-  const auto controller_state_interfaces = controller_it->c->state_interface_configuration().names;
+  const auto controller_cmd_interfaces = controller_it->get_command_interface_configuration().names;
+  const auto controller_state_interfaces = controller_it->get_state_interface_configuration().names;
   // get all interfaces of the controller
   auto controller_interfaces = controller_cmd_interfaces;
   controller_interfaces.insert(

controller_manager/test/test_controller_manager_srvs.cpp

@@ -155,18 +155,19 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
     result->controller[0].claimed_interfaces,
     UnorderedElementsAre(
       "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk",
-      "joint1/position", "joint1/max_velocity"));
+      "joint1/position", "joint1/max_velocity", "joint1/effort"));
   ASSERT_THAT(
     result->controller[0].required_command_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
-      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity"));
+      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort"));
   ASSERT_THAT(
     result->controller[0].required_state_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
       "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
-      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity"));
+      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity",
+      "joint1/effort"));
 
   // Switch with a very low timeout 1 ns and it should fail as there is no enough time to switch
   ASSERT_EQ(
@@ -183,18 +184,19 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
     result->controller[0].claimed_interfaces,
     UnorderedElementsAre(
       "joint2/velocity", "joint3/velocity", "joint2/max_acceleration", "configuration/max_tcp_jerk",
-      "joint1/position", "joint1/max_velocity"));
+      "joint1/position", "joint1/max_velocity", "joint1/effort"));
   ASSERT_THAT(
     result->controller[0].required_command_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
-      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity"));
+      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort"));
   ASSERT_THAT(
     result->controller[0].required_state_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
       "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
-      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity"));
+      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity",
+      "joint1/effort"));
 
   // Try again with higher timeout
   cm_->switch_controller(
@@ -209,13 +211,14 @@ TEST_F(TestControllerManagerSrvs, list_controllers_srv)
     result->controller[0].required_command_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/max_velocity", "joint1/position",
-      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity"));
+      "joint2/max_acceleration", "joint2/velocity", "joint3/velocity", "joint1/effort"));
   ASSERT_THAT(
     result->controller[0].required_state_interfaces,
     UnorderedElementsAre(
       "configuration/max_tcp_jerk", "joint1/position", "joint1/some_unlisted_interface",
       "joint1/velocity", "joint2/acceleration", "joint2/position", "joint2/velocity",
-      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity"));
+      "joint3/acceleration", "joint3/position", "joint3/velocity", "sensor1/velocity",
+      "joint1/effort"));
 
   ASSERT_EQ(
     controller_interface::return_type::OK,

controller_manager/test/test_controller_spawner_with_type.yaml

@@ -25,3 +25,28 @@ ctrl_with_parameters_and_no_type:
 /foo_namespace/ctrl_with_parameters_and_no_type:
   ros__parameters:
     joint_names: ["joint2"]
+
+controller_joint1:
+  ros__parameters:
+    type: "controller_manager/test_controller_with_interfaces"
+    joint_name: "joint1"
+
+controller_joint2:
+  ros__parameters:
+    type: "controller_manager/test_controller_with_interfaces"
+    joint_name: "joint2"
+    remap:
+      state_interfaces:
+        "joint2/effort": "joint2/torque"
+      command_interfaces:
+        "joint2/effort": "joint2/torque"
+
+controller_joint3:
+  ros__parameters:
+    type: "controller_manager/test_controller_with_interfaces"
+    joint_name: "joint3"
+    remap:
+      state_interfaces:
+        "joint3/effort": "joint3/force"
+      command_interfaces:
+        "joint3/effort": "joint3/force"