Add namespaced controllers

controller_manager/CMakeLists.txt

@@ -101,6 +101,15 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(test_controller_manager_with_namespaced_controllers
+    test/test_controller_manager_with_namespaced_controllers.cpp
+  )
+  target_link_libraries(test_controller_manager_with_namespaced_controllers
+    controller_manager
+    test_controller
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   ament_add_gmock(test_controller_manager_hardware_error_handling
     test/test_controller_manager_hardware_error_handling.cpp
   )

controller_manager/src/controller_manager.cpp

@@ -68,6 +68,64 @@ bool controller_name_compare(const controller_manager::ControllerSpec & a, const
   return a.info.name == name;
 }
 
+// Pass in full_name and the namespace of the manager node, receive
+/**
+ * \brief Creates controller naming
+ *
+ * A command, that based on the full name of the controller (e.g. from load_controller service call)
+ * calculates the namespace, name and parameter name of the controller.
+ *
+ * If the passed in name does not start with a '/' it is assumed to
+ * be relative to the manager namespace.
+ *
+ * If the passed in name starts with a '/' it is assumed to be
+ * relative to the root namespace. In this case the parameter
+ * is the full name with the initial '/' removed and all other
+ * '/' replaced with '.'.
+ *
+ * \param[in] passed_in_name
+ * \param[in] manager_namespace
+ * \param[out] node_namespace
+ * \param[out] node_name
+ * \param[out] node_parameter_name
+ */
+void calculate_controller_naming(
+  const std::string passed_in_name, const std::string manager_namespace,
+  std::string & node_namespace, std::string & node_name, std::string & node_parameter_name)
+{
+  auto split_pos = passed_in_name.find_last_of('/');
+  if (split_pos == std::string::npos)
+  {
+    node_name = passed_in_name;
+  }
+  else
+  {
+    node_name = passed_in_name.substr(split_pos + 1);
+  }
+  auto first_occ = passed_in_name.find_first_of('/');
+  if (first_occ == std::string::npos)
+  {
+    node_namespace = manager_namespace;
+    node_parameter_name = passed_in_name + ".type";
+  }
+  else if (first_occ != 0)
+  {
+    node_namespace = manager_namespace + '/' + passed_in_name.substr(0, split_pos);
+    node_parameter_name = std::regex_replace(passed_in_name, std::regex("/"), ".") + ".type";
+  }
+  else
+  {
+    node_namespace = passed_in_name.substr(0, split_pos);
+    node_parameter_name =
+      std::regex_replace(passed_in_name.substr(1), std::regex("/"), ".") + ".type";
+  }
+
+  RCLCPP_INFO(
+    rclcpp::get_logger("split_namespace_and_name"),
+    "node_namespace: %s, node_name: %s, node_param %s", node_namespace.c_str(), node_name.c_str(),
+    node_parameter_name.c_str());
+}
+
 /// Checks if a command interface belongs to a controller based on its prefix.
 /**
  * A command interface can be provided by a controller in which case is called "reference"
@@ -455,7 +513,10 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c
 controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_controller(
   const std::string & controller_name)
 {
-  const std::string param_name = controller_name + ".type";
+  std::string controller_name_temp, controller_namespace, param_name;
+  calculate_controller_naming(
+    controller_name, get_namespace(), controller_namespace, controller_name_temp, param_name);
+
   std::string controller_type;
 
   // We cannot declare the parameters for the controllers that will be loaded in the future,
@@ -1138,9 +1199,12 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
       controller.info.name.c_str());
     return nullptr;
   }
-
+  std::string controller_namespace_, controller_name_, controller_param_name_;
+  calculate_controller_naming(
+    controller.info.name, get_namespace(), controller_namespace_, controller_name_,
+    controller_param_name_);
   if (
-    controller.c->init(controller.info.name, get_namespace()) ==
+    controller.c->init(controller_name_, controller_namespace_) ==
     controller_interface::return_type::ERROR)
   {
     to.clear();

controller_manager/test/test_controller_manager_with_namespaced_controllers.cpp

@@ -0,0 +1,235 @@
+// Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt)
+// Copyright 2023 Christoph Hellmann Santos
+//
+// 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.
+
+#include <gtest/gtest.h>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "controller_manager/controller_manager.hpp"
+#include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_test_common.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+#include "test_controller/test_controller.hpp"
+
+using ::testing::_;
+using ::testing::Return;
+
+class TestControllerManagerWithNamespacedControllers
+: public ControllerManagerFixture<controller_manager::ControllerManager>,
+  public testing::WithParamInterface<Strictness>
+{
+public:
+  void SetUp()
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    cm_ = std::make_shared<controller_manager::ControllerManager>(
+      std::make_unique<hardware_interface::ResourceManager>(
+        ros2_control_test_assets::minimal_robot_urdf, true, true),
+      executor_, TEST_CM_NAME, "/test_namespace");
+    run_updater_ = false;
+  }
+};
+
+TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_absolute_namespace)
+{
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  constexpr char TEST_CONTROLLER1_NAME[] = "/device1/test_controller_name";
+  constexpr char TEST_CONTROLLER2_NAME[] = "/device2/test_controller_name";
+  cm_->add_controller(
+    test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+
+  // setup interface to claim from controllers
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg;
+  state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_state_interface_configuration(state_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
+  cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg2.names.push_back(interface);
+  }
+  test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg2;
+  state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
+  test_controller2->set_state_interface_configuration(state_itfs_cfg2);
+
+  // Check if namespace is set correctly
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
+    cm_->get_namespace());
+  EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
+    test_controller->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/device1");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
+    test_controller2->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/device2");
+}
+
+TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_same_namespace)
+{
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  constexpr char TEST_CONTROLLER1_NAME[] = "test_controller1_name";
+  constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
+  cm_->add_controller(
+    test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+
+  // setup interface to claim from controllers
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg;
+  state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_state_interface_configuration(state_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
+  cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg2.names.push_back(interface);
+  }
+  test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg2;
+  state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
+  test_controller2->set_state_interface_configuration(state_itfs_cfg2);
+
+  // Check if namespace is set correctly
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
+    cm_->get_namespace());
+  EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
+    test_controller->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
+    test_controller2->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace");
+}
+
+TEST_P(TestControllerManagerWithNamespacedControllers, controller_in_relative_namespace)
+{
+  auto test_controller = std::make_shared<test_controller::TestController>();
+  auto test_controller2 = std::make_shared<test_controller::TestController>();
+  constexpr char TEST_CONTROLLER1_NAME[] = "device1/test_controller1_name";
+  constexpr char TEST_CONTROLLER2_NAME[] = "device2/test_controller2_name";
+  cm_->add_controller(
+    test_controller, TEST_CONTROLLER1_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+  cm_->add_controller(
+    test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
+
+  EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
+  EXPECT_EQ(2, test_controller.use_count());
+
+  // setup interface to claim from controllers
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg;
+  cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_command_interface_configuration(cmd_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg;
+  state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
+  {
+    state_itfs_cfg.names.push_back(interface);
+  }
+  test_controller->set_state_interface_configuration(state_itfs_cfg);
+
+  controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
+  cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+  for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
+  {
+    cmd_itfs_cfg2.names.push_back(interface);
+  }
+  test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
+
+  controller_interface::InterfaceConfiguration state_itfs_cfg2;
+  state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
+  test_controller2->set_state_interface_configuration(state_itfs_cfg2);
+
+  // Check if namespace is set correctly
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller Manager namespace is '%s'",
+    cm_->get_namespace());
+  EXPECT_STREQ(cm_->get_namespace(), "/test_namespace");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 1 namespace is '%s'",
+    test_controller->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/test_namespace/device1");
+  RCLCPP_INFO(
+    rclcpp::get_logger("test_controll_manager_namespace"), "Controller 2 namespace is '%s'",
+    test_controller2->get_node()->get_namespace());
+  EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/test_namespace/device2");
+}
+
+INSTANTIATE_TEST_SUITE_P(
+  test_strict_best_effort, TestControllerManagerWithNamespacedControllers,
+  testing::Values(strict, best_effort));