0
$\begingroup$

I have been trying to load the two controller for two robot, But failed for loading /robot1/controllermanager

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
    
    # Robot 1 description
    robot1_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("robot_description"),
                 "urdf", "practice_multi_update.xacro"]
            ),
            " ",
            "namespace:=",
            "robot1",
        ]
    )
    robot1_description = {"robot_description": robot1_description_content}
    
    # Robot 2 description
    robot2_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [FindPackageShare("robot_description"),
                 "urdf", "practice_multi_update.xacro"]
            ),
            " ",
            "namespace:=",
            "robot2",
        ]
    )
    robot2_description = {"robot_description": robot2_description_content}
    
    # Robot 1 state publisher
    robot1_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        namespace="robot1",
        output="both",
        parameters=[robot1_description],
    )
    
    # Robot 2 state publisher
    robot2_state_pub_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        namespace="robot2",
        output="both",
        parameters=[robot2_description],
    )
    
    # Robot 1 joint state publisher GUI
    robot1_joint_state_publisher_node_gui = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        namespace="robot1"
    )
    
    # Robot 2 joint state publisher GUI
    robot2_joint_state_publisher_node_gui = Node(
        package='joint_state_publisher_gui',
        executable='joint_state_publisher_gui',
        namespace="robot2"
    )
    
    # RViz node
    rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="log",
    )
    
    # Robot 1 load controllers
    robot1_load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_broadcaster', '-c', "/robot1/controller_manager"],
        output='screen'
    )

    robot1_load_joint_trajectory_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             "joint_trajectory_controller", '-c', "/robot1/controller_manager"],
        output='screen'
    )
    
    # Robot 2 load controllers
    robot2_load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             'joint_state_broadcaster', '-c', "/robot2/controller_manager"],
        output='screen'
    )

    robot2_load_joint_trajectory_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
             "joint_trajectory_controller", '-c', "/robot2/controller_manager"],
        output='screen'
    )
    
    # Robot 1 controllers configuration
    robot1_controllers = PathJoinSubstitution(
        [
            FindPackageShare("robot_description"),
            "config",
            "prac_controller_robot1.yaml",
        ]
    )
    
    # Robot 2 controllers configuration
    robot2_controllers = PathJoinSubstitution(
        [
            FindPackageShare("robot_description"),
            "config",
            "prac_controller_robot2.yaml",
        ]
    )
    
    # Robot 1 control node
    robot1_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        namespace="robot1",
        parameters=[robot1_description, robot1_controllers],
        output="both",
    )
    
    # Robot 2 control node
    robot2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        namespace="robot2",
        parameters=[robot2_description, robot2_controllers],
        output="both",
    )
    
    # Robot 1 spawn entity
    robot1_ignition_spawn_entity = Node(
        package='ros_gz_sim',
        executable='create',
        output='screen',
        namespace="robot1",
        arguments=['-string', robot1_description_content,
                   '-name', "robot1",
                   '-allow_renaming', 'true',
                   '-x', "0.0",
                   '-y', "0.0",
                   '-z', "0.0",
                   ],
    )
    
    # Robot 2 spawn entity
    robot2_ignition_spawn_entity = Node(
        package='ros_gz_sim',
        executable='create',
        output='screen',
        namespace="robot2",
        arguments=['-string', robot2_description_content,
                   '-name', "robot2",
                   '-allow_renaming', 'true',
                   '-x', "5.0",
                   '-y', "0.0",
                   '-z', "0.0",
                   ],
    )
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('ros_ign_gazebo'),
                         'launch', 'ign_gazebo.launch.py')
        ),
        launch_arguments={'gz_args': ' -r -v 4 empty.sdf'}.items()
    )

    return LaunchDescription([
        DeclareLaunchArgument('use_sim_time', default_value='true', description='Use simulation time if true'),
        gazebo_launch,
        robot1_state_pub_node,
        robot2_state_pub_node,
        # robot1_joint_state_publisher_node_gui,
        # robot2_joint_state_publisher_node_gui,
        rviz_node,
        robot1_control_node,
        robot2_control_node,
        robot1_ignition_spawn_entity,
        robot2_ignition_spawn_entity,
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=robot1_control_node,
                on_exit=[robot1_load_joint_state_broadcaster],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=robot1_load_joint_state_broadcaster,
                on_exit=[robot1_load_joint_trajectory_controller],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=robot2_control_node,
                on_exit=[robot2_load_joint_state_broadcaster],
            )
        ),
        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=robot2_load_joint_state_broadcaster,
                on_exit=[robot2_load_joint_trajectory_controller],
            )
        ),
    ])

I have the yaml file for robot1

/robot1/controller_manager:
  ros__parameters:
    update_rate: 100 # Hz


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster


    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


/robot1/joint_trajectory_controller:
  ros__parameters:
    joints:
      - robot1_joint_1

    write_op_modes:
      - robot1_joint_1
    
    interface_name: position

    command_interfaces:
      - position
    
    state_interfaces:
      - position
      - velocity

    state_publish_rate: 50.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    hardware_state_has_offset: true
    deduce_states_from_derivatives: true
    
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

I have the yaml file for robot2

/robot2/controller_manager:
  ros__parameters:
    update_rate: 100 # Hz


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster


    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


/robot2/joint_trajectory_controller:
  ros__parameters:
    joints:
      - robot2_joint_1

    write_op_modes:
      - robot2_joint_1
    
    interface_name: position

    command_interfaces:
      - position
    
    state_interfaces:
      - position
      - velocity

    state_publish_rate: 50.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    hardware_state_has_offset: true
    deduce_states_from_derivatives: true
    
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

and xacro file

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot_name">
    <xacro:arg name="namespace" default="RB" />
    <xacro:property name="prefix" value="$(arg namespace)_" />


    <link name="${prefix}base_footprint">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="1.0 1.0 0.1" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="1.0 1.0 0.1" />
            </geometry>
        </collision>
        <inertial>
            <mass value="0.0" />
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
            <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
        </inertial>
    </link>
    <joint name="${prefix}base_joint" type="fixed">
        <parent link="${prefix}base_footprint" />
        <child link="${prefix}base_link" />
        <origin xyz="0.0 0.0 0.05" rpy="0.0 0.0 0.0" />
    </joint>

    <link name="${prefix}base_link">
        <visual>
            <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.1 0.1 1.0" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.1 0.1 1.0" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1.0" />
            <origin xyz="0.0 0.0 0.5" rpy="0.0 0.0 0.0" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
        </inertial>
    </link>
    <joint name="${prefix}joint_1" type="revolute">
        <parent link="${prefix}base_link" />
        <child link="${prefix}child_link" />
        <axis xyz="1 0 0" />
        <limit lower="-3.14159" upper="3.14159" velocity="50.0" effort="1000.0" />
        <origin xyz="0.1 0 0.9" rpy="0 0 0" />
    </joint>
    <link name="${prefix}child_link">
        <visual>
            <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.1 0.1 0.8" />
            </geometry>
            <material name="LightGrey">
                <color rgba="0.7 0.7 0.7 1.0" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0" />
            <geometry>
                <box size="0.1 0.1 0.6" />
            </geometry>
        </collision>
        <inertial>
            <mass value="1.0" />
            <origin xyz="0.0 0.0 0.3" rpy="0.0 0.0 0.0" />
            <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01" />
        </inertial>
    </link>
    <ros2_control name="IgnitionSystem" type="system">
        <hardware>
            <plugin>ign_ros2_control/IgnitionSystem</plugin>
        </hardware>

        <joint name="${prefix}joint_1">
            <command_interface name="position">
                <param name="min">-3</param>
                <param name="max">3</param>
            </command_interface>
            <state_interface name="position" />
            <state_interface name="velocity" />
            <state_interface name="effort" />
        </joint>

    </ros2_control>
    <gazebo>
        <plugin filename="ign_ros2_control-system"
            name="ign_ros2_control::IgnitionROS2ControlPlugin">
            <!-- <robot_param>/$(arg namespace)/robot_description</robot_param> -->
            <!-- <robot_param_node>/$(arg namespace)/robot_state_publisher</robot_param_node> -->
            <controller_manager_prefix_node_name>$(arg namespace)</controller_manager_prefix_node_name>
            <ros>
                <namespace>$(arg namespace)</namespace>
                <remapping>/tf:=/$(arg namespace)/tf</remapping>
            </ros>
            <parameters>$(find robot_description)/config/prac_controller_$(arg namespace).yaml</parameters>
        </plugin>
    </gazebo>
</robot>

Error is

ruby $(which ign) gazebo-1] [INFO] [1720107211.894796495] [GazeboSimROS2ControlPlugin]: robot_param_node is robot_state_publisher
[ruby $(which ign) gazebo-1] [INFO] [1720107211.894832735] [GazeboSimROS2ControlPlugin]: robot_param_node is robot_description
[ruby $(which ign) gazebo-1] [INFO] [1720107212.257423321] [robot2.gz_ros2_control]: connected to service!! /robot2/robot_state_publisher asking for robot_description
[ruby $(which ign) gazebo-1] [INFO] [1720107212.258672981] [robot2.gz_ros2_control]: Received URDF from param server
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263114767] [robot2.gz_ros2_control]: The position_proportional_gain has been set to: 0.1
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263168953] [robot2.gz_ros2_control]: Loading joint: robot2_joint_1
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263180525] [robot2.gz_ros2_control]:    State:
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263189227] [robot2.gz_ros2_control]:             position
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263197684] [robot2.gz_ros2_control]:             velocity
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263205057] [robot2.gz_ros2_control]:             effort
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263211895] [robot2.gz_ros2_control]:    Command:
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263218691] [robot2.gz_ros2_control]:             position
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263269145] [resource_manager]: Initialize hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [WARN] [1720107212.263286180] [robot2.gz_ros2_control]: On init...
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263331854] [resource_manager]: Successful initialization of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263366203] [resource_manager]: 'configure' hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263371511] [robot2.gz_ros2_control]: System Successfully configured!
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263382738] [resource_manager]: Successful 'configure' of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263388804] [resource_manager]: 'activate' hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263394719] [resource_manager]: Successful 'activate' of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.263401876] [robot2.gz_ros2_control]: Loading controller_manager
[ruby $(which ign) gazebo-1] [INFO] [1720107212.343513936] [GazeboSimROS2ControlPlugin]: robot_param_node is robot_state_publisher
[ruby $(which ign) gazebo-1] [INFO] [1720107212.343533347] [GazeboSimROS2ControlPlugin]: robot_param_node is robot_description
[ruby $(which ign) gazebo-1] [INFO] [1720107212.350886256] [robot1.gz_ros2_control]: connected to service!! /robot1/robot_state_publisher asking for robot_description
[ruby $(which ign) gazebo-1] [INFO] [1720107212.351165710] [robot1.gz_ros2_control]: Received URDF from param server
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353692996] [robot1.gz_ros2_control]: The position_proportional_gain has been set to: 0.1
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353748568] [robot1.gz_ros2_control]: Loading joint: robot1_joint_1
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353758832] [robot1.gz_ros2_control]:    State:
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353766922] [robot1.gz_ros2_control]:             position
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353775281] [robot1.gz_ros2_control]:             velocity
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353782920] [robot1.gz_ros2_control]:             effort
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353790410] [robot1.gz_ros2_control]:    Command:
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353797753] [robot1.gz_ros2_control]:             position
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353828786] [resource_manager]: Initialize hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [WARN] [1720107212.353835874] [robot1.gz_ros2_control]: On init...
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353850800] [resource_manager]: Successful initialization of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353864841] [resource_manager]: 'configure' hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353870226] [robot1.gz_ros2_control]: System Successfully configured!
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353880289] [resource_manager]: Successful 'configure' of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353886728] [resource_manager]: 'activate' hardware 'IgnitionSystem' 
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353892961] [resource_manager]: Successful 'activate' of hardware 'IgnitionSystem'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.353898106] [robot1.gz_ros2_control]: Loading controller_manager
[ruby $(which ign) gazebo-1] [WARN] [1720107212.365260629] [robot1.controller_manager]: 'update_rate' parameter not set, using default value.
[ruby $(which ign) gazebo-1] [ERROR] [1720107212.371377999] [robot1.gz_ros2_control]: controller manager doesn't have an update_rate parameter
[ruby $(which ign) gazebo-1] [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v125WorldE], which doesn't have `operator<<`. Component will not be serialized.
[ruby $(which ign) gazebo-1] [GUI] [Wrn] [Component.hh:189] Trying to deserialize component with data type [N3sdf3v125WorldE], which doesn't have `operator>>`. Component will not be deserialized.
[ruby $(which ign) gazebo-1] [WARN] [1720107212.387373019] [robot2.gz_ros2_control]:  Desired controller update period (0.01 s) is slower than the gazebo simulation period (0.001 s).
[ruby $(which ign) gazebo-1] [INFO] [1720107212.497306956] [robot2.controller_manager]: Loading controller 'joint_state_broadcaster'
[ruby $(which ign) gazebo-1] [ERROR] [1720107212.499999677] [robot1.controller_manager]: The 'type' param was not defined for 'joint_state_broadcaster'.
[ros2-9] Error loading controller, check controller_manager logs
[ruby $(which ign) gazebo-1] [INFO] [1720107212.515354525] [robot2.controller_manager]: Configuring controller 'joint_state_broadcaster'
[ruby $(which ign) gazebo-1] [INFO] [1720107212.515618375] [robot2.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ros2-10] Successfully loaded controller joint_state_broadcaster into state active
[ERROR] [ros2-9]: process has died [pid 19933, exit code 1, cmd 'ros2 control load_controller --set-state active joint_state_broadcaster -c /robot1/controller_manager'].
[INFO] [ros2-11]: process started with pid [20199]
[INFO] [ros2-10]: process has finished cleanly [pid 19935]
[INFO] [ros2-12]: process started with pid [20201]
[ruby $(which ign) gazebo-1] [ERROR] [1720107213.626074437] [robot1.controller_manager]: The 'type' param was not defined for 'joint_trajectory_controller'.
[ros2-11] Error loading controller, check controller_manager logs
[ERROR] [ros2-11]: process has died [pid 20199, exit code 1, cmd 'ros2 control load_controller --set-state active joint_trajectory_controller -c /robot1/controller_manager'].
[ruby $(which ign) gazebo-1] [INFO] [1720107215.936363208] [robot2.controller_manager]: Loading controller 'joint_trajectory_controller'
[ruby $(which ign) gazebo-1] [WARN] [1720107215.956249412] [robot2.joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ruby $(which ign) gazebo-1] [INFO] [1720107215.968568208] [robot2.controller_manager]: Configuring controller 'joint_trajectory_controller'
[ruby $(which ign) gazebo-1] [INFO] [1720107215.968697321] [robot2.joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ruby $(which ign) gazebo-1] [INFO] [1720107215.968723966] [robot2.joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ruby $(which ign) gazebo-1] [INFO] [1720107215.968740359] [robot2.joint_trajectory_controller]: Using 'splines' interpolation method.
[ruby $(which ign) gazebo-1] [INFO] [1720107215.969850299] [robot2.joint_trajectory_controller]: Controller state will be published at 50.00 Hz.
[ruby $(which ign) gazebo-1] [INFO] [1720107215.971066267] [robot2.joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2-12] Successfully loaded controller joint_trajectory_controller into state active
[INFO] [ros2-12]: process has finished cleanly [pid 20201]
$\endgroup$

1 Answer 1

0
$\begingroup$

You cannot use ros2_control_node along gazebo/ign/gz_ros2_control, see the ign_ros2_control demos how to spawn your robots in gazebo.

We don't have a multi-robot demo yet, but you should be able to load two gz_ros2_control plugins within their own namespace.

I'm not sure now, maybe the namespaces will be applied twice if you namespace them also in the yaml file? Have a look with rqt_reconfigure or similar, which parameters are set for the two controller managers?

When you get it working, we'd appreciate if you submit a PR with a multi-robot example to gz_ros2_control.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.