Fix the same hardware component node naming issue with multiple controller managers setup (#2657) (#2666)

Author: mergify[bot]

controller_manager/src/controller_manager.cpp

@@ -421,6 +421,7 @@ ControllerManager::ControllerManager(
   params.activate_all = activate_all_hw_components;
   params.update_rate = static_cast<unsigned int>(params_->update_rate);
   params.executor = executor_;
+  params.node_namespace = node_namespace;
   params.allow_controller_activation_with_inactive_hardware =
     params_->defaults.allow_controller_activation_with_inactive_hardware;
   params.return_failed_hardware_names_on_return_deactivate_write_cycle_ =
@@ -650,6 +651,7 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   params.clock = trigger_clock_;
   params.logger = this->get_logger();
   params.executor = executor_;
+  params.node_namespace = this->get_namespace();
   params.update_rate = static_cast<unsigned int>(params_->update_rate);
   if (!resource_manager_->load_and_initialize_components(params))
   {

controller_manager/test/test_controller_manager_with_namespace.cpp

@@ -34,11 +34,16 @@ class TestControllerManagerWithNamespace
   void SetUp()
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    hardware_interface::ResourceManagerParams params;
+    params.update_rate = 100;
+    params.clock = rm_node_->get_clock();
+    params.logger = rm_node_->get_logger();
+    params.executor = executor_;
+    params.robot_description = ros2_control_test_assets::minimal_robot_urdf;
+    params.node_namespace = TEST_NAMESPACE;
     cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::minimal_robot_urdf, rm_node_->get_node_clock_interface(),
-        rm_node_->get_node_logging_interface(), true),
-      executor_, TEST_CM_NAME, TEST_NAMESPACE);
+      std::make_unique<hardware_interface::ResourceManager>(params, true), executor_, TEST_CM_NAME,
+      TEST_NAMESPACE);
     run_updater_ = false;
   }
 };
@@ -56,6 +61,15 @@ TEST_P(TestControllerManagerWithNamespace, cm_and_controller_in_namespace)
   EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
   EXPECT_EQ(2, test_controller.use_count());
 
+  const auto all_node_names = cm_->get_node_names();
+  ASSERT_THAT(
+    all_node_names,
+    testing::UnorderedElementsAreArray(
+      {"/test_namespace/test_controller_manager", "/test_namespace/test_controller_name",
+       "/test_namespace/test_controller2_name", "/test_namespace/testactuatorhardware",
+       "/test_namespace/testsensorhardware", "/test_namespace/testsystemhardware",
+       "/ResourceManager"}));
+
   // setup interface to claim from controllers
   controller_interface::InterfaceConfiguration cmd_itfs_cfg;
   cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;

controller_manager/test/test_spawner_unspawner.cpp

@@ -983,6 +983,14 @@ TEST_F(TestLoadControllerWithNamespacedCM, multi_ctrls_test_type_in_param)
   EXPECT_EQ(
     call_spawner("ctrl_1 ctrl_2 -c test_controller_manager --ros-args -r __ns:=/foo_namespace"), 0);
 
+  const auto all_node_names = cm_->get_node_names();
+  ASSERT_THAT(
+    all_node_names,
+    testing::UnorderedElementsAreArray(
+      {"/foo_namespace/test_controller_manager", "/foo_namespace/ctrl_1", "/foo_namespace/ctrl_2",
+       "/ResourceManager", "/foo_namespace/testactuatorhardware",
+       "/foo_namespace/testsensorhardware", "/foo_namespace/testsystemhardware"}));
+
   auto validate_loaded_controllers = [&]()
   {
     auto loaded_controllers = cm_->get_loaded_controllers();

hardware_interface/include/hardware_interface/hardware_component_interface.hpp

@@ -204,13 +204,10 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif
 
     if (auto locked_executor = params.executor.lock())
     {
-      std::string node_name = params.hardware_info.name;
-      std::transform(
-        node_name.begin(), node_name.end(), node_name.begin(),
-        [](unsigned char c) { return std::tolower(c); });
+      std::string node_name = hardware_interface::to_lower_case(params.hardware_info.name);
       std::replace(node_name.begin(), node_name.end(), '/', '_');
-      hardware_component_node_ =
-        std::make_shared<rclcpp::Node>(node_name, get_hardware_component_node_options());
+      hardware_component_node_ = std::make_shared<rclcpp::Node>(
+        node_name, params.node_namespace, get_hardware_component_node_options());
       locked_executor->add_node(hardware_component_node_->get_node_base_interface());
     }
     else

hardware_interface/include/hardware_interface/types/hardware_component_params.hpp

@@ -16,6 +16,7 @@
 #define HARDWARE_INTERFACE__TYPES__HARDWARE_COMPONENT_PARAMS_HPP_
 
 #include <memory>
+#include <string>
 #include "hardware_interface/hardware_info.hpp"
 #include "rclcpp/rclcpp.hpp"
 
@@ -49,6 +50,12 @@ struct HardwareComponentParams
    */
   rclcpp::Clock::SharedPtr clock = nullptr;
 
+  /**
+   * @brief The namespace used by the hardware component's internal node.
+   * This is typically same as the controller manager's node namespace.
+   */
+  std::string node_namespace = "";
+
   /**
    * @brief Weak pointer to the rclcpp::Executor instance. Hardware components
    * can use this (after locking) to add their own internal ROS 2 nodes

hardware_interface/include/hardware_interface/types/resource_manager_params.hpp

@@ -48,6 +48,12 @@ struct ResourceManagerParams
    */
   rclcpp::Logger logger = rclcpp::get_logger("resource_manager");
 
+  /**
+   * @brief The namespace used by the ResourceManager and its components.
+   * This is typically same as the controller manager's node namespace.
+   */
+  std::string node_namespace = "";
+
   /**
    * @brief Shared pointer to the rclcpp::Executor instance that the
    * ResourceManager and its components (including plugins that opt-in) will use.

hardware_interface/src/resource_manager.cpp

@@ -229,6 +229,7 @@ class ResourceStorage
     component_params.clock = rm_clock_;
     component_params.logger = rm_logger_;
     component_params.executor = params.executor;
+    component_params.node_namespace = params.node_namespace;
     RCLCPP_INFO(
       get_logger(), "Initialize hardware '%s' ", component_params.hardware_info.name.c_str());
 
@@ -1298,6 +1299,7 @@ class ResourceStorage
   rclcpp::Clock::SharedPtr rm_clock_;
   rclcpp::Logger rm_logger_;
   rclcpp::Executor::WeakPtr executor_;
+  std::string node_namespace_;
 
   std::vector<Actuator> actuators_;
   std::vector<Sensor> sensors_;
@@ -1479,6 +1481,7 @@ bool ResourceManager::load_and_initialize_components(
     interface_params.executor = resource_storage_->executor_;
     interface_params.clock = resource_storage_->rm_clock_;
     interface_params.logger = resource_storage_->rm_logger_;
+    interface_params.node_namespace = resource_storage_->node_namespace_;
 
     if (individual_hardware_info.type == actuator_type)
     {
@@ -1535,6 +1538,7 @@ bool ResourceManager::load_and_initialize_components(
   resource_storage_->robot_description_ = params.robot_description;
   resource_storage_->cm_update_rate_ = params.update_rate;
   resource_storage_->executor_ = params.executor;
+  resource_storage_->node_namespace_ = params.node_namespace;
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
   return load_and_initialize_components(params.robot_description, params.update_rate);