Multiple Robot Workcell with MoveIt in ROS 2

I am trying to simultaneously plan motions and control two Universal Robots UR5e’s using ROS 2 and MoveIt. I have tried to follow the ROS example for a dual robot setup found here, but it seems there have been enough significant structural changes to the Universal Robots Driver between ROS and ROS 2 such that the example is not easily adaptable. It is straight forward to modify the URDF, but the complication arises from modifying the controller and launch files.

Has anyone successfully modified the Universal Robots ROS 2 driver to work with two robots in the same workcell? Or has anyone found an example of this being done? I.e. modifying ur_control.launch.py from the ur_robot_driver package and ur_moveit.launch.py from the ur_moveit ROS 2 package and expanding on these? I would love any help I can get.

Hi, we will publish a tutorial on a dual-arm setup for ROS 2 shortly. In the meantime you can have a look at Added some documentation for multiarm support by firesurfer · Pull Request #70 · UniversalRobots/Universal_Robots_ROS2_Description · GitHub

1 Like

Great, I will look at that. Thank you!

Hi, I’m trying to do the same experiment as Travis. I’m having problems with the ur_control.launch file, how should it be modified for ROS2? Should I convert it from .xml to .py? And also, which topics and nodes should I launch?

It seems that I am perhaps making some progress, but I am running into some issues. I have tried to copy and modify the example of a dual robot arm setup implemented here: GitHub - catmulti7/dual_ur_ros_2, but I am running into issues. I suspect they are related to how I have configured the various communication ports, i.e. the script_command_port, trajectory_port, reverse_port and script_sender_port that are used in the ur_control.launch.py file. I am able to successfully launch a single robot arm, so I am trying to compare the sequence of events and output of that process to those that occur when I try to launch two robot arms. Please see the details below.

Single robot case:

When I launch ur_control.launch.py from the Universal Robots ROS2 driver for a single robot with the appropriate ip address and description package, I get the expected results. Immediately after the processes of the launch file have “finished cleanly”, if I run

ss -atnp | grep 5000

in a separate terminal, I get the following output:

LISTEN 0      1            0.0.0.0:50004         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=33))
LISTEN 0      1            0.0.0.0:50001         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=27))
LISTEN 0      1            0.0.0.0:50003         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=30))
LISTEN 0      1            0.0.0.0:50002         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=24))
ESTAB  0      0          127.0.0.1:52435       127.0.0.1:50000                                            
ESTAB  0      0          127.0.0.1:50000       127.0.0.1:52435 

Then, when I run the external control ur cap on the robot and execute the same ss command, I get the following output:

LISTEN    0      1            0.0.0.0:50004         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=33))
LISTEN    0      1            0.0.0.0:50001         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=27))
LISTEN    0      1            0.0.0.0:50003         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=30))
LISTEN    0      1            0.0.0.0:50002         0.0.0.0:*     users:(("ur_ros2_control",pid=87617,fd=24))
ESTAB     0      32     192.168.1.101:50001   192.168.1.102:54540 users:(("ur_ros2_control",pid=87617,fd=37))
ESTAB     0      0          127.0.0.1:52435       127.0.0.1:50000                                            
ESTAB     0      0          127.0.0.1:50000       127.0.0.1:52435                                            
ESTAB     0      0      192.168.1.101:50003   192.168.1.102:59848 users:(("ur_ros2_control",pid=87617,fd=38))
ESTAB     0      0      192.168.1.101:50004   192.168.1.102:39810 users:(("ur_ros2_control",pid=87617,fd=39))

Dual robot case

When I launch dual_ur_control.launch.py I get the following output:

[INFO] [launch]: All log files can be found below /home/travis/.ros/log/2024-06-03-15-15-19-677873-MEK-2156-telerobotics-89018
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [dashboard_client-2]: process started with pid [89333]
[INFO] [controller_stopper_node-3]: process started with pid [89335]
[INFO] [dashboard_client-13]: process started with pid [89357]
[INFO] [controller_stopper_node-14]: process started with pid [89359]
[INFO] [ur_ros2_control_node-1]: process started with pid [89331]
[INFO] [urscript_interface-4]: process started with pid [89337]
[INFO] [robot_state_publisher-5]: process started with pid [89339]
[INFO] [spawner-6]: process started with pid [89341]
[INFO] [spawner-7]: process started with pid [89343]
[INFO] [spawner-8]: process started with pid [89345]
[INFO] [spawner-9]: process started with pid [89347]
[INFO] [spawner-10]: process started with pid [89349]
[INFO] [spawner-11]: process started with pid [89352]
[INFO] [ur_ros2_control_node-12]: process started with pid [89354]
[INFO] [urscript_interface-15]: process started with pid [89361]
[INFO] [robot_state_publisher-16]: process started with pid [89363]
[INFO] [spawner-17]: process started with pid [89365]
[INFO] [spawner-18]: process started with pid [89367]
[INFO] [spawner-19]: process started with pid [89369]
[INFO] [spawner-20]: process started with pid [89377]
[INFO] [spawner-21]: process started with pid [89401]
[INFO] [spawner-22]: process started with pid [89462]
[robot_state_publisher-5] [INFO] [1717449320.541535654] [north_ur5e.robot_state_publisher]: got segment north_ur5e_base
[robot_state_publisher-5] [INFO] [1717449320.541621487] [north_ur5e.robot_state_publisher]: got segment north_ur5e_base_link
[robot_state_publisher-5] [INFO] [1717449320.541630203] [north_ur5e.robot_state_publisher]: got segment north_ur5e_base_link_inertia
[robot_state_publisher-5] [INFO] [1717449320.541634562] [north_ur5e.robot_state_publisher]: got segment north_ur5e_flange
[robot_state_publisher-5] [INFO] [1717449320.541638580] [north_ur5e.robot_state_publisher]: got segment north_ur5e_forearm_link
[robot_state_publisher-5] [INFO] [1717449320.541641757] [north_ur5e.robot_state_publisher]: got segment north_ur5e_ft_frame
[robot_state_publisher-5] [INFO] [1717449320.541644784] [north_ur5e.robot_state_publisher]: got segment north_ur5e_shoulder_link
[robot_state_publisher-5] [INFO] [1717449320.541648487] [north_ur5e.robot_state_publisher]: got segment north_ur5e_tool0
[robot_state_publisher-5] [INFO] [1717449320.541651897] [north_ur5e.robot_state_publisher]: got segment north_ur5e_upper_arm_link
[robot_state_publisher-5] [INFO] [1717449320.541655223] [north_ur5e.robot_state_publisher]: got segment north_ur5e_wrist_1_link
[robot_state_publisher-5] [INFO] [1717449320.541658441] [north_ur5e.robot_state_publisher]: got segment north_ur5e_wrist_2_link
[robot_state_publisher-5] [INFO] [1717449320.541661504] [north_ur5e.robot_state_publisher]: got segment north_ur5e_wrist_3_link
[robot_state_publisher-5] [INFO] [1717449320.541664670] [north_ur5e.robot_state_publisher]: got segment world
[dashboard_client-2] [INFO] [1717449320.545017822] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-2] 
[controller_stopper_node-3] [INFO] [1717449320.545299415] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[dashboard_client-13] [INFO] [1717449320.549364450] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-13] 
[ur_ros2_control_node-1] [WARN] [1717449320.549186454] [north_ur5e.controller_manager]: 'update_rate' parameter not set, using default value.
[ur_ros2_control_node-1] [WARN] [1717449320.549388907] [north_ur5e.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.
[robot_state_publisher-16] [INFO] [1717449320.549321742] [south_ur5e.robot_state_publisher]: got segment south_ur5e_base
[robot_state_publisher-16] [INFO] [1717449320.549406913] [south_ur5e.robot_state_publisher]: got segment south_ur5e_base_link
[robot_state_publisher-16] [INFO] [1717449320.549415330] [south_ur5e.robot_state_publisher]: got segment south_ur5e_base_link_inertia
[robot_state_publisher-16] [INFO] [1717449320.549419589] [south_ur5e.robot_state_publisher]: got segment south_ur5e_flange
[robot_state_publisher-16] [INFO] [1717449320.549422729] [south_ur5e.robot_state_publisher]: got segment south_ur5e_forearm_link
[robot_state_publisher-16] [INFO] [1717449320.549426041] [south_ur5e.robot_state_publisher]: got segment south_ur5e_ft_frame
[robot_state_publisher-16] [INFO] [1717449320.549430532] [south_ur5e.robot_state_publisher]: got segment south_ur5e_shoulder_link
[robot_state_publisher-16] [INFO] [1717449320.549433804] [south_ur5e.robot_state_publisher]: got segment south_ur5e_tool0
[robot_state_publisher-16] [INFO] [1717449320.549437146] [south_ur5e.robot_state_publisher]: got segment south_ur5e_upper_arm_link
[robot_state_publisher-16] [INFO] [1717449320.549440563] [south_ur5e.robot_state_publisher]: got segment south_ur5e_wrist_1_link
[robot_state_publisher-16] [INFO] [1717449320.549444124] [south_ur5e.robot_state_publisher]: got segment south_ur5e_wrist_2_link
[robot_state_publisher-16] [INFO] [1717449320.549447591] [south_ur5e.robot_state_publisher]: got segment south_ur5e_wrist_3_link
[robot_state_publisher-16] [INFO] [1717449320.549450740] [south_ur5e.robot_state_publisher]: got segment world
[ur_ros2_control_node-1] [INFO] [1717449320.550751241] [resource_manager]: Loading hardware 'ur5e' 
[ur_ros2_control_node-1] [INFO] [1717449320.554560945] [resource_manager]: Initialize hardware 'ur5e' 
[ur_ros2_control_node-1] [INFO] [1717449320.554621102] [resource_manager]: Successful initialization of hardware 'ur5e'
[ur_ros2_control_node-1] [INFO] [1717449320.554760898] [resource_manager]: 'configure' hardware 'ur5e' 
[ur_ros2_control_node-1] [INFO] [1717449320.554765949] [resource_manager]: Successful 'configure' of hardware 'ur5e'
[ur_ros2_control_node-1] [INFO] [1717449320.554781202] [resource_manager]: 'activate' hardware 'ur5e' 
[ur_ros2_control_node-1] [INFO] [1717449320.554787627] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-1] [INFO] [1717449320.554799078] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-1] [WARN] [1717449320.555710906] [UR_Client_Library:north_ur5e_]: 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.
[controller_stopper_node-14] [INFO] [1717449320.558102285] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[ur_ros2_control_node-12] [WARN] [1717449320.569680833] [south_ur5e.controller_manager]: 'update_rate' parameter not set, using default value.
[ur_ros2_control_node-12] [WARN] [1717449320.569846635] [south_ur5e.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-12] [INFO] [1717449320.570084231] [resource_manager]: Loading hardware 'ur5e' 
[ur_ros2_control_node-12] [INFO] [1717449320.571497133] [resource_manager]: Initialize hardware 'ur5e' 
[ur_ros2_control_node-12] [INFO] [1717449320.571541241] [resource_manager]: Successful initialization of hardware 'ur5e'
[ur_ros2_control_node-12] [INFO] [1717449320.571662149] [resource_manager]: 'configure' hardware 'ur5e' 
[ur_ros2_control_node-12] [INFO] [1717449320.571667635] [resource_manager]: Successful 'configure' of hardware 'ur5e'
[ur_ros2_control_node-12] [INFO] [1717449320.571687748] [resource_manager]: 'activate' hardware 'ur5e' 
[ur_ros2_control_node-12] [INFO] [1717449320.571694351] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-12] [INFO] [1717449320.571706809] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-12] [WARN] [1717449320.572211483] [UR_Client_Library:south_ur5e_]: 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-12] [INFO] [1717449320.682071822] [UR_Client_Library:south_ur5e_]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-12] [INFO] [1717449320.682595818] [UR_Client_Library:south_ur5e_]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-1] [INFO] [1717449321.051412684] [UR_Client_Library:north_ur5e_]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1717449321.051582090] [UR_Client_Library:north_ur5e_]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-12] [WARN] [1717449321.713236880] [UR_Client_Library:south_ur5e_]: 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-12] [WARN] [1717449321.713279775] [UR_Client_Library:south_ur5e_]: 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-12] [INFO] [1717449321.713292254] [URPositionHardwareInterface]: Calibration checksum: 'calib_12788084448423163542'.
[ur_ros2_control_node-1] [WARN] [1717449322.062100235] [UR_Client_Library:north_ur5e_]: 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] [1717449322.062162424] [UR_Client_Library:north_ur5e_]: 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] [1717449322.062179460] [URPositionHardwareInterface]: Calibration checksum: 'calib_12788084448423163542'.
[ur_ros2_control_node-12] [ERROR] [1717449322.810660283] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_calibration/README.md] for details.
[ur_ros2_control_node-12] [INFO] [1717449322.810796490] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-12] [INFO] [1717449322.810827488] [resource_manager]: Successful 'activate' of hardware 'ur5e'
[spawner-19] [INFO] [1717449322.816306264] [south_ur5e.spawner_io_and_status_controller]: Waiting for '/south_ur5e/controller_manager' services to be available
[controller_stopper_node-14] [INFO] [1717449322.823699108] [Controller stopper]: Service available
[controller_stopper_node-14] [INFO] [1717449322.823766323] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-14] [INFO] [1717449322.823855123] [Controller stopper]: Service available
[ur_ros2_control_node-12] [WARN] [1717449322.830955969] [south_ur5e.controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-12] [WARN] [1717449322.831279571] [UR_Client_Library:south_ur5e_]: 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-12] [INFO] [1717449322.870102467] [south_ur5e.controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-12] [INFO] [1717449322.883545438] [south_ur5e.speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: south_ur5e_
[spawner-20] [INFO] [1717449322.892290138] [south_ur5e.spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-12] [INFO] [1717449322.893764328] [south_ur5e.controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-12] [INFO] [1717449322.893886390] [south_ur5e.speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[spawner-20] [INFO] [1717449322.921944638] [south_ur5e.spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[ur_ros2_control_node-12] [INFO] [1717449322.947866288] [south_ur5e.controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[spawner-8] [INFO] [1717449322.970584187] [north_ur5e.spawner_io_and_status_controller]: Waiting for '/north_ur5e/controller_manager' services to be available
[spawner-21] [INFO] [1717449322.971898930] [south_ur5e.spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-12] [INFO] [1717449322.972836474] [south_ur5e.controller_manager]: Loading controller 'joint_state_broadcaster'
[ur_ros2_control_node-12] [INFO] [1717449322.991439265] [south_ur5e.controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[spawner-18] [INFO] [1717449322.991887973] [south_ur5e.spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ur_ros2_control_node-12] [INFO] [1717449323.001335287] [south_ur5e.controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-12] [WARN] [1717449323.013211785] [south_ur5e.scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ur_ros2_control_node-12] [INFO] [1717449323.021306735] [south_ur5e.controller_manager]: Configuring controller 'joint_state_broadcaster'
[ur_ros2_control_node-12] [INFO] [1717449323.021356581] [south_ur5e.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-17] [INFO] [1717449323.021883032] [south_ur5e.spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-12] [INFO] [1717449323.031501276] [south_ur5e.controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-12] [INFO] [1717449323.031754596] [south_ur5e.scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-12] [INFO] [1717449323.031787316] [south_ur5e.scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-12] [INFO] [1717449323.031806713] [south_ur5e.scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[ur_ros2_control_node-12] [INFO] [1717449323.033004462] [south_ur5e.scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-12] [INFO] [1717449323.035434456] [south_ur5e.scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[ur_ros2_control_node-12] [INFO] [1717449323.041747156] [south_ur5e.controller_manager]: Loading controller 'io_and_status_controller'
[spawner-10] [INFO] [1717449323.059411549] [north_ur5e.spawner_force_torque_sensor_broadcaster]: Waiting for '/north_ur5e/controller_manager' services to be available
[spawner-7] [INFO] [1717449323.061651867] [north_ur5e.spawner_joint_state_broadcaster]: Waiting for '/north_ur5e/controller_manager' services to be available
[spawner-19] [INFO] [1717449323.062531987] [south_ur5e.spawner_io_and_status_controller]: Loaded io_and_status_controller
[ur_ros2_control_node-1] [ERROR] [1717449323.067790222] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_calibration/README.md] for details.
[ur_ros2_control_node-1] [INFO] [1717449323.067861249] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-1] [INFO] [1717449323.067875635] [resource_manager]: Successful 'activate' of hardware 'ur5e'
[controller_stopper_node-3] [INFO] [1717449323.074264743] [Controller stopper]: Service available
[controller_stopper_node-3] [INFO] [1717449323.074312746] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-3] [INFO] [1717449323.074416022] [Controller stopper]: Service available
[ur_ros2_control_node-1] [WARN] [1717449323.080109229] [north_ur5e.controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-1] [WARN] [1717449323.080426142] [UR_Client_Library:north_ur5e_]: 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-12] [INFO] [1717449323.081628422] [south_ur5e.controller_manager]: Configuring controller 'io_and_status_controller'
[spawner-21] [INFO] [1717449323.082243400] [south_ur5e.spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[INFO] [spawner-20]: process has finished cleanly [pid 89377]
[ur_ros2_control_node-1] [INFO] [1717449323.097011425] [north_ur5e.controller_manager]: Loading controller 'forward_position_controller'
[spawner-18] [INFO] [1717449323.112826263] [south_ur5e.spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[spawner-11] [INFO] [1717449323.121015805] [north_ur5e.spawner_forward_position_controller]: Loaded forward_position_controller
[ur_ros2_control_node-1] [INFO] [1717449323.121950209] [north_ur5e.controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [WARN] [1717449323.131156863] [north_ur5e.scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[spawner-17] [INFO] [1717449323.132001272] [south_ur5e.spawner_scaled_joint_trajectory_controller]: Configured and activated scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1717449323.140772348] [north_ur5e.controller_manager]: Configuring controller 'forward_position_controller'
[spawner-6] [INFO] [1717449323.141407877] [north_ur5e.spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1717449323.142228512] [north_ur5e.forward_position_controller]: configure successful
[ur_ros2_control_node-1] [INFO] [1717449323.150598478] [north_ur5e.controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[spawner-19] [INFO] [1717449323.151824788] [south_ur5e.spawner_io_and_status_controller]: Configured and activated io_and_status_controller
[ur_ros2_control_node-1] [INFO] [1717449323.155501048] [north_ur5e.speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: north_ur5e_
[ur_ros2_control_node-1] [INFO] [1717449323.160465956] [north_ur5e.controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [INFO] [1717449323.160725338] [north_ur5e.scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-1] [INFO] [1717449323.160758026] [north_ur5e.scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-1] [INFO] [1717449323.160767229] [north_ur5e.scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[spawner-9] [INFO] [1717449323.160953242] [north_ur5e.spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1717449323.162031208] [north_ur5e.scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-1] [INFO] [1717449323.164138666] [north_ur5e.scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[ur_ros2_control_node-1] [INFO] [1717449323.170530716] [north_ur5e.controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1717449323.170601252] [north_ur5e.speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[ur_ros2_control_node-1] [INFO] [1717449323.200437763] [north_ur5e.controller_manager]: Loading controller 'io_and_status_controller'
[spawner-6] [INFO] [1717449323.201013485] [north_ur5e.spawner_scaled_joint_trajectory_controller]: Configured and activated scaled_joint_trajectory_controller
[spawner-22] [INFO] [1717449323.219306881] [south_ur5e.spawner_forward_position_controller]: Waiting for '/south_ur5e/controller_manager' node to exist
[spawner-8] [INFO] [1717449323.220907535] [north_ur5e.spawner_io_and_status_controller]: Loaded io_and_status_controller
[INFO] [spawner-21]: process has finished cleanly [pid 89401]
[ur_ros2_control_node-1] [INFO] [1717449323.240696607] [north_ur5e.controller_manager]: Configuring controller 'io_and_status_controller'
[spawner-9] [INFO] [1717449323.241299771] [north_ur5e.spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[INFO] [spawner-18]: process has finished cleanly [pid 89367]
[INFO] [spawner-17]: process has finished cleanly [pid 89365]
[spawner-8] [INFO] [1717449323.270857820] [north_ur5e.spawner_io_and_status_controller]: Configured and activated io_and_status_controller
[ur_ros2_control_node-1] [INFO] [1717449323.271864924] [north_ur5e.controller_manager]: Loading controller 'joint_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1717449323.280559551] [north_ur5e.controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[spawner-7] [INFO] [1717449323.280869047] [north_ur5e.spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[INFO] [spawner-11]: process has finished cleanly [pid 89352]
[INFO] [spawner-19]: process has finished cleanly [pid 89369]
[ur_ros2_control_node-1] [INFO] [1717449323.300515507] [north_ur5e.controller_manager]: Configuring controller 'joint_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1717449323.300612515] [north_ur5e.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-10] [INFO] [1717449323.300952152] [north_ur5e.spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-1] [INFO] [1717449323.310537724] [north_ur5e.controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[INFO] [spawner-6]: process has finished cleanly [pid 89341]
[spawner-7] [INFO] [1717449323.341394591] [north_ur5e.spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[spawner-10] [INFO] [1717449323.361196648] [north_ur5e.spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[INFO] [spawner-9]: process has finished cleanly [pid 89347]
[INFO] [spawner-8]: process has finished cleanly [pid 89345]
[INFO] [spawner-7]: process has finished cleanly [pid 89343]
[INFO] [spawner-10]: process has finished cleanly [pid 89349]
[ur_ros2_control_node-12] [INFO] [1717449324.654508872] [south_ur5e.controller_manager]: Loading controller 'forward_position_controller'
[spawner-22] [INFO] [1717449324.672701368] [south_ur5e.spawner_forward_position_controller]: Loaded forward_position_controller
[ur_ros2_control_node-12] [INFO] [1717449324.674447985] [south_ur5e.controller_manager]: Configuring controller 'forward_position_controller'
[ur_ros2_control_node-12] [INFO] [1717449324.675527530] [south_ur5e.forward_position_controller]: configure successful
[INFO] [spawner-22]: process has finished cleanly [pid 89462]

Immediately after launching, if I run the same ss command as before, I get the following result:

LISTEN    0      1            0.0.0.0:50004         0.0.0.0:*     users:(("ur_ros2_control",pid=94429,fd=30))
LISTEN    0      1            0.0.0.0:50007         0.0.0.0:*     users:(("ur_ros2_control",pid=94451,fd=24))
LISTEN    0      1            0.0.0.0:50001         0.0.0.0:*     users:(("ur_ros2_control",pid=94429,fd=24))
LISTEN    0      1            0.0.0.0:50003         0.0.0.0:*     users:(("ur_ros2_control",pid=94429,fd=27))
LISTEN    0      1            0.0.0.0:50009         0.0.0.0:*     users:(("ur_ros2_control",pid=94451,fd=27))
ESTAB     0      0          127.0.0.1:52435       127.0.0.1:50000                                            
ESTAB     0      0          127.0.0.1:50000       127.0.0.1:52435           

This differs from what I would expect. Perhaps this is naive, but I would expect there to be 8 lines that begin with LISTEN, one for each separate port specified in the dual_ur_control.launch.py

When I try to run the (very same) external control ur cap on the teach pendant of the same robot as before (north_ur5e), I get an error message on the teach pendant saying:

The connection to the remote PC at 192.168.1.101:50002 could not be established. Reason: Connection refused (Connection refused)

When I try to run the external control ur cap on the teach pendant of the south_ur5e (i.e. the other robot), a small window appears on the teach pendant that says Starting Program... Please wait ... The window never updates or goes away. The following is then output to the terminal from which I ran the launch file:

[ur_ros2_control_node-12] [INFO] [1717450303.392779052] [UR_Client_Library:south_ur5e_]: Robot connected to reverse interface. Ready to receive control commands.
[ur_ros2_control_node-12] [WARN] [1717450303.393204380] [UR_Client_Library:south_ur5e_]: Message on ReverseInterface received. The reverse interface currently does not support any message handling. This message will be ignored.

If I press ctrl + c in the terminal from which I launched dual_ur_control.launch.py, the window on the south_ur5e teach pendant gets replaced by a window that warns of a compile error in the program, on the Control by 192.168.2.101 line.

Perhaps this is notable: I have configured the custom ports in the external control on north_ur5e to 50002 and south_ur5e to 50007. The custom port for the north_ur5e is configured like the example given in the Universal Robots driver where the custom port matches the script_sender_port. The custom port for the south_ur5e is configured differently, as an experiment to see what happened. If, instead, I configure the south_ur5e like the north_ur5e, I get the same results as described for the north_ur5e.

A sudo ufw status returns Status: inactive

In any case, my present code does not correctly launch the two arms for control with ROS2 and I am not sure why. (Separated into two posts to manage character limit)

Launch files

The launch files I am using are pasted below. They are adapted versions of the same launch files in this repository: GitHub - catmulti7/dual_ur_ros_2

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') 
    north_ur5e_robot_ip = LaunchConfiguration('north_ur5e_robot_ip') 
    north_ur5e_controller_file = LaunchConfiguration('north_ur5e_controller_file') 
    north_ur5e_tf_prefix = LaunchConfiguration('north_ur5e_tf_prefix') 
    north_ur5e_script_command_port = LaunchConfiguration('north_ur5e_script_command_port')
    north_ur5e_trajectory_port = LaunchConfiguration('north_ur5e_trajectory_port')
    north_ur5e_reverse_port = LaunchConfiguration('north_ur5e_reverse_port')
    north_ur5e_script_sender_port = LaunchConfiguration('north_ur5e_script_sender_port')

    south_ur5e_robot_ip = LaunchConfiguration('south_ur5e_robot_ip') 
    south_ur5e_controller_file = LaunchConfiguration('south_ur5e_controller_file') 
    south_ur5e_tf_prefix = LaunchConfiguration('south_ur5e_tf_prefix') 
    south_ur5e_script_command_port = LaunchConfiguration('south_ur5e_script_command_port')
    south_ur5e_trajectory_port = LaunchConfiguration('south_ur5e_trajectory_port')
    south_ur5e_reverse_port = LaunchConfiguration('south_ur5e_reverse_port')
    south_ur5e_script_sender_port = LaunchConfiguration('south_ur5e_script_sender_port')

    # # UR specific arguments
    ur_type_arg = DeclareLaunchArgument(
            "ur_type",
            default_value='ur5e',
            description="Type/series of used UR robot.",
            choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20"],
    )
    north_ur5e_robot_ip_arg = DeclareLaunchArgument(
            "north_ur5e_robot_ip",
            default_value='192.168.1.102',
            description="IP address by which the robot can be reached.",
    )
    north_ur5e_controller_file_arg = DeclareLaunchArgument(
            "north_ur5e_controller_file",
            default_value="north_ur5e_ur_controllers.yaml",
            description="YAML file with the controllers configuration.",
    )
    north_ur5e_tf_prefix_arg = DeclareLaunchArgument(
            "north_ur5e_tf_prefix",
            default_value="north_ur5e_",
            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.",
    )

    north_ur5e_script_command_port_arg =  DeclareLaunchArgument(
            "north_ur5e_script_command_port",
            default_value="50004",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    north_ur5e_trajectory_port_arg = DeclareLaunchArgument(
            "north_ur5e_trajectory_port",
            default_value="50003",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    north_ur5e_reverse_port_arg = DeclareLaunchArgument(
            "north_ur5e_reverse_port",
            default_value="50001",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    north_ur5e_script_sender_port_arg = DeclareLaunchArgument(
            "north_ur5e_script_sender_port",
            default_value="50002",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )


    south_ur5e_robot_ip_arg = DeclareLaunchArgument(
            "south_ur5e_robot_ip",
            default_value='192.168.2.102',
            description="IP address by which the robot can be reached.",
    )
    south_ur5e_controller_file_arg = DeclareLaunchArgument(
            "south_ur5e_controller_file",
            default_value="south_ur5e_ur_controllers.yaml",
            description="YAML file with the controllers configuration.",
    )
    south_ur5e_tf_prefix_arg = DeclareLaunchArgument(
            "south_ur5e_tf_prefix",
            default_value="south_ur5e_",
            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.",
    )
    south_ur5e_script_command_port_arg =  DeclareLaunchArgument(
            "south_ur5e_script_command_port",
            default_value="50010",
            description="Port that will be opened to forward script commands from the driver to the robot",
    )

    south_ur5e_trajectory_port_arg = DeclareLaunchArgument(
            "south_ur5e_trajectory_port",
            default_value="50009",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    south_ur5e_reverse_port_arg = DeclareLaunchArgument(
            "south_ur5e_reverse_port",
            default_value="50007",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    south_ur5e_script_sender_port_arg = DeclareLaunchArgument(
            "south_ur5e_script_sender_port",
            default_value="50008",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )


    

    ur_launch_dir = get_package_share_directory('test_ur_robot_driver')

    north_ur5e = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ur_launch_dir, 'launch', 'ur_control.launch.py')),
        launch_arguments={'ur_type': ur_type,
                          'robot_ip': north_ur5e_robot_ip,
                          'controllers_file': north_ur5e_controller_file,
                          'tf_prefix': north_ur5e_tf_prefix,
                          'script_command_port': north_ur5e_script_command_port,
                          'trajectory_port': north_ur5e_trajectory_port,
                          'reverse_port': north_ur5e_reverse_port,
                          'script_sender_port': north_ur5e_script_sender_port,
                          }.items())
    
    north_ur5e_with_namespace = GroupAction(
     actions=[
         PushRosNamespace('north_ur5e'),
         north_ur5e
      ]
    )

    south_ur5e = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(ur_launch_dir, 'launch', 'ur_control.launch.py')),
        launch_arguments={'ur_type': ur_type,
                          'robot_ip': south_ur5e_robot_ip,
                          'controllers_file': south_ur5e_controller_file,
                          'tf_prefix': south_ur5e_tf_prefix,
                          'script_command_port': south_ur5e_script_command_port,
                          'trajectory_port': south_ur5e_trajectory_port,
                          'reverse_port': south_ur5e_reverse_port,
                          'script_sender_port': south_ur5e_script_sender_port,
                          }.items())
    
    south_ur5e_with_namespace = GroupAction(
     actions=[
         PushRosNamespace('south_ur5e'),
         south_ur5e
      ]
    )

    
    return LaunchDescription([
        ur_type_arg,
        north_ur5e_robot_ip_arg,
        north_ur5e_controller_file_arg,
        north_ur5e_tf_prefix_arg,
        north_ur5e_script_command_port_arg,
        north_ur5e_trajectory_port_arg,
        north_ur5e_reverse_port_arg,
        north_ur5e_script_sender_port_arg,
        south_ur5e_robot_ip_arg,
        south_ur5e_controller_file_arg,
        south_ur5e_tf_prefix_arg,
        south_ur5e_script_command_port_arg,
        south_ur5e_trajectory_port_arg,
        south_ur5e_reverse_port_arg,
        south_ur5e_script_sender_port_arg,

        north_ur5e_with_namespace,
        south_ur5e_with_namespace
    ])
# 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_mock_hardware = LaunchConfiguration("use_mock_hardware")
    mock_sensor_commands = LaunchConfiguration("mock_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")
    trajectory_port=LaunchConfiguration("trajectory_port")#"50004"
    reverse_port=LaunchConfiguration("reverse_port")#"50001"
    script_sender_port=LaunchConfiguration("script_sender_port")#"50002"

    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"]
    )
    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("test_ur_robot_driver"), "resources", "rtde_input_recipe.txt"]
    )
    output_recipe_filename = PathJoinSubstitution(
        [FindPackageShare("test_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_mock_hardware:=",
            use_mock_hardware,
            " ",
            "mock_sensor_commands:=",
            mock_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,
            " ",
            "trajectory_port:=",
            trajectory_port,
            " ",
            "reverse_port:=",
            reverse_port,
            " ",
            "script_sender_port:=",
            script_sender_port,
            " ",
        ]
    )
    robot_description = {"robot_description": robot_description_content}

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

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

    # define update rate
    update_rate_config_file = PathJoinSubstitution(
        [
            FindPackageShare(runtime_config_package),
            "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_mock_hardware),
    )

    ur_control_node = Node(
        package="test_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_mock_hardware),
    )

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

    tool_communication_node = Node(
        package="test_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="test_ur_robot_driver",
        executable="urscript_interface",
        parameters=[{"robot_ip": robot_ip}],
        output="screen",
    )

    controller_stopper_node = Node(
        package="test_ur_robot_driver",
        executable="controller_stopper_node",
        name="controller_stopper",
        output="screen",
        emulate_tty=True,
        condition=UnlessCondition(use_mock_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="test_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="dual_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_mock_hardware",
            default_value="false",
            description="Start robot with mock hardware mirroring command to its states.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "mock_sensor_commands",
            default_value="false",
            description="Enable mock command interfaces for sensors used for simple simulations. \
            Used only if 'use_mock_hardware' parameter is true.",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "headless_mode",
            default_value="true",
            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 script commands from the driver to the robot",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "trajectory_port",
            default_value="50003",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "reverse_port",
            default_value="50001",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    )
    declared_arguments.append(
        DeclareLaunchArgument(
            "script_sender_port",
            default_value="50002",
            description="Port that will be opened to forward script commands from the driver to the robot",
        )
    )


    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

I am happy to provide any of the code used to generate these results. Does anyone have any advice as to what I can do to fix these issues? Thank you in advance.

Hello, I’m trying to do the exact same thing as @travis3. Does anyone have any updates or know of a repo that is confirmed to work? Thank you!