[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 <utility>
@@ -313,12 +314,32 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy
   CONTROLLER_INTERFACE_PUBLIC
   void wait_for_trigger_update_to_finish();
 
+  /// 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_;
   std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
   unsigned int update_rate_ = 0;
   bool is_async_ = false;

controller_interface/src/controller_interface_base.cpp

@@ -32,11 +32,48 @@ return_type ControllerInterfaceBase::init(
     controller_name, node_namespace, node_options,
     false);  // disable LifecycleNode service interfaces
 
+  auto retrieve_remap_interface = [this](
+                                    const std::string & remap_namespace,
+                                    std::map<std::string, std::string> & remappings) -> bool
+  {
+    if (!node_->has_parameter(remap_namespace))
+    {
+      node_->declare_parameters(remap_namespace, remappings);
+    }
+    return node_->get_parameters(remap_namespace, remappings);
+  };
+
   try
   {
     auto_declare<int>("update_rate", update_rate_);
     auto_declare<bool>("is_async", false);
     auto_declare<int>("thread_priority", 50);
+    if (
+      retrieve_remap_interface("remap.state_interfaces", state_interfaces_remap_) &&
+      !state_interfaces_remap_.empty())
+    {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "The controller : %s will be able to remap the following state interfaces:",
+        controller_name.c_str());
+      for (const auto & [key, value] : state_interfaces_remap_)
+      {
+        RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str());
+      }
+    }
+    if (
+      retrieve_remap_interface("remap.command_interfaces", command_interfaces_remap_) &&
+      !command_interfaces_remap_.empty())
+    {
+      RCLCPP_WARN(
+        node_->get_logger(),
+        "The controller : %s will be able to remap the following command interfaces:",
+        controller_name.c_str());
+      for (const auto & [key, value] : command_interfaces_remap_)
+      {
+        RCLCPP_INFO(node_->get_logger(), "\t'%s' to '%s'", key.c_str(), value.c_str());
+      }
+    }
   }
   catch (const std::exception & e)
   {
@@ -129,7 +166,70 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure()
   }
   trigger_stats_.reset();
 
-  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(
@@ -205,4 +305,16 @@ void ControllerInterfaceBase::wait_for_trigger_update_to_finish()
     async_handler_->wait_for_trigger_cycle_to_finish();
   }
 }
+
+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

@@ -767,6 +767,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)
     {
@@ -850,8 +862,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;
@@ -1294,7 +1306,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)
       {
@@ -1329,7 +1341,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();
@@ -1342,7 +1354,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();
@@ -1439,7 +1451,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();
@@ -1674,7 +1686,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)
@@ -1722,7 +1734,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)
@@ -1842,7 +1854,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();
@@ -1854,7 +1866,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();
@@ -2633,8 +2645,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());
@@ -2671,8 +2683,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(