Reject duplicate state/command interfaces after configuring the controller

controller_manager/include/controller_manager/controller_manager.hpp

@@ -324,12 +324,20 @@ class ControllerManager : public rclcpp::Node
 
   void initialize_parameters();
 
+  /**
+   * Call cleanup to change the given controller lifecycle node to the unconfigured state.
+   *
+   * \param[in] controller controller to be shutdown.
+   */
+  controller_interface::return_type cleanup_controller(
+    const controller_manager::ControllerSpec & controller);
+
   /**
    * Call shutdown to change the given controller lifecycle node to the finalized state.
    *
    * \param[in] controller controller to be shutdown.
    */
-  void shutdown_controller(controller_manager::ControllerSpec & controller) const;
+  void shutdown_controller(const controller_manager::ControllerSpec & controller) const;
 
   /**
    * Clear request lists used when switching controllers. The lists are shared between "callback"

controller_manager/src/controller_manager.cpp

@@ -253,6 +253,12 @@ void get_controller_list_command_interfaces(
     }
   }
 }
+template <typename Collection>
+[[nodiscard]] bool is_unique(Collection collection)
+{
+  std::sort(collection.begin(), collection.end());
+  return std::adjacent_find(collection.cbegin(), collection.cend()) == collection.cend();
+}
 }  // namespace
 
 namespace controller_manager
@@ -843,7 +849,33 @@ controller_interface::return_type ControllerManager::unload_controller(
   return controller_interface::return_type::OK;
 }
 
-void ControllerManager::shutdown_controller(controller_manager::ControllerSpec & controller) const
+controller_interface::return_type ControllerManager::cleanup_controller(
+  const controller_manager::ControllerSpec & controller)
+{
+  try
+  {
+    cleanup_controller_exported_interfaces(controller);
+    const auto new_state = controller.c->get_node()->cleanup();
+    if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+    {
+      RCLCPP_ERROR(
+        get_logger(), "Controller '%s' is not cleaned-up properly, it is still in state '%s'",
+        controller.info.name.c_str(), new_state.label().c_str());
+      return controller_interface::return_type::ERROR;
+    }
+  }
+  catch (...)
+  {
+    RCLCPP_ERROR(
+      get_logger(), "Caught exception while cleaning-up the controller '%s'",
+      controller.info.name.c_str());
+    return controller_interface::return_type::ERROR;
+  }
+  return controller_interface::return_type::OK;
+}
+
+void ControllerManager::shutdown_controller(
+  const controller_manager::ControllerSpec & controller) const
 {
   try
   {
@@ -913,23 +945,8 @@ controller_interface::return_type ControllerManager::configure_controller(
   {
     RCLCPP_DEBUG(
       get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
-    try
+    if (cleanup_controller(*found_it) != controller_interface::return_type::OK)
     {
-      cleanup_controller_exported_interfaces(*found_it);
-      new_state = controller->get_node()->cleanup();
-      if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
-      {
-        RCLCPP_ERROR(
-          get_logger(), "Controller '%s' can not be cleaned-up before configuring",
-          controller_name.c_str());
-        return controller_interface::return_type::ERROR;
-      }
-    }
-    catch (...)
-    {
-      RCLCPP_ERROR(
-        get_logger(), "Caught exception while cleaning-up controller '%s' before configuring",
-        controller_name.c_str());
       return controller_interface::return_type::ERROR;
     }
   }
@@ -1021,6 +1038,36 @@ 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;
+
+  // Check if the cmd_itfs and the state_itfs are unique
+  if (!is_unique(cmd_itfs))
+  {
+    std::string cmd_itfs_str = std::accumulate(
+      std::next(cmd_itfs.begin()), cmd_itfs.end(), cmd_itfs.front(),
+      [](std::string a, std::string b) { return a + ", " + b; });
+    RCLCPP_ERROR(
+      get_logger(),
+      "The command interfaces of the controller '%s' are not unique. Please make sure that the "
+      "command interfaces are unique : '%s'.",
+      controller_name.c_str(), cmd_itfs_str.c_str());
+    cleanup_controller(*found_it);
+    return controller_interface::return_type::ERROR;
+  }
+
+  if (!is_unique(state_itfs))
+  {
+    std::string state_itfs_str = std::accumulate(
+      std::next(state_itfs.begin()), state_itfs.end(), state_itfs.front(),
+      [](std::string a, std::string b) { return a + ", " + b; });
+    RCLCPP_ERROR(
+      get_logger(),
+      "The state interfaces of the controller '%s' are not unique. Please make sure that the state "
+      "interfaces are unique : '%s'.",
+      controller_name.c_str(), state_itfs_str.c_str());
+    cleanup_controller(*found_it);
+    return controller_interface::return_type::ERROR;
+  }
+
   for (const auto & cmd_itf : cmd_itfs)
   {
     controller_manager::ControllersListIterator ctrl_it;

controller_manager/test/test_controller_manager_srvs.cpp

@@ -592,6 +592,9 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
   auto test_chainable_controller = std::make_shared<TestChainableController>();
   controller_interface::InterfaceConfiguration chained_cmd_cfg = {
     controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
+  controller_interface::InterfaceConfiguration duplicated_chained_cmd_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/position"}};
   controller_interface::InterfaceConfiguration chained_state_cfg = {
     controller_interface::interface_configuration_type::INDIVIDUAL,
     {"joint1/position", "joint1/velocity"}};
@@ -634,6 +637,53 @@ TEST_F(TestControllerManagerSrvs, configure_controller_srv)
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
     test_chainable_controller->get_lifecycle_state().id());
 
+  // Now try to configure the chainable controller with duplicated command interfaces
+  test_chainable_controller->set_command_interface_configuration(duplicated_chained_cmd_cfg);
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_FALSE(result->ok) << "Controller not configured: " << request->name;
+  EXPECT_EQ(
+    test_chainable_controller::TEST_CONTROLLER_NAME,
+    cm_->get_loaded_controllers()[0].c->get_name());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller->get_lifecycle_state().id());
+
+  // Now duplicate state interfaces
+  controller_interface::InterfaceConfiguration duplicated_chained_state_cfg = {
+    controller_interface::interface_configuration_type::INDIVIDUAL,
+    {"joint1/position", "joint1/position", "joint1/velocity"}};
+  test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg);
+  test_chainable_controller->set_state_interface_configuration(duplicated_chained_state_cfg);
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_FALSE(result->ok) << "Controller not configured: " << request->name;
+  EXPECT_EQ(
+    test_chainable_controller::TEST_CONTROLLER_NAME,
+    cm_->get_loaded_controllers()[0].c->get_name());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+    test_chainable_controller->get_lifecycle_state().id());
+
+  // Now try to configure the chainable controller again with unique state and command interfaces
+  test_chainable_controller->set_command_interface_configuration(chained_cmd_cfg);
+  test_chainable_controller->set_state_interface_configuration(chained_state_cfg);
+  result = call_service_and_wait(*client, request, srv_executor, true);
+  ASSERT_TRUE(result->ok);
+  EXPECT_EQ(
+    test_chainable_controller::TEST_CONTROLLER_NAME,
+    cm_->get_loaded_controllers()[0].c->get_name());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    cm_->get_loaded_controllers()[0].c->get_lifecycle_state().id());
+  EXPECT_EQ(
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+    test_chainable_controller->get_lifecycle_state().id());
+
   // now unload the controller and check the state
   auto unload_request = std::make_shared<controller_manager_msgs::srv::UnloadController::Request>();
   unload_request->name = test_controller::TEST_CONTROLLER_NAME;