Port robot_state_helper to ROS2 (#933)

---------

Co-authored-by: Felix Durchdewald <[email protected]>
Co-authored-by: Felix Exner <[email protected]>

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})
 
 #
@@ -95,13 +96,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 controller_stopper_node urscript_interface
+  TARGETS dashboard_client 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

@@ -17,3 +17,5 @@ ur_robot_driver
    setup_tool_communication
    hardware_interface_parameters
    dashboard_client
+   robot_state_helper
+   controller_stopper

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

@@ -71,12 +71,12 @@ Depending on the :ref:`robot control mode<operation_modes>` do the following:
 * When the driver is started with ``headless_mode:=true`` nothing is needed. The driver is running
   already.
 
-.. _continuation_after_interruptions:
+
+.. _verify_calibration:
 
 Verify calibration info is being used correctly
 -----------------------------------------------
 
-.. _verify_calibration:
 
 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
@@ -98,9 +98,12 @@ Alternatively, search for the term *checksum* in the console output after launch
 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.``

ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp

@@ -0,0 +1,114 @@
+// Copyright 2024, FZI Forschungszentrum Informatik, Created on behalf of Universal Robots A/S
+//
+// 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.
+
+#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_
+
+#include <chrono>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/create_server.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_srvs/srv/trigger.hpp"
+
+#include "ur_dashboard_msgs/action/set_mode.hpp"
+#include "ur_dashboard_msgs/msg/safety_mode.hpp"
+#include "ur_dashboard_msgs/msg/robot_mode.hpp"
+#include "ur_client_library/ur/datatypes.h"
+
+namespace ur_robot_driver
+{
+class RobotStateHelper
+{
+public:
+  using SetModeGoalHandle = rclcpp_action::ServerGoalHandle<ur_dashboard_msgs::action::SetMode>;
+
+  explicit RobotStateHelper(const rclcpp::Node::SharedPtr& node);
+  RobotStateHelper() = delete;
+  virtual ~RobotStateHelper() = default;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+
+  void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg);
+  void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg);
+
+  void updateRobotState();
+
+  bool recoverFromSafety();
+  bool doTransition(const urcl::RobotMode target_mode);
+  bool jumpToRobotMode(const urcl::RobotMode target_mode);
+
+  bool safeDashboardTrigger(rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr srv);
+
+  bool stopProgram();
+
+  void setModeAcceptCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+  rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid,
+                                                  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal);
+  rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  void setModeExecute(const std::shared_ptr<SetModeGoalHandle> goal_handle);
+
+  bool headless_mode_;
+
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Result> result_;
+  std::shared_ptr<ur_dashboard_msgs::action::SetMode::Feedback> feedback_;
+  std::shared_ptr<const ur_dashboard_msgs::action::SetMode::Goal> goal_;
+  std::shared_ptr<SetModeGoalHandle> current_goal_handle_;
+
+  std::atomic<urcl::RobotMode> robot_mode_;
+  std::atomic<urcl::SafetyMode> safety_mode_;
+  std::atomic<bool> error_ = false;
+  std::atomic<bool> in_action_;
+  std::atomic<bool> program_running_;
+  std::mutex goal_mutex_;
+
+  rclcpp_action::Server<ur_dashboard_msgs::action::SetMode>::SharedPtr set_mode_as_;
+
+  rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_;
+
+  rclcpp::Subscription<ur_dashboard_msgs::msg::RobotMode>::SharedPtr robot_mode_sub_;
+  rclcpp::Subscription<ur_dashboard_msgs::msg::SafetyMode>::SharedPtr safety_mode_sub_;
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr program_running_sub;
+
+  rclcpp::CallbackGroup::SharedPtr service_cb_grp_;
+
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr unlock_protective_stop_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr restart_safety_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_on_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr power_off_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr brake_release_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr stop_program_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr play_program_srv_;
+  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr resend_robot_program_srv_;
+};
+}  // namespace ur_robot_driver
+
+#endif  // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_

ur_robot_driver/launch/ur_control.launch.py

@@ -91,6 +91,16 @@ def launch_setup(context):
         parameters=[{"robot_ip": robot_ip}],
     )
 
+    robot_state_helper_node = Node(
+        package="ur_robot_driver",
+        executable="robot_state_helper",
+        name="ur_robot_state_helper",
+        output="screen",
+        parameters=[
+            {"headless_mode": headless_mode},
+        ],
+    )
+
     tool_communication_node = Node(
         package="ur_robot_driver",
         condition=IfCondition(use_tool_communication),
@@ -202,6 +212,7 @@ def controller_spawner(controllers, active=True):
     nodes_to_start = [
         control_node,
         dashboard_client_node,
+        robot_state_helper_node,
         tool_communication_node,
         controller_stopper_node,
         urscript_interface,