Connecting to robot during boot

[urmahp]

This problem is related to the changes just merged in the client library with the bootup, see the merged pull request. The problem only occurs when the robot is actually booting or if you connect to the robot just after it has booted.

The problem is that the hardware interface is waiting for the robot to come up and the controller manager is not available during this. We are still trying to spawn the controllers, but since the controller manager is unavailable this fails and essential times out (I believe) and the controllers will therefore not be spawned (see terminal output). The hardware interface comes up as expected once the robot is booted. See below terminal output.

To reproduce just launch the ROS2 driver while the robot is booting.

ros2 launch ur_bringup ur_control.launch.py ur_type:=ur5 robot_ip:=127.0.0.1
[INFO] [launch]: All log files can be found below /root/.ros/log/2022-04-22-13-54-53-837437-ur-li-95qgtn2.unirobotts.local-3053
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [dashboard_client-2]: process started with pid [3059]
[INFO] [controller_stopper_node-3]: process started with pid [3061]
[INFO] [ur_ros2_control_node-1]: process started with pid [3057]
[INFO] [robot_state_publisher-4]: process started with pid [3063]
[INFO] [rviz2-5]: process started with pid [3065]
[INFO] [spawner-6]: process started with pid [3070]
[INFO] [spawner-7]: process started with pid [3072]
[INFO] [spawner-8]: process started with pid [3081]
[INFO] [spawner-9]: process started with pid [3092]
[INFO] [spawner-10]: process started with pid [3103]
[INFO] [spawner-11]: process started with pid [3115]
[ur_ros2_control_node-1] 1650635695.829920 [0] ur_ros2_co: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[ur_ros2_control_node-1] [INFO] [1650635695.867884505] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-1] [INFO] [1650635695.868007683] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-1] [WARN] [1650635695.885376801] [UR_Client_Library]: No realtime capabilities found. Consider using a realtime system for better performance
[robot_state_publisher-4] 1650635695.816748 [0] robot_stat: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[robot_state_publisher-4] Link base_link had 2 children
[robot_state_publisher-4] Link base had 0 children
[robot_state_publisher-4] Link base_link_inertia had 1 children
[robot_state_publisher-4] Link shoulder_link had 1 children
[robot_state_publisher-4] Link upper_arm_link had 1 children
[robot_state_publisher-4] Link forearm_link had 1 children
[robot_state_publisher-4] Link wrist_1_link had 1 children
[robot_state_publisher-4] Link wrist_2_link had 1 children
[robot_state_publisher-4] Link wrist_3_link had 2 children
[robot_state_publisher-4] Link flange had 1 children
[robot_state_publisher-4] Link tool0 had 0 children
[robot_state_publisher-4] Link ft_frame had 0 children
[robot_state_publisher-4] [INFO] [1650635695.844032175] [robot_state_publisher]: got segment base
[robot_state_publisher-4] [INFO] [1650635695.844197970] [robot_state_publisher]: got segment base_link
[robot_state_publisher-4] [INFO] [1650635695.845600512] [robot_state_publisher]: got segment base_link_inertia
[robot_state_publisher-4] [INFO] [1650635695.845638551] [robot_state_publisher]: got segment flange
[robot_state_publisher-4] [INFO] [1650635695.845652881] [robot_state_publisher]: got segment forearm_link
[robot_state_publisher-4] [INFO] [1650635695.845664592] [robot_state_publisher]: got segment ft_frame
[robot_state_publisher-4] [INFO] [1650635695.845683872] [robot_state_publisher]: got segment shoulder_link
[robot_state_publisher-4] [INFO] [1650635695.845695094] [robot_state_publisher]: got segment tool0
[robot_state_publisher-4] [INFO] [1650635695.845706381] [robot_state_publisher]: got segment upper_arm_link
[robot_state_publisher-4] [INFO] [1650635695.845717236] [robot_state_publisher]: got segment world
[robot_state_publisher-4] [INFO] [1650635695.845728150] [robot_state_publisher]: got segment wrist_1_link
[robot_state_publisher-4] [INFO] [1650635695.845739622] [robot_state_publisher]: got segment wrist_2_link
[robot_state_publisher-4] [INFO] [1650635695.845759032] [robot_state_publisher]: got segment wrist_3_link
[rviz2-5] qt.qpa.xcb: could not connect to display 
[rviz2-5] qt.qpa.plugin: Could not load the Qt platform plugin "xcb" in "" even though it was found.
[rviz2-5] This application failed to start because no Qt platform plugin could be initialized. Reinstalling the application may fix this problem.
[rviz2-5] 
[rviz2-5] Available platform plugins are: eglfs, linuxfb, minimal, minimalegl, offscreen, vnc, xcb.
[rviz2-5] 
[dashboard_client-2] 1650635695.808438 [0] dashboard_: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[dashboard_client-2] [ERROR] [1650635695.826066429] [UR_Client_Library]: Failed to connect to robot on IP 127.0.0.1. Please check that the robot is booted and reachable on 127.0.0.1. Retrying in 10 seconds
[controller_stopper_node-3] 1650635695.890780 [0] controller: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[controller_stopper_node-3] [INFO] [1650635695.899571418] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[spawner-7] 1650635696.104941 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[ur_ros2_control_node-1] [WARN] [1650635696.125363099] [UR_Client_Library]: Did not receive protocol negotiation answer from robot. Message received instead: 
[ur_ros2_control_node-1] message: SafetySetup has not been confirmed yet, therefore no data will be send yet
[ur_ros2_control_node-1] source: 
[ur_ros2_control_node-1] warning level: 0. Retrying...
[ur_ros2_control_node-1] [INFO] [1650635696.125411611] [UR_Client_Library]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1650635696.130790204] [UR_Client_Library]: Setting up RTDE communication with frequency 500.000000
[spawner-6] 1650635696.184467 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[spawner-7] [INFO] [1650635696.310689028] [spawner_io_and_status_controller]: Waiting for '/controller_manager' services to be available
[spawner-10] 1650635696.317702 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[spawner-6] [INFO] [1650635696.321891165] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[ERROR] [rviz2-5]: process has died [pid 3065, exit code -6, cmd '/opt/ros/galactic/lib/rviz2/rviz2 -d /~/workspace/ros_ws_ur_driver/install/ur_description/share/ur_description/rviz/view_robot.rviz --ros-args -r __node:=rviz2'].
[spawner-8] 1650635696.332632 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[spawner-9] 1650635696.350844 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[spawner-9] [INFO] [1650635696.460968878] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] 1650635696.463989 [0]    python3: using network interface enx482ae3ba050a (udp/10.53.253.27) selected arbitrarily from: enx482ae3ba050a, wlp2s0, docker0, br-3362adecda1e
[spawner-10] [INFO] [1650635696.513349352] [spawner_forward_position_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1650635696.539262403] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] [INFO] [1650635696.643291983] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[ur_ros2_control_node-1] [ERROR] [1650635698.165402498] [UR_Client_Library]: Failed to initialize RTDE client, retrying in 10 seconds
[spawner-7] [INFO] [1650635698.336689576] [spawner_io_and_status_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1650635698.344016238] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-9] [INFO] [1650635698.477897500] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-10] [INFO] [1650635698.530526098] [spawner_forward_position_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1650635698.557948508] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] [INFO] [1650635698.662132858] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[spawner-7] [INFO] [1650635700.361872638] [spawner_io_and_status_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1650635700.363234718] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-9] [INFO] [1650635700.493866938] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-10] [INFO] [1650635700.547488700] [spawner_forward_position_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1650635700.575418206] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] [INFO] [1650635700.677958256] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[spawner-7] [INFO] [1650635702.375798324] [spawner_io_and_status_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1650635702.378271506] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-9] [INFO] [1650635702.507009384] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-10] [INFO] [1650635702.563519736] [spawner_forward_position_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1650635702.588778757] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] [INFO] [1650635702.693941690] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[spawner-7] [INFO] [1650635704.392885581] [spawner_io_and_status_controller]: Waiting for '/controller_manager' services to be available
[spawner-6] [INFO] [1650635704.395893863] [spawner_joint_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-9] [INFO] [1650635704.523839564] [spawner_force_torque_sensor_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-10] [INFO] [1650635704.578200192] [spawner_forward_position_controller]: Waiting for '/controller_manager' services to be available
[spawner-8] [INFO] [1650635704.606414809] [spawner_speed_scaling_state_broadcaster]: Waiting for '/controller_manager' services to be available
[spawner-11] [INFO] [1650635704.709107068] [spawner_scaled_joint_trajectory_controller]: Waiting for '/controller_manager' services to be available
[dashboard_client-2] [ERROR] [1650635705.826385917] [UR_Client_Library]: Failed to connect to robot on IP 127.0.0.1. Please check that the robot is booted and reachable on 127.0.0.1. Retrying in 10 seconds
[spawner-7] [ERROR] [1650635706.407647245] [spawner_io_and_status_controller]: Controller manager not available
[spawner-6] [ERROR] [1650635706.415199044] [spawner_joint_state_broadcaster]: Controller manager not available
[spawner-9] [ERROR] [1650635706.538108241] [spawner_force_torque_sensor_broadcaster]: Controller manager not available
[spawner-10] [ERROR] [1650635706.593400310] [spawner_forward_position_controller]: Controller manager not available
[spawner-8] [ERROR] [1650635706.625617778] [spawner_speed_scaling_state_broadcaster]: Controller manager not available
[spawner-11] [ERROR] [1650635706.729242868] [spawner_scaled_joint_trajectory_controller]: Controller manager not available
[ERROR] [spawner-7]: process has died [pid 3072, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner io_and_status_controller -c /controller_manager --ros-args'].
[ERROR] [spawner-6]: process has died [pid 3070, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner joint_state_broadcaster --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-9]: process has died [pid 3092, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner force_torque_sensor_broadcaster --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-10]: process has died [pid 3103, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner forward_position_controller -c /controller_manager --stopped --ros-args'].
[ERROR] [spawner-8]: process has died [pid 3081, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner speed_scaling_state_broadcaster --controller-manager /controller_manager --ros-args'].
[ERROR] [spawner-11]: process has died [pid 3115, exit code 1, cmd '/~/workspace/ros_ws_ur_driver/install/controller_manager/lib/controller_manager/spawner scaled_joint_trajectory_controller -c /controller_manager --ros-args'].
[ur_ros2_control_node-1] [WARN] [1650635708.167080349] [UR_Client_Library]: No realtime capabilities found. Consider using a realtime system for better performance
[ur_ros2_control_node-1] [WARN] [1650635708.834964839] [UR_Client_Library]: Did not receive protocol negotiation answer from robot. Message received instead: 
[ur_ros2_control_node-1] message: SafetySetup has not been confirmed yet, therefore no data will be send yet
[ur_ros2_control_node-1] source: 
[ur_ros2_control_node-1] warning level: 128. Retrying...
[ur_ros2_control_node-1] [INFO] [1650635708.835023318] [UR_Client_Library]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1650635708.838673024] [UR_Client_Library]: Setting up RTDE communication with frequency 500.000000
[ur_ros2_control_node-1] [INFO] [1650635712.086444500] [URPositionHardwareInterface]: Calibration checksum: 'calib_209549117540498681'.
[ur_ros2_control_node-1] [WARN] [1650635712.086739446] [UR_Client_Library]: No realtime capabilities found. Consider using a realtime system for better performance
[ur_ros2_control_node-1] [ERROR] [1650635713.135234884] [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] [WARN] [1650635713.135397066] [UR_Client_Library]: No realtime capabilities found. Consider using a realtime system for better performance
[ur_ros2_control_node-1] [INFO] [1650635713.135449917] [URPositionHardwareInterface]: System successfully started!
[controller_stopper_node-3] [INFO] [1650635713.141319039] [Controller stopper]: Service available
[controller_stopper_node-3] [INFO] [1650635713.141348490] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-3] [INFO] [1650635713.141418319] [Controller stopper]: Service available
[dashboard_client-2] [INFO] [1650635715.828479092] [UR_Client_Library]: Connected: Universal Robots Dashboard Server
[dashboard_client-2] 

[destogl]

A way to “solve” this would be to make timeout for spawners longer. But on the other hand, I am not sure if it is OK that controller just start some time in the future.

As a solution, I would rather document this behavior and add information for users to start controllers manually when hardware starts to run.

What would be preferred behavior for you?

[fmauch]

We could also add a small workaround node.

Imagine we don't activate controllers at all and have a separate node that is waiting for the robot to become alive which then starts the controllers. Of course, this could be controlled by yet another flag, but I would suggest to always use that scheme unless fake_hardware is used.

[gavanderhoorn]

We could also add a small workaround node.

like an inverse "controller stopper".

Making this state-based rather than time-based would be a good idea indeed.

[destogl]

@fmauch and @gavanderhoorn this is also a very plausible solution. On the other hand, as for the controller stopper, this could be one of the additional functionalities of controller manager. What do you think?

For the controller, stopper is this obvious how to add and also needed by other projects, just wasn't implemented yet. For this “delayed starting feature”, this would need more discussions and clear design. A simple extension of spawner could solve this, and it is probably the easiest and more consistent solution, e.g., --wait-for-hardware flag. @bmagyar what do you think?

[bmagyar]

We were discussing making the controller_manager a lifecycle node. In this case, spawner could simply depend on controller_manager in the launch file so it only tries to spawn controllers once there's someone to receive them

[destogl]

We were discussing making the controller_manager a lifecycle node. In this case, spawner could simply depend on controller_manager in the launch file so it only tries to spawn controllers once there's someone to receive them

This is a possible solution, but it could fail in multi-robot systems. For example, CM is running with a hardware and UR hardware is started later, i.e., becomes available later. So this is rather that one would need sync with the hardware lifecycle and not only CM