Issue using more than one hardware interface to control 2 robots

[Zarnack]

Describe the bug
I want to use two robot arms (Franka Emika Panda) at the same time with Moveit 2 and ros2_control. For this I have set up two different hardware components called:

• panda_3_FrankaHardwareInterface
  • with command/state interface like this: panda_3_joint1/....
  • with the panda_3_arm_controller of type joint_trajectory_controller/JointTrajectoryController
• panda_1_FrankaHardwareInterface
  • with command/state interface like this: panda_1_joint1/....
  • with the panda_1_arm_controller of type joint_trajectory_controller/JointTrajectoryController

Using only the fake_components/GenericSystem everything works just fine. I can control both robots and no errors occur.
Using the franka_hardware/FrankaHardwareInterface the problems start:

• Everything works with only one robot (e.g. only using panda_1_arm_controller)
• using a second robot the initialization of the second controller (panda_3_arm_controller) fails:

[ros2_control_node-4] [INFO] [1700586290.750446657] [FrankaHardwareInterface]: Preparing command mode switch
[ros2_control_node-4] [INFO] [1700586290.750465133] [FrankaHardwareInterface]: ++ panda_3_joint1/effort
[ros2_control_node-4] [INFO] [1700586290.750468434] [FrankaHardwareInterface]: ++ panda_3_joint2/effort
[ros2_control_node-4] [INFO] [1700586290.750470715] [FrankaHardwareInterface]: ++ panda_3_joint3/effort
[ros2_control_node-4] [INFO] [1700586290.750472862] [FrankaHardwareInterface]: ++ panda_3_joint4/effort
[ros2_control_node-4] [INFO] [1700586290.750475009] [FrankaHardwareInterface]: ++ panda_3_joint5/effort
[ros2_control_node-4] [INFO] [1700586290.750477177] [FrankaHardwareInterface]: ++ panda_3_joint6/effort
[ros2_control_node-4] [INFO] [1700586290.750479193] [FrankaHardwareInterface]: ++ panda_3_joint7/effort
[ros2_control_node-4] [ERROR] [1700586290.750490794] [FrankaHardwareInterface]: Switching between control modes without stopping it first is not supported.
[ros2_control_node-4] Please stop the running controller first.
[ros2_control_node-4] [ERROR] [1700586290.750508309] [resource_manager]: Component 'panda_1_FrankaHardwareInterface' did not accept new command resource combination: 
[ros2_control_node-4]  Start interfaces: 
[ros2_control_node-4] [
[ros2_control_node-4]   panda_3_joint1/effort
[ros2_control_node-4]   panda_3_joint2/effort
[ros2_control_node-4]   panda_3_joint3/effort
[ros2_control_node-4]   panda_3_joint4/effort
[ros2_control_node-4]   panda_3_joint5/effort
[ros2_control_node-4]   panda_3_joint6/effort
[ros2_control_node-4]   panda_3_joint7/effort
[ros2_control_node-4] ]
[ros2_control_node-4] Stop interfaces: 
[ros2_control_node-4] [
[ros2_control_node-4] ]
[ros2_control_node-4] 
[ros2_control_node-4] [ERROR] [1700586290.750512947] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected.

The join_state_broadcaster still works fine for both robots and I can control the first robot without any problems.
It seems like it is trying to start the panda_3_... interfaces on the wrong hardware component (panda_1_FrankaHardwareInterface instead of panda_3_FrankaHardwareInterface) when activating the panda_3_arm_controller.

Expected behavior
The second controller should start succesfully. The controller manager should try to start the panda_3_... interfaces on the panda_3_FrankaHardwareInterface' and not on the panda_1_FrankaHardwareInterface'.

Questions
Am I doing something wrong here? Do I need to make changes to the hardware interface (e.g. some namespacing stuff) to have it working with multiple instances of itself or is this just a bug?

Environment:

• ROS2 Humble in Docker with Ubuntu 22.04
• tested with ros2_control installed from binaries and from source
• franka_hardware/FrankaHardwareInterface from here

detailed Setup
panda_ros_controllers.yaml:

controller_manager:
  ros__parameters:
    update_rate: 1000  # Hz

    panda_1_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    panda_2_arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

panda_1_arm_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    joints:
      - panda_1_joint1
      - panda_1_joint2
      - panda_1_joint3
      - panda_1_joint4
      - panda_1_joint5
      - panda_1_joint6
      - panda_1_joint7

panda_2_arm_controller:
  ros__parameters:
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity
    joints:
      - panda_2_joint1
      - panda_2_joint2
      - panda_2_joint3
      - panda_2_joint4
      - panda_2_joint5
      - panda_2_joint6
      - panda_2_joint7

ros2_control xacro/urdf setup. This is a macro and called seperately for each robot with the corresponding robot_ip and namespace.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="panda_arm_ros2_control" params="ns robot_ip use_fake_hardware:=^|false fake_sensor_commands:=^|false">
    <ros2_control name="${ns}_HardwareInterface" type="system">
      <hardware>
        <xacro:if value="${use_fake_hardware}">
          <plugin>fake_components/GenericSystem</plugin>
          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
          <param name="state_following_offset">0.0</param>
        </xacro:if>
        <xacro:unless value="${use_fake_hardware}">
          <plugin>franka_hardware/FrankaHardwareInterface</plugin>
          <param name="robot_ip">${robot_ip}</param>
        </xacro:unless>
      </hardware>

      <xacro:macro name="configure_joint" params="joint_name initial_position">
        <joint name="${joint_name}">
          <param name="initial_position">${initial_position}</param>
          <command_interface name="effort"/>
          <command_interface name="position"/>
          <command_interface name="velocity"/>
          <state_interface name="velocity">
            <param name="initial_value">0.0</param> 
          </state_interface>
          <state_interface name="effort">
            <param name="initial_value">0.0</param> 
          </state_interface>
          <state_interface name="position">
            <param name="initial_value">${initial_position}</param> 
          </state_interface>
        </joint>
      </xacro:macro>

      <xacro:configure_joint joint_name="${ns}_joint1" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint2" initial_position="${-pi/4}"/>
      <xacro:configure_joint joint_name="${ns}_joint3" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint4" initial_position="${-3*pi/4}"/>
      <xacro:configure_joint joint_name="${ns}_joint5" initial_position="0.0"/>
      <xacro:configure_joint joint_name="${ns}_joint6" initial_position="${pi/2}"/>
      <xacro:configure_joint joint_name="${ns}_joint7" initial_position="${pi/4}"/>

    </ros2_control>
  </xacro:macro>
</robot>

launch file (simplified and only relevant section):

robot_state_publisher = Node(
    package='robot_state_publisher',
    executable='robot_state_publisher',
    name='robot_state_publisher',
    output='both',
    parameters=[config.robot_description],
)

ros2_controllers_path = os.path.join(
    get_package_share_directory('panda_workcell_config'),
    'config',
    'panda_ros_controllers.yaml',
)
ros2_control_node = Node(
    package='controller_manager',
    executable='ros2_control_node',
    parameters=[config.robot_description,
                config.joint_limits, ros2_controllers_path],
    remappings=[('joint_states', 'franka/joint_states')],
    output={
        'stdout': 'screen',
        'stderr': 'screen',
    },
    on_exit=Shutdown(),
)

# Load controllers
controller_list = ["joint_state_broadcaster", "robot_1_arm_controller", "robot_2_arm_controller"]
load_controllers = []
for controller in controller_list:
    load_controllers += [
        Node(
            package="controller_manager",
            executable="spawner",
            output="log",
            arguments=[controller, "--ros-args", "--log-level", "warn"],
            parameters=[
                config.robot_description,
                config.joint_limits,
            ],
        ),
    ]

Output of ros2 control list_hardware_components (shortened):

Hardware Component 1
        name: panda_3_FrankaHardwareInterface
        type: system
        plugin name: franka_hardware/FrankaHardwareInterface
        state: id=3 label=active
        command interfaces
                panda_3_joint1/effort [available] [unclaimed]
                panda_3_joint1/position [available] [unclaimed]
                panda_3_joint1/velocity [available] [unclaimed]
                panda_3_joint2/effort [available] [unclaimed]
                panda_3_joint2/position [available] [unclaimed]
                panda_3_joint2/velocity [available] [unclaimed]
   ....
Hardware Component 2
        name: panda_1_FrankaHardwareInterface
        type: system
        plugin name: franka_hardware/FrankaHardwareInterface
        state: id=3 label=active
        command interfaces
                panda_1_joint1/effort [available] [claimed]
                panda_1_joint1/position [available] [unclaimed]
                panda_1_joint1/velocity [available] [unclaimed]
                panda_1_joint2/effort [available] [claimed]
                panda_1_joint2/position [available] [unclaimed]
                panda_1_joint2/velocity [available] [unclaimed]
....

Any help or tips will be appreciated!

[Zarnack]

This seems to be a problem with moveit2 and using the default resource manager logic of ros2_control. When I switch the controller manager to the moveit_simple_controller_manager/MoveItSimpleControllerManager this problem disappears.

To change, add this to your yaml describing the controller structure for moveit:

moveit_manage_controllers: True
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager