Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dual UR16e arms #1016

Open
1 task done
eladpar opened this issue Jun 6, 2024 · 9 comments
Open
1 task done

Dual UR16e arms #1016

eladpar opened this issue Jun 6, 2024 · 9 comments

Comments

@eladpar
Copy link

eladpar commented Jun 6, 2024

Affected ROS2 Driver version(s)

2.2.10

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

Build both the ROS driver and UR Client Library from source

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

5.16

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

Hey, iv'e been trying to make 2 UR arms work together on moveit2 and ros2.
I tried doing similar to this:
https://github.com/catmulti7/dual_ur_ros_2
but I guess Im using a newer driver than that project..
im trying to load both ur arms but I get failed to load controllers

[ur_ros2_control_node-13] [ERROR] [1717674177.459959337] [arm_1.controller_manager]: The 'type' param was not defined for 'joint_state_broadcaster'.

this is my launch file:
dual.launch.py:

import os
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch import LaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.actions import DeclareLaunchArgument, LogInfo
from launch.substitutions import LaunchConfiguration

from launch.actions import GroupAction
from launch_ros.actions import PushRosNamespace


def generate_launch_description():

    ur_type = LaunchConfiguration('ur_type') 
    arm_0_robot_ip = LaunchConfiguration('arm_0_robot_ip') 
    arm_0_controller_file = LaunchConfiguration('arm_0_controller_file') 
    arm_0_tf_prefix = LaunchConfiguration('arm_0_tf_prefix') 
    arm_0_script_command_port = LaunchConfiguration('arm_0_script_command_port')
    arm_0_trajectory_port = LaunchConfiguration('arm_0_trajectory_port')
    arm_0_reverse_port = LaunchConfiguration('arm_0_reverse_port')
    arm_0_script_sender_port = LaunchConfiguration('arm_0_script_sender_port')

    arm_1_robot_ip = LaunchConfiguration('arm_1_robot_ip') 
    arm_1_controller_file = LaunchConfiguration('arm_1_controller_file') 
    arm_1_tf_prefix = LaunchConfiguration('arm_1_tf_prefix') 
    arm_1_script_command_port = LaunchConfiguration('arm_1_script_command_port')
    arm_1_trajectory_port = LaunchConfiguration('arm_1_trajectory_port')
    arm_1_reverse_port = LaunchConfiguration('arm_1_reverse_port')
    arm_1_script_sender_port = LaunchConfiguration('arm_1_script_sender_port')

    # # UR specific arguments
    ur_type_arg = DeclareLaunchArgument(
            "ur_type",
            default_value='ur16e',
            description="Type/series of used UR robot.",
            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
    )
    arm_0_robot_ip_arg = DeclareLaunchArgument(
            "arm_0_robot_ip",
            default_value='192.168.1.102',
            description="IP address by which the robot can be reached.",
    )
    arm_0_controller_file_arg = DeclareLaunchArgument(
            "arm_0_controller_file",
            default_value="arm_0_ur_controllers.yaml",
            description="YAML file with the controllers configuration.",
    )
    arm_0_tf_prefix_arg = DeclareLaunchArgument(
            "arm_0_tf_prefix",
            default_value="arm_0_",
            description="tf_prefix of the joint names, useful for \
            multi-robot setup. If changed, also joint names in the controllers' configuration \
            have to be updated.",
    )

    arm_0_script_command_port_arg =  DeclareLaunchArgument(
            "arm_0_script_command_port",
            default_value="50002",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    arm_0_trajectory_port_arg = DeclareLaunchArgument(
            "arm_0_trajectory_port",
            default_value="50003",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    arm_0_reverse_port_arg = DeclareLaunchArgument(
            "arm_0_reverse_port",
            default_value="50001",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    arm_0_script_sender_port_arg = DeclareLaunchArgument(
            "arm_0_script_sender_port",
            default_value="50005",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )


    arm_1_robot_ip_arg = DeclareLaunchArgument(
            "arm_1_robot_ip",
            default_value='192.168.1.103',
            description="IP address by which the robot can be reached.",
    )
    arm_1_controller_file_arg = DeclareLaunchArgument(
            "arm_1_controller_file",
            default_value="arm_1_ur_controllers.yaml",
            description="YAML file with the controllers configuration.",
    )
    arm_1_tf_prefix_arg = DeclareLaunchArgument(
            "arm_1_tf_prefix",
            default_value="arm_1_",
            description="tf_prefix of the joint names, useful for \
            multi-robot setup. If changed, also joint names in the controllers' configuration \
            have to be updated.",
    )
    arm_1_script_command_port_arg =  DeclareLaunchArgument(
            "arm_1_script_command_port",
            default_value="50010",
            description="Port that will be opened to forward script commands from the driver to the robot",
    )

    arm_1_trajectory_port_arg = DeclareLaunchArgument(
            "arm_1_trajectory_port",
            default_value="50009",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    arm_1_reverse_port_arg = DeclareLaunchArgument(
            "arm_1_reverse_port",
            default_value="50006",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    arm_1_script_sender_port_arg = DeclareLaunchArgument(
            "arm_1_script_sender_port",
            default_value="50007",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )


    

    ur_launch_dir = get_package_share_directory('ur_robot_driver')

    arm_0 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ur_launch_dir, 'launch', 'ur_control_dual.launch.py')),
        launch_arguments={'ur_type': ur_type,
                          'robot_ip': arm_0_robot_ip,
                          'controllers_file': arm_0_controller_file,
                          'tf_prefix': arm_0_tf_prefix,
                          'script_command_port': arm_0_script_command_port,
                          'trajectory_port': arm_0_trajectory_port,
                          'reverse_port': arm_0_reverse_port,
                          'script_sender_port': arm_0_script_sender_port,
                          }.items())
    
    arm_0_with_namespace = GroupAction(
     actions=[
         PushRosNamespace('arm_0'),
         arm_0
      ]
    )

    arm_1 = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ur_launch_dir, 'launch', 'ur_control_dual.launch.py')),
        launch_arguments={'ur_type': ur_type,
                          'robot_ip': arm_1_robot_ip,
                          'controllers_file': arm_1_controller_file,
                          'tf_prefix': arm_1_tf_prefix,
                          'script_command_port': arm_1_script_command_port,
                          'trajectory_port': arm_1_trajectory_port,
                          'reverse_port': arm_1_reverse_port,
                          'script_sender_port': arm_1_script_sender_port,
                          }.items())
    
    arm_1_with_namespace = GroupAction(
     actions=[
         PushRosNamespace('arm_1'),
         arm_1
      ]
    )

    
    return LaunchDescription([
        ur_type_arg,
        arm_0_robot_ip_arg,
        arm_0_controller_file_arg,
        arm_0_tf_prefix_arg,
        arm_0_script_command_port_arg,
        arm_0_trajectory_port_arg,
        arm_0_reverse_port_arg,
        arm_0_script_sender_port_arg,
        arm_1_robot_ip_arg,
        arm_1_controller_file_arg,
        arm_1_tf_prefix_arg,
        arm_1_script_command_port_arg,
        arm_1_trajectory_port_arg,
        arm_1_reverse_port_arg,
        arm_1_script_sender_port_arg,

        arm_0_with_namespace,
        arm_1_with_namespace
    ])

and this launchs a modified ur_control.launch.py called
ur_control_dual.launch.py

# Copyright (c) 2021 PickNik, Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#    * Redistributions of source code must retain the above copyright
#      notice, this list of conditions and the following disclaimer.
#
#    * Redistributions in binary form must reproduce the above copyright
#      notice, this list of conditions and the following disclaimer in the
#      documentation and/or other materials provided with the distribution.
#
#    * Neither the name of the {copyright_holder} nor the names of its
#      contributors may be used to endorse or promote products derived from
#      this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.

#
# Author: Denis Stogl

from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution


def launch_setup(context, *args, **kwargs):

    # Initialize Arguments
    ur_type = LaunchConfiguration("ur_type")
    robot_ip = LaunchConfiguration("robot_ip")
    safety_limits = LaunchConfiguration("safety_limits")
    safety_pos_margin = LaunchConfiguration("safety_pos_margin")
    safety_k_position = LaunchConfiguration("safety_k_position")
    # General arguments
    runtime_config_package = LaunchConfiguration("runtime_config_package")
    controllers_file = LaunchConfiguration("controllers_file")
    description_package = LaunchConfiguration("description_package")
    description_file = LaunchConfiguration("description_file")
    tf_prefix = LaunchConfiguration("tf_prefix")
    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
    fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
    controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
    initial_joint_controller = LaunchConfiguration("initial_joint_controller")
    activate_joint_controller = LaunchConfiguration("activate_joint_controller")
    launch_rviz = LaunchConfiguration("launch_rviz")
    headless_mode = LaunchConfiguration("headless_mode")
    launch_dashboard_client = LaunchConfiguration("launch_dashboard_client")
    use_tool_communication = LaunchConfiguration("use_tool_communication")
    tool_parity = LaunchConfiguration("tool_parity")
    tool_baud_rate = LaunchConfiguration("tool_baud_rate")
    tool_stop_bits = LaunchConfiguration("tool_stop_bits")
    tool_rx_idle_chars = LaunchConfiguration("tool_rx_idle_chars")
    tool_tx_idle_chars = LaunchConfiguration("tool_tx_idle_chars")
    tool_device_name = LaunchConfiguration("tool_device_name")
    tool_tcp_port = LaunchConfiguration("tool_tcp_port")
    tool_voltage = LaunchConfiguration("tool_voltage")
    reverse_ip = LaunchConfiguration("reverse_ip")
    script_command_port = LaunchConfiguration("script_command_port")
    reverse_port = LaunchConfiguration("reverse_port")
    script_sender_port = LaunchConfiguration("script_sender_port")
    trajectory_port = LaunchConfiguration("trajectory_port")

    joint_limit_params = PathJoinSubstitution(
        [FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
    )
    kinematics_params = PathJoinSubstitution(
        # [FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"]
        [FindPackageShare("ur_robot_driver"), "config" , tf_prefix, "calibration.yaml"]

    )
    physical_params = PathJoinSubstitution(
        [FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
    )
    visual_params = PathJoinSubstitution(
        [FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
    )
    script_filename = PathJoinSubstitution(
        [FindPackageShare("ur_client_library"), "resources", "external_control.urscript"]
    )
    input_recipe_filename = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
    )
    output_recipe_filename = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "resources", "rtde_output_recipe.txt"]
    )

    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
            " ",
            "robot_ip:=",
            robot_ip,
            " ",
            "joint_limit_params:=",
            joint_limit_params,
            " ",
            "kinematics_params:=",
            kinematics_params,
            " ",
            "physical_params:=",
            physical_params,
            " ",
            "visual_params:=",
            visual_params,
            " ",
            "safety_limits:=",
            safety_limits,
            " ",
            "safety_pos_margin:=",
            safety_pos_margin,
            " ",
            "safety_k_position:=",
            safety_k_position,
            " ",
            "name:=",
            ur_type,
            " ",
            "script_filename:=",
            script_filename,
            " ",
            "input_recipe_filename:=",
            input_recipe_filename,
            " ",
            "output_recipe_filename:=",
            output_recipe_filename,
            " ",
            "tf_prefix:=",
            tf_prefix,
            " ",
            "use_fake_hardware:=",
            use_fake_hardware,
            " ",
            "fake_sensor_commands:=",
            fake_sensor_commands,
            " ",
            "headless_mode:=",
            headless_mode,
            " ",
            "use_tool_communication:=",
            use_tool_communication,
            " ",
            "tool_parity:=",
            tool_parity,
            " ",
            "tool_baud_rate:=",
            tool_baud_rate,
            " ",
            "tool_stop_bits:=",
            tool_stop_bits,
            " ",
            "tool_rx_idle_chars:=",
            tool_rx_idle_chars,
            " ",
            "tool_tx_idle_chars:=",
            tool_tx_idle_chars,
            " ",
            "tool_device_name:=",
            tool_device_name,
            " ",
            "tool_tcp_port:=",
            tool_tcp_port,
            " ",
            "tool_voltage:=",
            tool_voltage,
            " ",
            "reverse_ip:=",
            reverse_ip,
            " ",
            "script_command_port:=",
            script_command_port,
            " ",
            "reverse_port:=",
            reverse_port,
            " ",
            "script_sender_port:=",
            script_sender_port,
            " ",
            "trajectory_port:=",
            trajectory_port,
            " ",
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    initial_joint_controllers = PathJoinSubstitution(
        [FindPackageShare("ur_robot_driver"), "config", controllers_file]
    )

    rviz_config_file = PathJoinSubstitution(
        [FindPackageShare(description_package), "rviz", "view_robot.rviz"]
    )

    # define update rate
    update_rate_config_file = PathJoinSubstitution(
        [
            FindPackageShare("ur_robot_driver"),
            "config",
            ur_type.perform(context) + "_update_rate.yaml",
        ]
    )

    control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[
            robot_description,
            update_rate_config_file,
            ParameterFile(initial_joint_controllers, allow_substs=True),
        ],
        output="screen",
        condition=IfCondition(use_fake_hardware),
    )

    ur_control_node = Node(
        package="ur_robot_driver",
        executable="ur_ros2_control_node",
        parameters=[
            robot_description,
            update_rate_config_file,
            ParameterFile(initial_joint_controllers, allow_substs=True),
        ],
        output="screen",
        condition=UnlessCondition(use_fake_hardware),
    )

    dashboard_client_node = Node(
        package="ur_robot_driver",
        condition=IfCondition(launch_dashboard_client) and UnlessCondition(use_fake_hardware),
        executable="dashboard_client",
        name="dashboard_client",
        output="screen",
        emulate_tty=True,
        parameters=[{"robot_ip": robot_ip}],
    )

    tool_communication_node = Node(
        package="ur_robot_driver",
        condition=IfCondition(use_tool_communication),
        executable="tool_communication.py",
        name="ur_tool_comm",
        output="screen",
        parameters=[
            {
                "robot_ip": robot_ip,
                "tcp_port": tool_tcp_port,
                "device_name": tool_device_name,
            }
        ],
    )

    urscript_interface = Node(
        package="ur_robot_driver",
        executable="urscript_interface",
        parameters=[{"robot_ip": robot_ip}],
        output="screen",
    )

    controller_stopper_node = Node(
        package="ur_robot_driver",
        executable="controller_stopper_node",
        name="controller_stopper",
        output="screen",
        emulate_tty=True,
        condition=UnlessCondition(use_fake_hardware),
        parameters=[
            {"headless_mode": headless_mode},
            {"joint_controller_active": activate_joint_controller},
            {
                "consistent_controllers": [
                    "io_and_status_controller",
                    "force_torque_sensor_broadcaster",
                    "joint_state_broadcaster",
                    "speed_scaling_state_broadcaster",
                ]
            },
        ],
    )

    robot_state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="both",
        parameters=[robot_description],
    )

    rviz_node = Node(
        package="rviz2",
        condition=IfCondition(launch_rviz),
        executable="rviz2",
        name="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
    )

    # Spawn controllers
    def controller_spawner(name, active=True):
        inactive_flags = ["--inactive"] if not active else []
        return Node(
            package="controller_manager",
            executable="spawner",
            arguments=[
                name,
                "--controller-manager",
                "controller_manager",
                "--controller-manager-timeout",
                controller_spawner_timeout,
            ]
            + inactive_flags,
        )

    controller_spawner_names = [
        "joint_state_broadcaster",
        "io_and_status_controller",
        "speed_scaling_state_broadcaster",
        "force_torque_sensor_broadcaster",
    ]
    controller_spawner_inactive_names = ["forward_position_controller"]

    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
    ]

    # There may be other controllers of the joints, but this is the initially-started one
    initial_joint_controller_spawner_started = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            initial_joint_controller,
            "-c",
            "controller_manager",
            "--controller-manager-timeout",
            controller_spawner_timeout,
        ],
        condition=IfCondition(activate_joint_controller),
    )
    initial_joint_controller_spawner_stopped = Node(
        package="controller_manager",
        executable="spawner",
        arguments=[
            initial_joint_controller,
            "-c",
            "controller_manager",
            "--controller-manager-timeout",
            controller_spawner_timeout,
            "--inactive",
        ],
        condition=UnlessCondition(activate_joint_controller),
    )

    nodes_to_start = [
        control_node,
        ur_control_node,
        dashboard_client_node,
        tool_communication_node,
        controller_stopper_node,
        urscript_interface,
        robot_state_publisher_node,
        rviz_node,
        initial_joint_controller_spawner_stopped,
        initial_joint_controller_spawner_started,
    ] + controller_spawners

    return nodes_to_start


def generate_launch_description():
    declared_arguments = []
    # UR specific arguments
    declared_arguments.append(
        DeclareLaunchArgument(
            "ur_type",
            description="Type/series of used UR robot.",
            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "robot_ip", description="IP address by which the robot can be reached."
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "safety_limits",
            default_value="true",
            description="Enables the safety limits controller if true.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "safety_pos_margin",
            default_value="0.15",
            description="The margin to lower and upper limits in the safety controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "safety_k_position",
            default_value="20",
            description="k-position factor in the safety controller.",
        )
    )
    # General arguments
    declared_arguments.append(
        DeclareLaunchArgument(
            "runtime_config_package",
            default_value="ur_robot_driver",
            description='Package with the controller\'s configuration in "config" folder. \
        Usually the argument is not set, it enables use of a custom setup.',
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "controllers_file",
            default_value="ur_controllers.yaml",
            description="YAML file with the controllers configuration.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_package",
            default_value="ur_description",
            description="Description package with robot URDF/XACRO files. Usually the argument \
        is not set, it enables use of a custom description.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "description_file",
            default_value="ur.urdf.xacro",
            description="URDF/XACRO description file with the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tf_prefix",
            default_value="",
            description="tf_prefix of the joint names, useful for \
        multi-robot setup. If changed, also joint names in the controllers' configuration \
        have to be updated.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_fake_hardware",
            default_value="false",
            description="Start robot with fake hardware mirroring command to its states.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "fake_sensor_commands",
            default_value="false",
            description="Enable fake command interfaces for sensors used for simple simulations. \
            Used only if 'use_fake_hardware' parameter is true.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "headless_mode",
            default_value="false",
            description="Enable headless mode for robot control",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "controller_spawner_timeout",
            default_value="10",
            description="Timeout used when spawning controllers.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "initial_joint_controller",
            default_value="scaled_joint_trajectory_controller",
            description="Initially loaded robot controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "activate_joint_controller",
            default_value="true",
            description="Activate loaded joint controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "launch_dashboard_client", default_value="true", description="Launch Dashboard Client?"
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "use_tool_communication",
            default_value="false",
            description="Only available for e series!",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_parity",
            default_value="0",
            description="Parity configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_baud_rate",
            default_value="115200",
            description="Baud rate configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_stop_bits",
            default_value="1",
            description="Stop bits configuration for serial communication. Only effective, if \
            use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_rx_idle_chars",
            default_value="1.5",
            description="RX idle chars configuration for serial communication. Only effective, \
            if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_tx_idle_chars",
            default_value="3.5",
            description="TX idle chars configuration for serial communication. Only effective, \
            if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_device_name",
            default_value="/tmp/ttyUR",
            description="File descriptor that will be generated for the tool communication device. \
            The user has be be allowed to write to this location. \
            Only effective, if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_tcp_port",
            default_value="54321",
            description="Remote port that will be used for bridging the tool's serial device. \
            Only effective, if use_tool_communication is set to True.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "tool_voltage",
            default_value="0",  # 0 being a conservative value that won't destroy anything
            description="Tool voltage that will be setup.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "reverse_ip",
            default_value="0.0.0.0",
            description="IP that will be used for the robot controller to communicate back to the driver.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "script_command_port",
            default_value="50004",
            description="Port that will be opened to forward URScript commands to the robot.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "reverse_port",
            default_value="50001",
            description="Port that will be opened to send cyclic instructions from the driver to the robot controller.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "script_sender_port",
            default_value="50002",
            description="The driver will offer an interface to query the external_control URScript on this port.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "trajectory_port",
            default_value="50003",
            description="Port that will be opened for trajectory control.",
        )
    )
    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

any ideas?

Relevant log output

[INFO] [launch]: All log files can be found below /home/hvmr/.ros/log/2024-06-06-14-42-54-382148-hvmr-NUC12DCMi9-384850
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [dashboard_client-2]: process started with pid [384857]
[INFO] [controller_stopper_node-3]: process started with pid [384859]
[INFO] [dashboard_client-14]: process started with pid [384881]
[INFO] [controller_stopper_node-15]: process started with pid [384883]
[INFO] [ur_ros2_control_node-1]: process started with pid [384855]
[INFO] [urscript_interface-4]: process started with pid [384861]
[INFO] [robot_state_publisher-5]: process started with pid [384863]
[INFO] [rviz2-6]: process started with pid [384865]
[INFO] [spawner-7]: process started with pid [384867]
[INFO] [spawner-8]: process started with pid [384869]
[INFO] [spawner-9]: process started with pid [384871]
[INFO] [spawner-10]: process started with pid [384873]
[INFO] [spawner-11]: process started with pid [384875]
[INFO] [spawner-12]: process started with pid [384877]
[INFO] [ur_ros2_control_node-13]: process started with pid [384879]
[INFO] [urscript_interface-16]: process started with pid [384885]
[INFO] [robot_state_publisher-17]: process started with pid [384887]
[INFO] [rviz2-18]: process started with pid [384890]
[INFO] [spawner-19]: process started with pid [384892]
[INFO] [spawner-20]: process started with pid [384894]
[INFO] [spawner-21]: process started with pid [384897]
[INFO] [spawner-22]: process started with pid [384899]
[INFO] [spawner-23]: process started with pid [384902]
[INFO] [spawner-24]: process started with pid [384942]
[ur_ros2_control_node-1] [WARN] [1717674175.216789262] [arm_0.controller_manager]: 'update_rate' parameter not set, using default value.
[ur_ros2_control_node-1] [WARN] [1717674175.216907815] [arm_0.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ur_ros2_control_node-1] [INFO] [1717674175.217213935] [resource_manager]: Loading hardware 'ur16e' 
[ur_ros2_control_node-1] [INFO] [1717674175.219001371] [resource_manager]: Initialize hardware 'ur16e' 
[ur_ros2_control_node-1] [INFO] [1717674175.219061859] [resource_manager]: Successful initialization of hardware 'ur16e'
[ur_ros2_control_node-1] [WARN] [1717674175.219206142] [arm_0.controller_manager]: [Deprecated]: Automatic activation of all hardware components will not be supported in the future anymore. Use hardware_spawner instead.
[ur_ros2_control_node-1] [INFO] [1717674175.219231865] [resource_manager]: 'configure' hardware 'ur16e' 
[ur_ros2_control_node-1] [INFO] [1717674175.219237441] [resource_manager]: Successful 'configure' of hardware 'ur16e'
[ur_ros2_control_node-1] [INFO] [1717674175.219259761] [resource_manager]: 'activate' hardware 'ur16e' 
[ur_ros2_control_node-1] [INFO] [1717674175.219267025] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-1] [INFO] [1717674175.219278363] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-1] [WARN] [1717674175.221985415] [UR_Client_Library:arm_0_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-1] [INFO] [1717674175.224861646] [UR_Client_Library:arm_0_]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1717674175.225588330] [UR_Client_Library:arm_0_]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-1] [WARN] [1717674175.235280767] [UR_Client_Library:arm_0_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-1] [WARN] [1717674175.235320054] [UR_Client_Library:arm_0_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-1] [INFO] [1717674175.235328352] [URPositionHardwareInterface]: Calibration checksum: 'calib_11928420912726930711'.
[robot_state_publisher-5] [INFO] [1717674175.221047307] [arm_0.robot_state_publisher]: got segment arm_0_base
[robot_state_publisher-5] [INFO] [1717674175.221168014] [arm_0.robot_state_publisher]: got segment arm_0_base_link
[robot_state_publisher-5] [INFO] [1717674175.221175514] [arm_0.robot_state_publisher]: got segment arm_0_base_link_inertia
[robot_state_publisher-5] [INFO] [1717674175.221179254] [arm_0.robot_state_publisher]: got segment arm_0_flange
[robot_state_publisher-5] [INFO] [1717674175.221182258] [arm_0.robot_state_publisher]: got segment arm_0_forearm_link
[robot_state_publisher-5] [INFO] [1717674175.221185228] [arm_0.robot_state_publisher]: got segment arm_0_ft_frame
[robot_state_publisher-5] [INFO] [1717674175.221188375] [arm_0.robot_state_publisher]: got segment arm_0_shoulder_link
[robot_state_publisher-5] [INFO] [1717674175.221192167] [arm_0.robot_state_publisher]: got segment arm_0_tool0
[robot_state_publisher-5] [INFO] [1717674175.221195712] [arm_0.robot_state_publisher]: got segment arm_0_upper_arm_link
[robot_state_publisher-5] [INFO] [1717674175.221199650] [arm_0.robot_state_publisher]: got segment arm_0_wrist_1_link
[robot_state_publisher-5] [INFO] [1717674175.221203998] [arm_0.robot_state_publisher]: got segment arm_0_wrist_2_link
[robot_state_publisher-5] [INFO] [1717674175.221207045] [arm_0.robot_state_publisher]: got segment arm_0_wrist_3_link
[robot_state_publisher-5] [INFO] [1717674175.221209726] [arm_0.robot_state_publisher]: got segment world
[robot_state_publisher-17] [INFO] [1717674175.228476881] [arm_1.robot_state_publisher]: got segment arm_1_base
[robot_state_publisher-17] [INFO] [1717674175.228685882] [arm_1.robot_state_publisher]: got segment arm_1_base_link
[robot_state_publisher-17] [INFO] [1717674175.228712604] [arm_1.robot_state_publisher]: got segment arm_1_base_link_inertia
[robot_state_publisher-17] [INFO] [1717674175.228750998] [arm_1.robot_state_publisher]: got segment arm_1_flange
[robot_state_publisher-17] [INFO] [1717674175.228758878] [arm_1.robot_state_publisher]: got segment arm_1_forearm_link
[robot_state_publisher-17] [INFO] [1717674175.228765275] [arm_1.robot_state_publisher]: got segment arm_1_ft_frame
[robot_state_publisher-17] [INFO] [1717674175.228771584] [arm_1.robot_state_publisher]: got segment arm_1_shoulder_link
[robot_state_publisher-17] [INFO] [1717674175.228777793] [arm_1.robot_state_publisher]: got segment arm_1_tool0
[robot_state_publisher-17] [INFO] [1717674175.228783665] [arm_1.robot_state_publisher]: got segment arm_1_upper_arm_link
[robot_state_publisher-17] [INFO] [1717674175.228790081] [arm_1.robot_state_publisher]: got segment arm_1_wrist_1_link
[robot_state_publisher-17] [INFO] [1717674175.228795943] [arm_1.robot_state_publisher]: got segment arm_1_wrist_2_link
[robot_state_publisher-17] [INFO] [1717674175.228801596] [arm_1.robot_state_publisher]: got segment arm_1_wrist_3_link
[robot_state_publisher-17] [INFO] [1717674175.228807441] [arm_1.robot_state_publisher]: got segment world
[dashboard_client-2] [INFO] [1717674175.218228314] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-2] 
[controller_stopper_node-3] [INFO] [1717674175.222369204] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[dashboard_client-14] [INFO] [1717674175.230351495] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-14] 
[ur_ros2_control_node-13] [WARN] [1717674175.249254511] [arm_1.controller_manager]: 'update_rate' parameter not set, using default value.
[ur_ros2_control_node-13] [WARN] [1717674175.249360181] [arm_1.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ur_ros2_control_node-13] [INFO] [1717674175.249577175] [resource_manager]: Loading hardware 'ur16e' 
[ur_ros2_control_node-13] [INFO] [1717674175.250904588] [resource_manager]: Initialize hardware 'ur16e' 
[ur_ros2_control_node-13] [INFO] [1717674175.250972644] [resource_manager]: Successful initialization of hardware 'ur16e'
[ur_ros2_control_node-13] [WARN] [1717674175.251131352] [arm_1.controller_manager]: [Deprecated]: Automatic activation of all hardware components will not be supported in the future anymore. Use hardware_spawner instead.
[ur_ros2_control_node-13] [INFO] [1717674175.251165736] [resource_manager]: 'configure' hardware 'ur16e' 
[ur_ros2_control_node-13] [INFO] [1717674175.251172674] [resource_manager]: Successful 'configure' of hardware 'ur16e'
[ur_ros2_control_node-13] [INFO] [1717674175.251190167] [resource_manager]: 'activate' hardware 'ur16e' 
[ur_ros2_control_node-13] [INFO] [1717674175.251196645] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-13] [INFO] [1717674175.251206894] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-13] [WARN] [1717674175.252257105] [UR_Client_Library:arm_1_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-13] [INFO] [1717674175.253931189] [UR_Client_Library:arm_1_]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-13] [INFO] [1717674175.254513165] [UR_Client_Library:arm_1_]: Setting up RTDE communication with frequency 500.000000
[controller_stopper_node-15] [INFO] [1717674175.261677382] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[ur_ros2_control_node-13] [WARN] [1717674176.292181946] [UR_Client_Library:arm_1_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-13] [WARN] [1717674176.292202302] [UR_Client_Library:arm_1_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-13] [INFO] [1717674176.292206682] [URPositionHardwareInterface]: Calibration checksum: 'calib_14126653758194988271'.
[ur_ros2_control_node-1] [INFO] [1717674176.328693925] [URPositionHardwareInterface]: Calibration checked successfully.
[ur_ros2_control_node-1] [INFO] [1717674176.328743276] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-1] [INFO] [1717674176.328754160] [resource_manager]: Successful 'activate' of hardware 'ur16e'
[controller_stopper_node-3] [INFO] [1717674176.337273623] [Controller stopper]: Service available
[controller_stopper_node-3] [INFO] [1717674176.337297876] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-3] [INFO] [1717674176.337307480] [Controller stopper]: Service available
[ur_ros2_control_node-1] [WARN] [1717674176.340394487] [arm_0.controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-1] [WARN] [1717674176.340496507] [UR_Client_Library:arm_0_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-1] [ERROR] [1717674176.347687303] [arm_0.controller_manager]: The 'type' param was not defined for 'force_torque_sensor_broadcaster'.
[spawner-11] [FATAL] [1717674176.348178946] [arm_0.spawner_force_torque_sensor_broadcaster]: Failed loading controller force_torque_sensor_broadcaster
[ur_ros2_control_node-1] [ERROR] [1717674176.352734276] [arm_0.controller_manager]: The 'type' param was not defined for 'speed_scaling_state_broadcaster'.
[spawner-10] [FATAL] [1717674176.353104119] [arm_0.spawner_speed_scaling_state_broadcaster]: Failed loading controller speed_scaling_state_broadcaster
[ur_ros2_control_node-1] [ERROR] [1717674176.369829202] [arm_0.controller_manager]: The 'type' param was not defined for 'forward_position_controller'.
[spawner-12] [FATAL] [1717674176.370210981] [arm_0.spawner_forward_position_controller]: Failed loading controller forward_position_controller
[rviz2-18] [INFO] [1717674176.406132330] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-18] [INFO] [1717674176.406190698] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-6] [INFO] [1717674176.407693551] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-6] [INFO] [1717674176.407736539] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[ur_ros2_control_node-1] [ERROR] [1717674176.479975719] [arm_0.controller_manager]: The 'type' param was not defined for 'scaled_joint_trajectory_controller'.
[spawner-7] [FATAL] [1717674176.480340528] [arm_0.spawner_scaled_joint_trajectory_controller]: Failed loading controller scaled_joint_trajectory_controller
[ERROR] [spawner-11]: process has died [pid 384875, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner force_torque_sensor_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_0'].
[rviz2-18] [INFO] [1717674176.494185768] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-6] [INFO] [1717674176.494366654] [rviz2]: Stereo is NOT SUPPORTED
[ERROR] [spawner-10]: process has died [pid 384873, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner speed_scaling_state_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_0'].
[ERROR] [spawner-12]: process has died [pid 384877, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner forward_position_controller --controller-manager controller_manager --controller-manager-timeout 10 --inactive --ros-args -r __ns:=/arm_0'].
[ur_ros2_control_node-1] [ERROR] [1717674176.513044087] [arm_0.controller_manager]: The 'type' param was not defined for 'joint_state_broadcaster'.
[spawner-8] [FATAL] [1717674176.513415460] [arm_0.spawner_joint_state_broadcaster]: Failed loading controller joint_state_broadcaster
[ur_ros2_control_node-1] [ERROR] [1717674176.539427492] [arm_0.controller_manager]: The 'type' param was not defined for 'io_and_status_controller'.
[spawner-9] [FATAL] [1717674176.540199287] [arm_0.spawner_io_and_status_controller]: Failed loading controller io_and_status_controller
[ERROR] [spawner-7]: process has died [pid 384867, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner scaled_joint_trajectory_controller -c controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_0'].
[ERROR] [spawner-8]: process has died [pid 384869, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_state_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_0'].
[ERROR] [spawner-9]: process has died [pid 384871, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner io_and_status_controller --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_0'].
[ur_ros2_control_node-13] [INFO] [1717674177.360554902] [URPositionHardwareInterface]: Calibration checked successfully.
[ur_ros2_control_node-13] [INFO] [1717674177.360611360] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-13] [INFO] [1717674177.360622030] [resource_manager]: Successful 'activate' of hardware 'ur16e'
[controller_stopper_node-15] [INFO] [1717674177.368975609] [Controller stopper]: Service available
[controller_stopper_node-15] [INFO] [1717674177.369013862] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-15] [INFO] [1717674177.369046953] [Controller stopper]: Service available
[ur_ros2_control_node-13] [WARN] [1717674177.377542027] [arm_1.controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-13] [WARN] [1717674177.378224377] [UR_Client_Library:arm_1_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-13] [ERROR] [1717674177.400261696] [arm_1.controller_manager]: The 'type' param was not defined for 'io_and_status_controller'.
[spawner-21] [FATAL] [1717674177.400715061] [arm_1.spawner_io_and_status_controller]: Failed loading controller io_and_status_controller
[ur_ros2_control_node-13] [ERROR] [1717674177.459959337] [arm_1.controller_manager]: The 'type' param was not defined for 'joint_state_broadcaster'.
[spawner-20] [FATAL] [1717674177.460665598] [arm_1.spawner_joint_state_broadcaster]: Failed loading controller joint_state_broadcaster
[ur_ros2_control_node-13] [ERROR] [1717674177.517783005] [arm_1.controller_manager]: The 'type' param was not defined for 'speed_scaling_state_broadcaster'.
[spawner-22] [FATAL] [1717674177.518675958] [arm_1.spawner_speed_scaling_state_broadcaster]: Failed loading controller speed_scaling_state_broadcaster
[ur_ros2_control_node-13] [ERROR] [1717674177.544606234] [arm_1.controller_manager]: The 'type' param was not defined for 'force_torque_sensor_broadcaster'.
[spawner-23] [FATAL] [1717674177.544999199] [arm_1.spawner_force_torque_sensor_broadcaster]: Failed loading controller force_torque_sensor_broadcaster
[ur_ros2_control_node-13] [ERROR] [1717674177.553444154] [arm_1.controller_manager]: The 'type' param was not defined for 'forward_position_controller'.
[spawner-24] [FATAL] [1717674177.553915996] [arm_1.spawner_forward_position_controller]: Failed loading controller forward_position_controller
[ERROR] [spawner-21]: process has died [pid 384897, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner io_and_status_controller --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_1'].
[ur_ros2_control_node-13] [ERROR] [1717674177.575979238] [arm_1.controller_manager]: The 'type' param was not defined for 'scaled_joint_trajectory_controller'.
[spawner-19] [FATAL] [1717674177.576457260] [arm_1.spawner_scaled_joint_trajectory_controller]: Failed loading controller scaled_joint_trajectory_controller
[ERROR] [spawner-20]: process has died [pid 384894, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner joint_state_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_1'].
[ERROR] [spawner-22]: process has died [pid 384899, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner speed_scaling_state_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_1'].
[ERROR] [spawner-23]: process has died [pid 384902, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner force_torque_sensor_broadcaster --controller-manager controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_1'].
[ERROR] [spawner-24]: process has died [pid 384942, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner forward_position_controller --controller-manager controller_manager --controller-manager-timeout 10 --inactive --ros-args -r __ns:=/arm_1'].
[ERROR] [spawner-19]: process has died [pid 384892, exit code 1, cmd '/opt/ros/humble/lib/controller_manager/spawner scaled_joint_trajectory_controller -c controller_manager --controller-manager-timeout 10 --ros-args -r __ns:=/arm_1'].

Accept Public visibility

  • I agree to make this context public
@fmauch
Copy link
Collaborator

fmauch commented Jun 11, 2024

This seems like there's something wrong with the controller configuration passed to the controller manager. The joint_state_broadcaster's type cannot be deduced. This could result from an illegal config file or from the config file not being properly forwarded to the controller manager.

Note: I would recommend using one controller manager for both robots. This way, you would only have to provide one description containing both arms and then start the control node as usual. This would require one common controller file containing all controllers for both arms. I haven't tried this myself, yet, though.

We are currently working on a dual-arm setup tutorial, but this will be targeting rolling/jazzy. Most of that should be backportable to humble, though.

@firesurfer
Copy link
Contributor

@eladpar Let me know if you still need help. We have an running dual-arm setup for more than a year now. So it is definitely possible. But there are quite a few things you have to take care of.

As @fmauch said I would recommend to use a single controller manager. But then you need to make sure that you set non_blocking_read: true and you need to use the default ros2 control controller manager:

from my launch.py

Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robot_description_parameter, controller_config_file],

Other than that I would recommend you to write your own minimal launch file that just starts the controller manager at the beginning. If that works you can continue!

@eladpar
Copy link
Author

eladpar commented Jun 18, 2024

Thank you guys I will keep track with the tutorial in the making..
I managed fixing the controllers errors seems to happen because I didn't address in the controller yaml the namespace of each arm.
Now when activating the remote control in them arm I get
Received 16 bytes on script command interface. Expecting 4 bytes, so ignoring this message

@firesurfer
Copy link
Contributor

Did you make sure to set different ports for both arms ?

These are the reverse ports (on your PC) that are used for the communication afterwards. You need to make sure that for both arms you use different ports.

reverse_port: 50001
script_sender_port: 50002
trajectory_port: 50004
script_command_port: 50005

@eladpar
Copy link
Author

eladpar commented Jun 18, 2024

Did you make sure to set different ports for both arms ?

These are the reverse ports (on your PC) that are used for the communication afterwards. You need to make sure that for both arms you use different ports.

reverse_port: 50001
script_sender_port: 50002
trajectory_port: 50004
script_command_port: 50005

yes you can see at my first file (launch file )
dual.launch.py
that each arm gets different ports

@firesurfer
Copy link
Contributor

It is way easier and less error prone to build your own urdf with both arms. There you make sure to use the correct parameters and instantiate the controller manager with that urdf.

What you are trying to do reminds me of my first try. There I had the issue that it looked fine with mocked hardware but it did not properly propagate the parameter to the correct instance in the urdf.

Just as a reminder: ros2control takes the parameter from the urdf. What we do in the example launch script is passing all arm parameters as xacro args. But they are not unique for both arms as far as I can remember

@levmarki
Copy link

It is way easier and less error prone to build your own urdf with both arms. There you make sure to use the correct parameters and instantiate the controller manager with that urdf.

What you are trying to do reminds me of my first try. There I had the issue that it looked fine with mocked hardware but it did not properly propagate the parameter to the correct instance in the urdf.

Just as a reminder: ros2control takes the parameter from the urdf. What we do in the example launch script is passing all arm parameters as xacro args. But they are not unique for both arms as far as I can remember

Hi firesurfer!
I'm experiencing similar issues to those described by @eladpar earlier. Imagine I'm starting from scratch—what advice would you recommend as the best approach?

@firesurfer
Copy link
Contributor

firesurfer commented Jul 19, 2024

Hi @levmarki
So first of all did you take a look at the (currently it is just a draft) tutorial in this PR: https://github.com/UniversalRobots/Universal_Robots_ROS2_Tutorials/pull/3/files (I did not write it so I can not tell if it is any good).

I can explain how I managed to get multiarm support running:

  1. Get the ur_description package (for communication with real hardware you need of course also the driver)
  2. Then setup a basic xacro where you include <xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro" />
    It is important to directly use the <xacro:ur_robot macro
  3. Then it is is just filling in the necessary configuration parameters twice. In our setup we read it from a configuration file.
    Whats important is that you know which parameters need to stay the same and which needs to be changed for both arms.

The list of parameters that have to unique for both arms:

  • name
  • parent
  • prefix
  • robot_ip
  • reverse_port
  • script_sender_port
  • trajectory_port
  • script_command_port
  • calibration_hash
  • kinematics_file

As I wrote above: You need to select different ports for both arms!
If you want to use the tool communication you also need to adapt the tool_device_name

Additionally you need to set:

  • non_blocking_read="true"
  • keep_alive_count="10"

Furthermore I only managed to get it running in headless_mode

By adjusting the use_fake_hardware parameter you select if ros2control loads the mocked hardware interface or the real hardware interface.

I have to admit btw. that after failing to configure it via the launch file (not sure if that is even possible at the moment) I directly went to: Write my own urdf + own launch file. For debugging purposes it can also help to directly call xarco on your urdf.xacro file and check the outcome.

In the launch file it is btw. important I think to use the normal controller_manager:

   package="controller_manager",
  executable="ros2_control_node",

Additionally for testing (if you just want to make sure not to accidentially move the robot) you can just turn it on but not completely start it on the touch panel: It will then report its joint states but will not allow you to move it.

EDIT: Btw. I also started to add some documentation here: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description/pull/70/files but @fmauch decided to rather implement a full example.

@levmarki
Copy link

@firesurfer
Thank you for your detailed answer!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants