Port robot_state_helper to ROS2 (backport #933)

ur_dashboard_msgs/action/SetMode.action

@@ -1,7 +1,10 @@
 # This action is for setting the robot into a desired mode (e.g. RUNNING) and safety mode into a
 # non-critical state (e.g. NORMAL or REDUCED), for example after a safety incident happened.
 
-# goal
+# Target modes can be one of
+# - 3: ROBOT_MODE_POWER_OFF
+# - 5: ROBOT_MODE_IDLE
+# - 7: ROBOT_MODE_RUNNING
 int8 target_robot_mode
 
 # Stop program execution before restoring the target mode. Can be used together with 'play_program'.

ur_robot_driver/CMakeLists.txt

@@ -58,6 +58,7 @@ add_library(ur_robot_driver_plugin
   src/dashboard_client_ros.cpp
   src/hardware_interface.cpp
   src/urcl_log_handler.cpp
+  src/robot_state_helper.cpp
 )
 target_link_libraries(
   ur_robot_driver_plugin
@@ -83,7 +84,7 @@ add_executable(dashboard_client
   src/dashboard_client_node.cpp
   src/urcl_log_handler.cpp
 )
-target_link_libraries(dashboard_client ${catkin_LIBRARIES} ur_client_library::urcl)
+target_link_libraries(dashboard_client ur_client_library::urcl)
 ament_target_dependencies(dashboard_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 #
@@ -106,13 +107,23 @@ add_executable(controller_stopper_node
 )
 ament_target_dependencies(controller_stopper_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+#
+# robot_state_helper
+#
+add_executable(robot_state_helper
+  src/robot_state_helper.cpp
+  src/robot_state_helper_node.cpp
+)
+target_link_libraries(robot_state_helper ur_client_library::urcl)
+ament_target_dependencies(robot_state_helper ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 add_executable(urscript_interface
   src/urscript_interface.cpp
 )
 ament_target_dependencies(urscript_interface ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 install(
-  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface
+  TARGETS dashboard_client ur_ros2_control_node controller_stopper_node urscript_interface robot_state_helper
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/doc/controller_stopper.rst

@@ -0,0 +1,29 @@
+.. _controller_stopper:
+
+Controller stopper
+==================
+
+As explained in the section :ref:`robot_startup_program`, the robot needs to run a program in order
+to receive motion commands from the ROS driver. When the program is not running, commands sent to
+the robot will have no effect.
+
+To make that transparent, the ``controller_stopper`` node mirrors that state in the ROS
+controller's state. It listens to the ``/io_and_status_controller/robot_program_running`` topic and
+deactivates all motion controllers  (or any controller not explicitly marked as "consistent", see
+below)when the program is not running.
+
+Once the program is running again, any previously active motion controller will be activated again.
+
+This way, when sending commands to an inactive controller the caller should be transparently
+informed, that the controller cannot accept commands at the moment.
+
+In the same way, any running action on the ROS controller will be aborted, as the controller gets
+deactivated by the controller_stopper.
+
+Parameters
+----------
+
+- ``~consistent_controllers`` (list of strings, default: ``[]``)
+
+  A list of controller names that should not be stopped when the program is not running. Any
+  controller that doesn't require the robot program to be running should be in that list.

ur_robot_driver/doc/index.rst

@@ -16,7 +16,8 @@ Welcome to ur_robot_driver's documentation!
    setup_tool_communication
    ROS_INTERFACE
    generated/index
-
+   robot_state_helper
+   controller_stopper
 
 
 Indices and tables

ur_robot_driver/doc/robot_state_helper.rst

@@ -0,0 +1,59 @@
+.. _robot_state_helper:
+
+Robot state helper
+==================
+After switching on the robot, it has to be manually started, the brakes have to be released and a
+program has to be started in order to make the robot ready to use. This is usually done using the
+robot's teach pendant.
+
+Whenever the robot encounters an error, manual intervention is required to resolve the issue. For
+example, if the robot goes into a protective stop, the error has to be acknowledged and the robot
+program has to be unpaused.
+
+When the robot is in :ref:`remote_control_mode <operation_modes>`, most interaction with the robot can be done
+without using the teach pendant, many of that through the :ref:`dashboard client
+<dashboard_client_ros2>`.
+
+The ROS driver provides a helper node that can be used to automate some of these tasks. The
+``robot_state_helper`` node can be used to start the robot, release the brakes, and (re-)start the
+program through an action call. It is started by default and provides a
+`dashboard_msgs/action/SetMode
+<https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_dashboard_msgs/action/SetMode.action>`_ action.
+
+For example, to make the robot ready to be used by the ROS driver, call
+
+.. code-block:: console
+
+   $ ros2 action send_goal /ur_robot_state_helper/set_mode ur_dashboard_msgs/action/SetMode "{ target_robot_mode: 7, stop_program: true, play_program: true}"
+
+The ``target_robot_mode`` can be one of the following:
+
+.. table:: target_robot_mode
+   :widths: auto
+
+   =====  =====
+   index  meaning
+   =====  =====
+   3      POWER_OFF -- Robot is powered off
+   5      IDLE -- Robot is powered on, but brakes are engaged
+   7      RUNNING -- Robot is powered on, brakes are released, ready to run a program
+   =====  =====
+
+.. note::
+
+   When the ROBOT_STATE is in ``RUNNING``, that is equivalent to the robot showing the green dot in
+   the lower left corner of the teach pendant (On PolyScope 5). The program state is independent of
+   that and shows with the text next to that button.
+
+The ``stop_program`` flag is used to stop the currently running program before changing the robot
+state. In combination with the :ref:`controller_stopper`, this will deactivate any motion
+controller and therefore stop any ROS action being active on those controllers.
+
+.. warning::
+   A robot's protective stop or emergency stop is only pausing the running program. If the program
+   is resumed after the P-Stop or EM-Stop is released, the robot will continue executing what it
+   has been doing. Therefore, it is advised to stop and re-start the program when recovering from a
+   fault.
+
+The ``play_program`` flag is used to start the program after the robot state has been set. This has
+the same effects as explained in :ref:`continuation_after_interruptions`.

ur_robot_driver/doc/usage/startup.rst

@@ -0,0 +1,142 @@
+.. _ur_robot_driver_startup:
+
+Startup the driver
+==================
+
+Prepare the robot
+-----------------
+
+If you want to use a real robot, or a URSim simulator, with this driver, you need to prepare it,
+first. Make sure that you complete all steps from the :ref:`setup instructions<robot_setup>`,
+installed the External Control URCap and created a program as explained
+:ref:`here<install_urcap>`.
+
+Launch files
+------------
+
+For starting the driver it is recommended to start the ``ur_control.launch.py`` launchfile from the
+``ur_robot_driver`` package. It starts the driver, a set of controllers and a couple of helper
+nodes for UR robots. The only required arguments are the ``ur_type`` and ``robot_ip`` parameters.
+
+.. code-block:: console
+
+   $ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5e robot_ip:=192.168.56.101
+
+Allowed ``ur_type`` strings: ``ur3``, ``ur3e``, ``ur5``, ``ur5e``, ``ur10``, ``ur10e``, ``ur16e``,
+``ur20``, ``ur30``.
+
+Other important arguments are:
+
+
+* ``kinematics_params_file`` (default: *None*) - Path to the calibration file extracted from the robot, as described in :ref:`calibration_extraction`.
+* ``use_mock_hardware`` (default: *false* ) - Use simple hardware emulator from ros2_control. Useful for testing launch files, descriptions, etc.
+* ``headless_mode`` (default: *false*) - Start driver in :ref:`headless_mode`.
+* ``launch_rviz`` (default: *true*) - Start RViz together with the driver.
+* ``initial_joint_controller`` (default: *scaled_joint_trajectory_controller*) - Use this if you
+  want to start the robot with another controller.
+
+  .. note::
+     When the driver is started, you can list all loaded controllers using the ``ros2 control
+     list_controllers`` command. For this, the package ``ros2controlcli`` must be installed (``sudo
+     apt-get install ros-${ROS_DISTRO}-ros2controlcli``).
+
+
+For all other arguments, please see
+
+
+.. code-block:: console
+
+   $ ros2 launch ur_robot_driver ur_control.launch.py --show-args
+
+Also, there are predefined launch files for all supported types of UR robots.
+
+.. _robot_startup_program:
+
+Finish startup on the robot
+---------------------------
+
+Unless :ref:`headless_mode` is used, you will now have to start the *External Control URCap* program on
+the robot that you have created earlier.
+
+Depending on the :ref:`robot control mode<operation_modes>` do the following:
+
+* In *local control mode*, load the program on the robot and press the "Play" button |play_button| on the teach pendant.
+* In *remote control mode* load and start the program using the following dashboard calls:
+
+  .. code-block:: console
+
+     $ ros2 service call /dashboard_client/load_program ur_dashboard_msgs/srv/Load "filename: my_robot_program.urp"``
+     $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {}
+
+* When the driver is started with ``headless_mode:=true`` nothing is needed. The driver is running
+  already.
+
+
+.. _verify_calibration:
+
+Verify calibration info is being used correctly
+-----------------------------------------------
+
+
+If you passed a path to an extracted calibration via the *kinematics_params_file*
+parameter, ensure that the loaded calibration matches that of the robot by inspecting the console
+output after launching the ``ur_robot_driver``. If the calibration does not match, you will see an error:
+
+.. code-block::
+
+  [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx'
+  [ERROR] [1694437690.516957265] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file.
+
+With the correct calibration you should see:
+
+.. code-block::
+
+  [INFO] [1694437690.406932381] [URPositionHardwareInterface]: Calibration checksum: 'calib_xxxxxxxxxxxxxxxxxxx'
+  [INFO] [1694437690.516957265] [URPositionHardwareInterface]: Calibration checked successfully.
+
+Alternatively, search for the term *checksum* in the console output after launching the driver.
+Verify that the printed checksum matches that on the final line of your extracted calibration file.
+
+
+.. _continuation_after_interruptions:
+
+Continuation after interruptions
+--------------------------------
+
+
+Whenever the *External Control URCap* program gets interrupted, it has to be unpaused / restarted.
+
+If that happens, you will see the output ``Connection to reverse interface dropped.``
+
+This can happen, e,g, when
+
+* The running program is actively stopped.
+* The robot goes into a protective stop / EM stop. (The program will be paused, then)
+* The communication is stopped, since the external source did not receive a command in time.
+* There was another script sent for execution e.g.
+
+  * Script code was sent to the robot via its primary interface
+  * Robot motion is performed using the Teach pendant
+
+Depending on the operation mode, perform one of the following steps:
+
+* In *local control mode*, simply press the "Play" button |play_button| on the teach pendant.
+* In *remote control mode* start the program using the following dashboard call:
+
+  .. code-block:: console
+
+     $ ros2 service call /dashboard_client/play std_srvs/srv/Trigger {}
+
+* When the driver is started with ``headless_mode:=true`` perform the following service call:
+
+  .. code-block:: console
+
+     $ ros2 service call /io_and_status_controller/resend_robot_program std_srvs/srv/Trigger {}
+
+
+
+
+
+.. |play_button| image:: ../resources/play_button.svg
+                 :height: 20px
+                 :width: 20px