diff --git a/ur_dashboard_msgs/action/SetMode.action b/ur_dashboard_msgs/action/SetMode.action index d110997f1..c0ee7e27b 100644 --- a/ur_dashboard_msgs/action/SetMode.action +++ b/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'. diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 68c082aa1..0a6f1b747 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/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} ) diff --git a/ur_robot_driver/doc/controller_stopper.rst b/ur_robot_driver/doc/controller_stopper.rst new file mode 100644 index 000000000..d02d6b012 --- /dev/null +++ b/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. diff --git a/ur_robot_driver/doc/index.rst b/ur_robot_driver/doc/index.rst index bf536ad81..d2889276a 100644 --- a/ur_robot_driver/doc/index.rst +++ b/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 diff --git a/ur_robot_driver/doc/robot_state_helper.rst b/ur_robot_driver/doc/robot_state_helper.rst new file mode 100644 index 000000000..92564fe32 --- /dev/null +++ b/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 `, most interaction with the robot can be done +without using the teach pendant, many of that through the :ref:`dashboard client +`. + +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 +`_ 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`. diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst new file mode 100644 index 000000000..6b0644563 --- /dev/null +++ b/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`, +installed the External Control URCap and created a program as explained +:ref:`here`. + +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` 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 diff --git a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp new file mode 100644 index 000000000..d3c03189b --- /dev/null +++ b/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 +#include + +#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; + + 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::SharedPtr srv); + + bool stopProgram(); + + void setModeAcceptCallback(const std::shared_ptr goal_handle); + rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr goal_handle); + + void setModeExecute(const std::shared_ptr goal_handle); + + bool headless_mode_; + + std::shared_ptr result_; + std::shared_ptr feedback_; + std::shared_ptr goal_; + std::shared_ptr current_goal_handle_; + + std::atomic robot_mode_; + std::atomic safety_mode_; + std::atomic error_ = false; + std::atomic in_action_; + std::atomic program_running_; + std::mutex goal_mutex_; + + rclcpp_action::Server::SharedPtr set_mode_as_; + + rclcpp::CallbackGroup::SharedPtr robot_mode_sub_cb_; + + rclcpp::Subscription::SharedPtr robot_mode_sub_; + rclcpp::Subscription::SharedPtr safety_mode_sub_; + rclcpp::Subscription::SharedPtr program_running_sub; + + rclcpp::CallbackGroup::SharedPtr service_cb_grp_; + + rclcpp::Client::SharedPtr unlock_protective_stop_srv_; + rclcpp::Client::SharedPtr restart_safety_srv_; + rclcpp::Client::SharedPtr power_on_srv_; + rclcpp::Client::SharedPtr power_off_srv_; + rclcpp::Client::SharedPtr brake_release_srv_; + rclcpp::Client::SharedPtr stop_program_srv_; + rclcpp::Client::SharedPtr play_program_srv_; + rclcpp::Client::SharedPtr resend_robot_program_srv_; +}; +} // namespace ur_robot_driver + +#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 72de393ab..4c47b07b3 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -257,6 +257,16 @@ def launch_setup(context, *args, **kwargs): 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), @@ -367,6 +377,7 @@ def controller_spawner(controllers, active=True): control_node, ur_control_node, dashboard_client_node, + robot_state_helper_node, tool_communication_node, controller_stopper_node, urscript_interface, diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp new file mode 100644 index 000000000..0f35df6d7 --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -0,0 +1,397 @@ +// 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. + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/create_server.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 +{ +RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) + : node_(node) + , robot_mode_(urcl::RobotMode::UNKNOWN) + , safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE) + , in_action_(false) +{ + robot_mode_sub_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = robot_mode_sub_cb_; + // Topic on which the robot_mode is published by the driver + robot_mode_sub_ = node_->create_subscription( + "io_and_status_controller/robot_mode", rclcpp::SensorDataQoS(), + std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1), options); + // Topic on which the safety is published by the driver + safety_mode_sub_ = node_->create_subscription( + "io_and_status_controller/safety_mode", 1, + std::bind(&RobotStateHelper::safetyModeCallback, this, std::placeholders::_1)); + program_running_sub = node_->create_subscription( + "io_and_status_controller/robot_program_running", 1, + [this](std_msgs::msg::Bool::UniquePtr msg) -> void { program_running_ = msg->data; }); + + service_cb_grp_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + node->declare_parameter("headless_mode", false); + headless_mode_ = node->get_parameter("headless_mode").as_bool(); + + // Service to unlock protective stop + unlock_protective_stop_srv_ = node_->create_client( + "dashboard_client/unlock_protective_stop", rmw_qos_profile_services_default, service_cb_grp_); + // Service to restart safety + restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to power on the robot + power_on_srv_ = node_->create_client("dashboard_client/power_on", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to power off the robot + power_off_srv_ = node_->create_client("dashboard_client/power_off", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to release the robot's brakes + brake_release_srv_ = node_->create_client("dashboard_client/brake_release", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to stop UR program execution on the robot + stop_program_srv_ = node_->create_client("dashboard_client/stop", + rmw_qos_profile_services_default, service_cb_grp_); + // Service to start UR program execution on the robot + play_program_srv_ = node_->create_client("dashboard_client/play", + rmw_qos_profile_services_default, service_cb_grp_); + play_program_srv_->wait_for_service(); + + resend_robot_program_srv_ = node_->create_client( + "io_and_status_controller/resend_robot_program", rmw_qos_profile_services_default, service_cb_grp_); + resend_robot_program_srv_->wait_for_service(); + + feedback_ = std::make_shared(); + result_ = std::make_shared(); + set_mode_as_ = rclcpp_action::create_server( + node_, "~/set_mode", + std::bind(&RobotStateHelper::setModeGoalCallback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&RobotStateHelper::setModeCancelCallback, this, std::placeholders::_1), + std::bind(&RobotStateHelper::setModeAcceptCallback, this, std::placeholders::_1)); +} + +void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg) +{ + if (robot_mode_ != static_cast(msg->mode)) { + robot_mode_ = urcl::RobotMode(msg->mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot is currently in mode " << robotModeString(robot_mode_) << "."); + if (in_action_) { + std::scoped_lock lock(goal_mutex_); + feedback_->current_robot_mode = + static_cast(robot_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); + } + } +} + +void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg) +{ + if (safety_mode_ != static_cast(msg->mode)) { + safety_mode_ = urcl::SafetyMode(msg->mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot is currently in safety mode " << safetyModeString(safety_mode_) << "."); + if (in_action_) { + std::scoped_lock lock(goal_mutex_); + feedback_->current_safety_mode = + static_cast(safety_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); + } + } +} + +bool RobotStateHelper::recoverFromSafety() +{ + switch (safety_mode_) { + case urcl::SafetyMode::PROTECTIVE_STOP: + return safeDashboardTrigger(this->unlock_protective_stop_srv_); + case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:; + case urcl::SafetyMode::ROBOT_EMERGENCY_STOP: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in safety mode." + << safetyModeString(safety_mode_) + << ". Please release the EM-Stop to proceed."); + return false; + case urcl::SafetyMode::VIOLATION:; + case urcl::SafetyMode::FAULT: + return safeDashboardTrigger(this->restart_safety_srv_); + default: + // nothing to do + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("robot_state_helper"), "No safety recovery needed."); + } + return true; +} + +bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode) +{ + switch (target_mode) { + case urcl::RobotMode::POWER_OFF: + return safeDashboardTrigger(this->power_off_srv_); + case urcl::RobotMode::IDLE: + return safeDashboardTrigger(this->power_on_srv_); + case urcl::RobotMode::RUNNING: + return safeDashboardTrigger(this->brake_release_srv_); + default: + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Unreachable target robot mode."); + } + return false; +} + +bool RobotStateHelper::doTransition(const urcl::RobotMode target_mode) +{ + if (!recoverFromSafety()) { + return false; + } + switch (robot_mode_) { + case urcl::RobotMode::CONFIRM_SAFETY: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". It is required to interact with " + "the " + "teach pendant at this point."); + break; + case urcl::RobotMode::BOOTING: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". Please wait until the robot is " + "booted up..."); + break; + case urcl::RobotMode::POWER_OFF: + return jumpToRobotMode(target_mode); + case urcl::RobotMode::POWER_ON: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". Please wait until the robot is in " + "mode " + << robotModeString(urcl::RobotMode::IDLE)); + break; + case urcl::RobotMode::IDLE: + return jumpToRobotMode(target_mode); + break; + case urcl::RobotMode::BACKDRIVE: + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". It will automatically return to " + "mode " + << robotModeString(urcl::RobotMode::IDLE) + << " once the teach button is " + "released."); + break; + case urcl::RobotMode::RUNNING: + if (target_mode == urcl::RobotMode::IDLE) { + // We cannot engage the brakes directly. + if (!jumpToRobotMode(urcl::RobotMode::POWER_OFF)) { + return false; + } + } + return jumpToRobotMode(target_mode); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "The robot has reached operational mode " << robotModeString(robot_mode_)); + break; + default: + RCLCPP_WARN_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " + << robotModeString(robot_mode_) + << ". This won't be handled by this " + "helper. Please resolve this " + "manually."); + } + return false; +} + +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) +{ + assert(srv != nullptr); + auto request = std::make_shared(); + auto future = srv->async_send_request(request); + future.wait(); + auto result = future.get(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << result->message); + return result->success; +} + +void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr goal_handle) +{ + std::thread{ std::bind(&RobotStateHelper::setModeExecute, this, std::placeholders::_1), goal_handle }.detach(); +} + +bool RobotStateHelper::stopProgram() +{ + if (safeDashboardTrigger(this->stop_program_srv_)) { + auto start = std::chrono::steady_clock::now(); + while (program_running_ && std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + return true; + } + } + return false; +} + +void RobotStateHelper::setModeExecute(const std::shared_ptr goal_handle) +{ + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_ = goal_handle; + } + in_action_ = true; + const auto goal = goal_handle->get_goal(); + this->goal_ = goal; + urcl::RobotMode target_mode; + try { + target_mode = static_cast(goal->target_robot_mode); + switch (target_mode) { + case urcl::RobotMode::POWER_OFF: + case urcl::RobotMode::IDLE: + case urcl::RobotMode::RUNNING: + if (goal_->stop_program && program_running_) { + if (!stopProgram()) { + result_->message = "Stopping the program failed."; + result_->success = false; + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + return; + } + } + if (robot_mode_ != target_mode || safety_mode_ > urcl::SafetyMode::REDUCED) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "Target mode was set to " << robotModeString(target_mode) << "."); + if (!doTransition(target_mode)) { + result_->message = "Transition to target mode failed."; + result_->success = false; + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + return; + } + } + break; + case urcl::RobotMode::NO_CONTROLLER: + case urcl::RobotMode::DISCONNECTED: + case urcl::RobotMode::CONFIRM_SAFETY: + case urcl::RobotMode::BOOTING: + case urcl::RobotMode::POWER_ON: + case urcl::RobotMode::BACKDRIVE: + case urcl::RobotMode::UPDATING_FIRMWARE: + result_->message = + "Requested target mode " + robotModeString(target_mode) + " which cannot be explicitly selected."; + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + break; + default: + result_->message = "Requested illegal mode."; + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + break; + } + } catch (const std::invalid_argument& e) { + result_->message = e.what(); + result_->success = false; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; + } + + // Wait until the robot reached the target mode or something went wrong + while (robot_mode_ != target_mode && !error_) { + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Waiting for robot to reach target mode... Current_mode: %s", + robotModeString(robot_mode_).c_str()); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + if (robot_mode_ == target_mode) { + result_->success = true; + result_->message = "Reached target robot mode."; + if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program && !program_running_) { + if (headless_mode_) { + result_->success = safeDashboardTrigger(this->resend_robot_program_srv_); + } else { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + result_->success = safeDashboardTrigger(this->play_program_srv_); + } + } + if (result_->success) { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->succeed(result_); + } else { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + } else { + result_->success = false; + result_->message = "Robot reached higher mode than requested during recovery. This either means that something " + "went wrong or that a higher mode was requested from somewhere else (e.g. the teach " + "pendant.)"; + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + } +} + +rclcpp_action::GoalResponse RobotStateHelper::setModeGoalCallback( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + (void)uuid; + if (robot_mode_ == urcl::RobotMode::UNKNOWN) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Robot mode is unknown. Cannot accept goal, yet. Is " + "the robot switched on and connected to the driver?"); + return rclcpp_action::GoalResponse::REJECT; + } + + if (safety_mode_ == urcl::SafetyMode::UNDEFINED_SAFETY_MODE) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("robot_state_helper"), "Safety mode is unknown. Cannot accept goal, yet. Is " + "the robot switched on and connected to the driver?"); + return rclcpp_action::GoalResponse::REJECT; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse +RobotStateHelper::setModeCancelCallback(const std::shared_ptr goal_handle) +{ + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "Received request to cancel goal"); + (void)goal_handle; + return rclcpp_action::CancelResponse::REJECT; +} + +} // namespace ur_robot_driver diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp new file mode 100755 index 000000000..37bf9a902 --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -0,0 +1,42 @@ +// 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. + +#include "ur_robot_driver/robot_state_helper.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper"); + ur_robot_driver::RobotStateHelper state_helper(node); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + return 0; +}