sendRobotProgram() doesn't always work on the first try

[RobertWilbrandt]

While working on the ROS2 driver integration tests i realized that resending the robot program in headless mode does not work completely reliably right now. The first scenario for this is:

• Start ursim, e.g. ros2 run ur_robot_driver start_ursim.sh
• Start client with headless_mode = true, e.g. ros2 launch ur_robot_driver ur5e.launch.py robot_ip:=192.168.56.101 headless_mode:=true
• Power on and start the robot
• UrDriver::sendRobotProgram() (e.g. by ros2 service call /io_and_status_controller/resend_robot_program std_srvs/srv/Trigger "{}")
• The program doesn't start, the VNC view only shows it switching to Stopped. No error is emitted.
• A second call actually starts the program

This behavior also happens if i pause the program through the teach pendant and then resend the program. While i have only tried this through the ROS2 driver, i am pretty certain that the issue is in the client library as not even an error is returned in this case.

[fmauch]

I'm able to reproduce this with URSim. Note that the two cases seem to be equivalent: When have a booted (but not yet started - in terms of brake_release) robot, the program state also seems to be paused. Therefore, another way to reproduce is:

• start robot and initialize
• start client in headless mode as mentioned above
• While the program is running, pause it. (e.g. by calling ros2 service call /dashboard_client/pause std_srvs/srv/Trigger)
• UrDriver::sendRobotProgram() (e.g. by ros2 service call /io_and_status_controller/resend_robot_program std_srvs/srv/Trigger "{}")

That this behavior is the same when having a freshly booted robot could be a URSim issue (e.g. you'll get joint states for URSim before initializing it, which you won't with a real robot), but resending while being in paused state definitely seems like something that would be present on a real robot, as well. However, since local and remote mode are rather different in URSim this could also only be a URSim problem after all. I'll check in my next lab session.

Edit: Same behavior on a CB3 URSim btw.