Ros2ControlMultiManager is not working properly

[wizard-coder]

Description

Problem

When using two ROS2 controls, the namespace is automatically detected, but the namespace is not properly applied when registering the action client.

The execution of the two ROS controls is as follows:

# left arm controller
    left_ros2_control_config = os.path.join(
        get_package_share_directory(MOVEIT_CONFIG_PACKAGE),
        "config",
        "ros2_controllers.left.yaml",
    )
    left_ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="both",
        parameters=[left_ros2_control_config],
        namespace="left",
        remappings=[
            ("robot_description", "/robot_description"),
            ("joint_states", "/joint_states"),
        ],
        condition=IfCondition(simulation),
        emulate_tty=True,
    )
    left_joint_trajectory_controller_spawner_node = Node(
        package="controller_manager",
        executable="spawner",
        output="both",
        arguments=[JOINT_TRAJECTORY_CONTROLLER],
        namespace="left",
        condition=IfCondition(simulation),
        emulate_tty=True,
    )
    left_joint_state_broadcaster_spawner_node = Node(
        package="controller_manager",
        executable="spawner",
        output="both",
        arguments=["joint_state_broadcaster"],
        namespace="left",
        condition=IfCondition(simulation),
        emulate_tty=True,
    )

    # right arm controller
    right_ros2_control_config = os.path.join(
        get_package_share_directory(MOVEIT_CONFIG_PACKAGE),
        "config",
        "ros2_controllers.right.yaml",
    )
    right_ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        output="both",
        parameters=[right_ros2_control_config],
        namespace="right",
        remappings=[
            ("robot_description", "/robot_description"),
            ("joint_states", "/joint_states"),
        ],
        condition=IfCondition(simulation),
        emulate_tty=True,
    )
    right_joint_trajectory_controller_spawner_node = Node(
        package="controller_manager",
        executable="spawner",
        output="both",
        arguments=[JOINT_TRAJECTORY_CONTROLLER],
        namespace="right",
        condition=IfCondition(simulation),
        emulate_tty=True,
    )
    right_joint_state_broadcaster_spawner_node = Node(
        package="controller_manager",
        executable="spawner",
        output="both",
        arguments=["joint_state_broadcaster"],
        namespace="right",
        condition=IfCondition(simulation),
        emulate_tty=True,
    )//

When executed this way, the following ros_control_interface log is displayed

However, when checking the /moveit_simple_controller_manager node, only the /controller_manager namespace exists, and the /left/controller_manager and /right/controller_manager namespaces are missing.

analysis

From the log, it can be seen that the namespace is correctly found based on the service name, and the Ros2ControlManager is properly initialized.
controller_manager_plugin.cpp#L555-L581

void discover()
  {
    // Skip if last discovery is too new for discovery rate
    if ((node_->now() - controller_managers_stamp_) < CONTROLLER_INFORMATION_VALIDITY_AGE)
      return;

    controller_managers_stamp_ = node_->now();

    const std::map<std::string, std::vector<std::string>> services = node_->get_service_names_and_types();

    for (const auto& service : services)
    {
      const auto& service_name = service.first;
      std::size_t found = service_name.find("controller_manager/list_controllers");
      if (found != std::string::npos)
      {
        std::string ns = service_name.substr(0, found);
        if (controller_managers_.find(ns) == controller_managers_.end())
        {  // create Ros2ControlManager if it does not exist
          RCLCPP_INFO_STREAM(getLogger(), "Adding controller_manager interface for node at namespace " << ns);
          auto controller_manager = std::make_shared<moveit_ros_control_interface::Ros2ControlManager>(ns);
          controller_manager->initialize(node_);
          controller_managers_.insert(std::make_pair(ns, controller_manager));
        }
      }
    }
  }

controller_manager_plugin.cpp#L258-L262

Ros2ControlManager(const std::string& ns)
    : ns_(ns), loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
  {
    RCLCPP_INFO_STREAM(getLogger(), "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_);
  }

The issue seems to be within the initialize function of the Ros2ControlManager.
Since ns_ is not empty, if ros_control_namespace is not defined, it will be decided as the / namespace; if it is defined, it will be determined by the defined namespace.
I think if ns_ is not empty, it should be skipped.

controller_manager_plugin.cpp#L264-L291

void initialize(const rclcpp::Node::SharedPtr& node) override
  {
    node_ = node;
    if (!ns_.empty())
    {
      if (!node_->has_parameter("ros_control_namespace"))
      {
        ns_ = node_->declare_parameter<std::string>("ros_control_namespace", "/");
      }
      else
      {
        node_->get_parameter<std::string>("ros_control_namespace", ns_);
      }
    }
    else if (node->has_parameter("ros_control_namespace"))
    {
      node_->get_parameter<std::string>("ros_control_namespace", ns_);
      RCLCPP_INFO_STREAM(getLogger(), "Namespace for controller manager was specified, namespace: " << ns_);
    }

    list_controllers_service_ = node_->create_client<controller_manager_msgs::srv::ListControllers>(
        getAbsName("controller_manager/list_controllers"));
    switch_controller_service_ = node_->create_client<controller_manager_msgs::srv::SwitchController>(
        getAbsName("controller_manager/switch_controller"));

    std::scoped_lock<std::mutex> lock(controllers_mutex_);
    discover(true);
  }

I confirmed that it works correctly after modifying it as below. However, I haven't thoroughly tested it in various cases.

    if (ns_.empty())
    {
      if (!node_->has_parameter("ros_control_namespace"))
      {
        ns_ = node_->declare_parameter<std::string>("ros_control_namespace", "/");
      }
      else
      {
        node_->get_parameter<std::string>("ros_control_namespace", ns_);
      }
    }

ROS Distro

Jazzy

OS and version

Ubuntu24.04

Source or binary build?

Binary

If binary, which release version?

2.10.0-1noble.20241118.194622

If source, which branch?

No response

Which RMW are you using?

CycloneDDS

Steps to Reproduce

.

Expected behavior

There should be two, /left/controller_manager and /right/controller_manager, in the /moveit_simple_controller_manager node.

Actual behavior

when checking the /moveit_simple_controller_manager node, only the /controller_manager namespace exists, and the /left/controller_manager and /right/controller_manager namespaces are missing.

Backtrace or Console output

No response