Parse controller/hardware interface names to support namespacing

The names of controller and hardware interfaces can now be used to
control the target namespace for that interface. For instance, creating
a hardware interface with the name "/my/namespaced/hw_interface" will
create a node called "hw_interface" in the namespace "/my/namespaced".

Author: cadkin

controller_interface/src/controller_interface_base.cpp

@@ -18,6 +18,8 @@
 #include <string>
 #include <vector>
 
+#include "controller_interface/helpers.hpp"
+
 #include "hardware_interface/introspection.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 
@@ -73,9 +75,11 @@ return_type ControllerInterfaceBase::init(
 return_type ControllerInterfaceBase::init(
   const controller_interface::ControllerInterfaceParams & params)
 {
+  std::string node_name = ros2_control::get_component_node_name(params.controller_name);
+
   impl_->ctrl_itf_params_ = params;
   impl_->node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
-    params.controller_name, params.node_namespace, params.node_options,
+    node_name, params.node_namespace, params.node_options,
     false);  // disable LifecycleNode service interfaces
   impl_->lifecycle_id_.store(this->get_lifecycle_state().id(), std::memory_order_release);
 

controller_interface/test/test_controller_interface.cpp

@@ -66,6 +66,33 @@ TEST(TestableControllerInterface, init)
   rclcpp::shutdown();
 }
 
+TEST(TestableControllerInterface, init_with_namespace)
+{
+  char const * const argv[] = {""};
+  int argc = arrlen(argv);
+  rclcpp::init(argc, argv);
+
+  TestableControllerInterface controller;
+
+  // initialize, create node
+  controller_interface::ControllerInterfaceParams params;
+  params.controller_name =
+    std::string(TEST_CONTROLLER_NAMESPACE) + "/" + std::string(TEST_CONTROLLER_NAME);
+  params.robot_description = "";
+  params.update_rate = 10;
+  params.node_namespace = TEST_CONTROLLER_NAMESPACE;
+  params.node_options = controller.define_custom_node_options();
+  controller.init(params);
+
+  // Check names
+  ASSERT_EQ(std::string(controller.get_node()->get_name()), std::string(TEST_CONTROLLER_NAME));
+  ASSERT_EQ(
+    std::string(controller.get_node()->get_namespace()), std::string(TEST_CONTROLLER_NAMESPACE));
+
+  controller.get_node()->shutdown();
+  rclcpp::shutdown();
+}
+
 TEST(TestableControllerInterface, setting_negative_update_rate_in_configure)
 {
   // mocks the declaration of overrides parameters in a yaml file

controller_interface/test/test_controller_interface.hpp

@@ -18,6 +18,7 @@
 #include "controller_interface/controller_interface.hpp"
 
 constexpr char TEST_CONTROLLER_NAME[] = "testable_controller_interface";
+constexpr char TEST_CONTROLLER_NAMESPACE[] = "/test/controller/namespace";
 
 class TestableControllerInterface : public controller_interface::ControllerInterface
 {

controller_manager/controller_manager/spawner.py

@@ -374,6 +374,7 @@ def get_ros_params_files(argv):
 
     # Use the first controller name for the logger/lock
     first_controller_name = controllers[0]["name"]
+    first_controller_name.replace("/", "_")
     logger = rclpy.logging.get_logger("ros2_control_controller_spawner_" + first_controller_name)
 
     try:

controller_manager/doc/userdoc.rst

@@ -152,6 +152,32 @@ When dealing with multiple controller managers, you have two options for managin
        namespace="rrbot",
    )
 
+Controller Namespacing
+----------------------
+Controllers may specify a namespace that the node should be placed in via its name:
+
+.. code-block:: yaml
+   controller_manager:
+     ros__parameters:
+       .....
+
+       /my/namespaced/joint_state_broadcaster:
+         type: joint_state_broadcaster/JointStateBroadcaster
+
+   .....
+
+In this example, the spawned controller node will be named ``joint_state_broadcaster`` and will be placed in the namespace ``/my/namedspaced``.
+
+The name specified to the ``spawn`` helper or the CLI ``load_controller`` command will be the full name specified in the ``controller_manager`` configuration:
+
+.. code-block:: console
+   $ ros2 control load_controller /my/namespaced/joint_state_broadcaster
+
+This is helpful when multiple controllers share the same ``controller_manager`` but require topics/services/actions to be namespaced per controller.
+
+.. note::
+   The namespace semantics are the same as when starting a node via the launch system. Names without a preceeding ``/`` are placed in the namespace relative to the ``controller_manager`` node. Names with a preceeding ``/`` are placed in an absolute namespace.
+
 Helper scripts
 --------------
 There are two scripts to interact with controller manager from launch files:

controller_manager/src/controller_manager.cpp

@@ -2470,6 +2470,9 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
     return nullptr;
   }
 
+  std::string node_namespace =
+    ros2_control::get_component_node_namespace(controller.info.name).value_or(get_namespace());
+
   const rclcpp::NodeOptions controller_node_options = determine_controller_node_options(controller);
   // Catch whatever exception the controller might throw
   try
@@ -2479,7 +2482,7 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_co
     controller_params.robot_description = robot_description_;
     controller_params.update_rate = get_update_rate();
     controller_params.controller_manager_update_rate = get_update_rate();
-    controller_params.node_namespace = get_namespace();
+    controller_params.node_namespace = node_namespace;
     controller_params.node_options = controller_node_options;
     controller_params.hard_joint_limits = resource_manager_->get_hard_joint_limits();
     controller_params.soft_joint_limits = resource_manager_->get_soft_joint_limits();

hardware_interface/doc/hardware_interface_types_userdoc.rst

@@ -40,6 +40,15 @@ A generic example which shows the structure is provided below. More specific exa
       </joint>
     </ros2_control>
 
+Name and Node Namespaces
+*****************************
+The name parameter of the ``<ros2_control>``-tag has :ref:`similar semantics to controller namespacing <doc/ros2_control/controller_manager/doc/userdoc:controller-namespacing>`:
+
+.. code:: xml
+   <ros2_control name="/my/namespaced/hw_interface" type="system">
+
+In this example, when the hardware component is activated, the attached lifecycle node will be named ``hw_interface`` and will be placed in the ``/my/namespaced`` namespace.
+
 Joints
 *****************************
 ``<joint>``-tag groups the interfaces associated with the joints of physical robots and actuators.

hardware_interface/include/hardware_interface/helpers.hpp

@@ -18,6 +18,7 @@
 #include <algorithm>
 #include <functional>
 #include <map>
+#include <optional>
 #include <string>
 #include <unordered_map>
 #include <vector>
@@ -206,6 +207,36 @@ inline std::string strip(const std::string & str)
   return str.substr(start, end - start + 1);
 }
 
+/**
+ * @brief Extract the namespace from a component's (hardware/controller interface) full name.
+ * @param str The component's full name.
+ * @return The namespace of the component without the trailing slash
+ */
+inline std::optional<std::string> get_component_node_namespace(const std::string & component_name)
+{
+  if (component_name.find("/") != std::string::npos)
+  {
+    return component_name.substr(0, component_name.find_last_of('/'));
+  }
+
+  return {};
+}
+
+/**
+ * @brief Extract the name from a component's (hardware/controller interface) full name.
+ * @param str The component's full name.
+ * @return The name of the component without the preceeding slash
+ */
+inline std::string get_component_node_name(const std::string & component_name)
+{
+  if (component_name.find("/") != std::string::npos)
+  {
+    return component_name.substr(component_name.find_last_of('/') + 1, component_name.length());
+  }
+
+  return component_name;
+}
+
 }  // namespace ros2_control
 
 #endif  // HARDWARE_INTERFACE__HELPERS_HPP_

hardware_interface/src/hardware_component_interface.cpp

@@ -19,6 +19,8 @@
 #include <string>
 #include <vector>
 
+#include "hardware_interface/helpers.hpp"
+
 #include "rclcpp/node_options.hpp"
 
 namespace hardware_interface
@@ -122,8 +124,8 @@ CallbackReturn HardwareComponentInterface::init(
 
   if (auto locked_executor = params.executor.lock())
   {
-    std::string node_name = hardware_interface::to_lower_case(params.hardware_info.name);
-    std::replace(node_name.begin(), node_name.end(), '/', '_');
+    std::string node_name = ros2_control::get_component_node_name(
+      hardware_interface::to_lower_case(params.hardware_info.name));
 
     auto options = define_custom_node_options();
     options.arguments({"--ros-args", "-r", "__node:=" + node_name});

hardware_interface/src/resource_manager.cpp

@@ -1566,12 +1566,17 @@ bool ResourceManager::load_and_initialize_components(
       components_are_loaded_and_initialized_ = false;
       break;
     }
+
+    std::string node_namespace =
+      ros2_control::get_component_node_namespace(individual_hardware_info.name)
+        .value_or(params.node_namespace);
+
     hardware_interface::HardwareComponentParams interface_params;
     interface_params.hardware_info = individual_hardware_info;
     interface_params.executor = params.executor;
     interface_params.clock = params.clock;
     interface_params.logger = params.logger;
-    interface_params.node_namespace = params.node_namespace;
+    interface_params.node_namespace = node_namespace;
 
     if (individual_hardware_info.type == actuator_type)
     {