From 7f10eced90fe57c8f64c823d4a9a2a9a342f2fa9 Mon Sep 17 00:00:00 2001 From: Felix Durchdewald Date: Thu, 9 Nov 2023 16:59:10 +0100 Subject: [PATCH 01/13] Port robot_state_helper to ROS2 --- ur_robot_driver/CMakeLists.txt | 13 +- .../ur_robot_driver/robot_state_helper.hpp | 72 +++++ ur_robot_driver/launch/ur_control.launch.py | 9 + ur_robot_driver/src/robot_state_helper.cpp | 287 ++++++++++++++++++ .../src/robot_state_helper_node.cpp | 13 + 5 files changed, 393 insertions(+), 1 deletion(-) create mode 100644 ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp create mode 100755 ur_robot_driver/src/robot_state_helper.cpp create mode 100755 ur_robot_driver/src/robot_state_helper_node.cpp diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 1b05dc34a..0074db57b 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -55,6 +55,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 @@ -103,13 +104,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 ${catkin_LIBRARIES} 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/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..1d5348b94 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -0,0 +1,72 @@ +#ifndef UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ +#define UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ + +#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 +{ +class RobotStateHelper +{ +public: + using SetModeGoalHandle = rclcpp_action::ServerGoalHandle; + + 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 called_from_thread = false); + + void doTransition(bool called_from_thread = false); + + bool safeDashboardTrigger(rclcpp::Client::SharedPtr srv, bool called_from_thread = false); + + 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); + + void startActionServer(); + bool is_started_; + + std::shared_ptr result_; + std::shared_ptr feedback_; + std::shared_ptr goal_; + std::shared_ptr current_goal_handle_; + + urcl::RobotMode robot_mode_; + urcl::SafetyMode safety_mode_; + + rclcpp_action::Server::SharedPtr set_mode_as_; + + rclcpp::Subscription::SharedPtr robot_mode_sub_; + rclcpp::Subscription::SharedPtr safety_mode_sub_; + + 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_; +}; +} // namespace ur_robot_driver + +#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ \ No newline at end of file diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 4a8d51370..0be2d7909 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -236,6 +236,14 @@ 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="robot_state_helper", + output="screen", + ) + + tool_communication_node = Node( package="ur_robot_driver", condition=IfCondition(use_tool_communication), @@ -354,6 +362,7 @@ def controller_spawner(name, 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 100755 index 000000000..26f439abd --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -0,0 +1,287 @@ +#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) + , is_started_(false) + , robot_mode_(urcl::RobotMode::UNKNOWN) + , safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE) +{ + // Topic on which the robot_mode is published by the driver + robot_mode_sub_ = node_->create_subscription( + "io_and_status_controller/robot_mode", 1, + std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1)); + // 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)); + + // Service to unlock protective stop + unlock_protective_stop_srv_ = node_->create_client("dashboard_client/unlock_protective_stop"); + // Service to restart safety + restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety"); + // Service to power on the robot + power_on_srv_ = node_->create_client("dashboard_client/power_on"); + // Service to power off the robot + power_off_srv_ = node_->create_client("dashboard_client/power_off"); + // Service to release the robot's brakes + brake_release_srv_ = node_->create_client("dashboard_client/brake_release"); + // Service to stop UR program execution on the robot + stop_program_srv_ = node_->create_client("dashboard_client/stop"); + // Service to start UR program execution on the robot + play_program_srv_ = node_->create_client("dashboard_client/play"); + play_program_srv_->wait_for_service(); + + feedback_ = std::make_shared(); + result_ = std::make_shared(); +} + +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_) << "."); + updateRobotState(); + if (!is_started_) { + startActionServer(); + } + } +} + +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_) << "."); + updateRobotState(); + if (!is_started_) { + startActionServer(); + } + } +} + +void RobotStateHelper::doTransition(bool called_from_thread) +{ + if (static_cast(goal_->target_robot_mode) < robot_mode_) { + safeDashboardTrigger(this->power_off_srv_, called_from_thread); + } else { + switch (safety_mode_) { + case urcl::SafetyMode::PROTECTIVE_STOP: + safeDashboardTrigger(this->unlock_protective_stop_srv_, called_from_thread); + break; + 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."); + break; + case urcl::SafetyMode::VIOLATION:; + case urcl::SafetyMode::FAULT: + safeDashboardTrigger(this->restart_safety_srv_, called_from_thread); + break; + default: + 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: + safeDashboardTrigger(this->power_on_srv_, called_from_thread); + break; + 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: + safeDashboardTrigger(this->brake_release_srv_, called_from_thread); + 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: + 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."); + } + } + } +} + +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv, + bool called_from_thread) +{ + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "trigger"); + assert(srv != nullptr); + auto request = std::make_shared(); + auto future = srv->async_send_request(request); + if (called_from_thread) { + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "called from thread"); + future.wait(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "Service response received: " << future.get()->message); + } else { + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "not from thread"); + if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "Service response received: " << future.get()->message); + } else { + RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "Failed to call dashboard trigger service"); + return false; + } + } + return true; +} + +void RobotStateHelper::updateRobotState(bool called_from_thread) +{ + if (is_started_) { + // Update action feedback + feedback_->current_robot_mode = + static_cast(robot_mode_); + feedback_->current_safety_mode = + static_cast(safety_mode_); + current_goal_handle_->publish_feedback(feedback_); + + if (robot_mode_ < static_cast(goal_->target_robot_mode) || + safety_mode_ > urcl::SafetyMode::REDUCED) { + // Transition to next mode + RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "transition"); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_robot_state_helper"), + "Current robot mode is " + << robotModeString(robot_mode_) << " while target mode is " + << robotModeString(static_cast(goal_->target_robot_mode))); + doTransition(); + } else if (robot_mode_ == static_cast(goal_->target_robot_mode)) { + // Final mode reached + result_->success = true; + result_->message = "Reached target robot mode."; + if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program) { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + safeDashboardTrigger(this->play_program_srv_, called_from_thread); + } + current_goal_handle_->succeed(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.)"; + current_goal_handle_->abort(result_); + } + } +} + +void RobotStateHelper::startActionServer() +{ + if (robot_mode_ != urcl::RobotMode::UNKNOWN && safety_mode_ != urcl::SafetyMode::UNDEFINED_SAFETY_MODE) { + is_started_ = true; + } + 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::setModeAcceptCallback(const std::shared_ptr goal_handle) +{ + std::thread{ std::bind(&RobotStateHelper::setModeExecute, this, std::placeholders::_1), goal_handle }.detach(); +} + +void RobotStateHelper::setModeExecute(const std::shared_ptr goal_handle) +{ + current_goal_handle_ = goal_handle; + const auto goal = current_goal_handle_->get_goal(); + this->goal_ = goal; + if (goal_->target_robot_mode > 8 || goal_->target_robot_mode < -1) { + result_->message = "Requested illegal mode."; + result_->success = false; + current_goal_handle_->abort(result_); + } else { + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), + "Target mode was set to " + << robotModeString(static_cast(goal_->target_robot_mode)) << "."); + switch (static_cast(goal_->target_robot_mode)) { + case urcl::RobotMode::POWER_OFF: + case urcl::RobotMode::IDLE: + case urcl::RobotMode::RUNNING: + if (robot_mode_ != static_cast(goal_->target_robot_mode) || + safety_mode_ > urcl::SafetyMode::REDUCED) { + if (goal_->stop_program) { + safeDashboardTrigger(this->stop_program_srv_, true); + } + doTransition(true); + } else { + updateRobotState(true); + } + 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(static_cast(goal_->target_robot_mode)) + + " which cannot be explicitly selected."; + result_->success = false; + current_goal_handle_->abort(result_); + break; + default: + result_->message = "Requested illegal mode."; + result_->success = false; + current_goal_handle_->abort(result_); + break; + } + } +} + +rclcpp_action::GoalResponse RobotStateHelper::setModeGoalCallback( + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) +{ + (void)uuid; + 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 \ No newline at end of file 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..eae1d15fa --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -0,0 +1,13 @@ +#include "ur_robot_driver/robot_state_helper.hpp" +#include "rclcpp/rclcpp.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::spin(node); + + return 0; +} From 7c72f7e2b3e20a38dc59cde574378171f8dde88d Mon Sep 17 00:00:00 2001 From: Felix Durchdewald Date: Thu, 15 Feb 2024 14:13:57 +0100 Subject: [PATCH 02/13] fix format errors --- ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp | 2 +- ur_robot_driver/launch/ur_control.launch.py | 1 - ur_robot_driver/src/robot_state_helper.cpp | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) 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 index 1d5348b94..1a40f88ff 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -69,4 +69,4 @@ class RobotStateHelper }; } // namespace ur_robot_driver -#endif // UR_ROBOT_DRIVER__ROBOT_STATE_HELPER_HPP_ \ No newline at end of file +#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 0be2d7909..46ba61b9d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -243,7 +243,6 @@ def launch_setup(context, *args, **kwargs): output="screen", ) - tool_communication_node = Node( package="ur_robot_driver", condition=IfCondition(use_tool_communication), diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 26f439abd..f2b31d377 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -284,4 +284,4 @@ RobotStateHelper::setModeCancelCallback(const std::shared_ptr Date: Wed, 28 Feb 2024 16:19:51 +0100 Subject: [PATCH 03/13] use callback groups for services and topics --- .../ur_robot_driver/robot_state_helper.hpp | 17 +++- ur_robot_driver/src/robot_state_helper.cpp | 91 ++++++++++--------- .../src/robot_state_helper_node.cpp | 6 +- 3 files changed, 68 insertions(+), 46 deletions(-) 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 index 1a40f88ff..4de0e0c3e 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -29,11 +29,11 @@ class RobotStateHelper void robotModeCallback(ur_dashboard_msgs::msg::RobotMode::SharedPtr msg); void safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::SharedPtr msg); - void updateRobotState(bool called_from_thread = false); + void updateRobotState(); - void doTransition(bool called_from_thread = false); + void doTransition(); - bool safeDashboardTrigger(rclcpp::Client::SharedPtr srv, bool called_from_thread = false); + bool safeDashboardTrigger(rclcpp::Client::SharedPtr srv); void setModeAcceptCallback(const std::shared_ptr goal_handle); rclcpp_action::GoalResponse setModeGoalCallback(const rclcpp_action::GoalUUID& uuid, @@ -45,6 +45,7 @@ class RobotStateHelper void startActionServer(); bool is_started_; + bool in_action_; std::shared_ptr result_; std::shared_ptr feedback_; @@ -56,9 +57,19 @@ class RobotStateHelper 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::CallbackGroup::SharedPtr unlock_cb_; + rclcpp::CallbackGroup::SharedPtr restart_cb_; + rclcpp::CallbackGroup::SharedPtr power_on_cb_; + rclcpp::CallbackGroup::SharedPtr power_off_cb_; + rclcpp::CallbackGroup::SharedPtr brake_release_cb_; + rclcpp::CallbackGroup::SharedPtr stop_program_cb_; + rclcpp::CallbackGroup::SharedPtr play_program_cb_; + rclcpp::Client::SharedPtr unlock_protective_stop_srv_; rclcpp::Client::SharedPtr restart_safety_srv_; rclcpp::Client::SharedPtr power_on_srv_; diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index f2b31d377..d49b8322b 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -14,32 +14,51 @@ namespace ur_robot_driver RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) : node_(node) , is_started_(false) + , in_action_(false) , robot_mode_(urcl::RobotMode::UNKNOWN) , safety_mode_(urcl::SafetyMode::UNDEFINED_SAFETY_MODE) { + 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", 1, - std::bind(&RobotStateHelper::robotModeCallback, this, std::placeholders::_1)); + "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)); + unlock_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + restart_cb_ = unlock_cb_; + power_on_cb_ = unlock_cb_; + power_off_cb_ = unlock_cb_; + brake_release_cb_ = unlock_cb_; + stop_program_cb_ = unlock_cb_; + play_program_cb_ = unlock_cb_; + // Service to unlock protective stop - unlock_protective_stop_srv_ = node_->create_client("dashboard_client/unlock_protective_stop"); + unlock_protective_stop_srv_ = node_->create_client( + "dashboard_client/unlock_protective_stop", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); // Service to restart safety - restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety"); + restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety", + rclcpp::QoS(rclcpp::KeepLast(10)), restart_cb_); // Service to power on the robot - power_on_srv_ = node_->create_client("dashboard_client/power_on"); + power_on_srv_ = node_->create_client("dashboard_client/power_on", + rclcpp::QoS(rclcpp::KeepLast(10)), power_on_cb_); // Service to power off the robot - power_off_srv_ = node_->create_client("dashboard_client/power_off"); + power_off_srv_ = node_->create_client("dashboard_client/power_off", + rclcpp::QoS(rclcpp::KeepLast(10)), power_off_cb_); // Service to release the robot's brakes - brake_release_srv_ = node_->create_client("dashboard_client/brake_release"); + brake_release_srv_ = node_->create_client( + "dashboard_client/brake_release", rclcpp::QoS(rclcpp::KeepLast(10)), brake_release_cb_); // Service to stop UR program execution on the robot - stop_program_srv_ = node_->create_client("dashboard_client/stop"); + stop_program_srv_ = node_->create_client("dashboard_client/stop", + rclcpp::QoS(rclcpp::KeepLast(10)), stop_program_cb_); // Service to start UR program execution on the robot - play_program_srv_ = node_->create_client("dashboard_client/play"); + play_program_srv_ = node_->create_client("dashboard_client/play", + rclcpp::QoS(rclcpp::KeepLast(10)), play_program_cb_); play_program_srv_->wait_for_service(); feedback_ = std::make_shared(); @@ -52,9 +71,11 @@ void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::Shar robot_mode_ = urcl::RobotMode(msg->mode); RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " << robotModeString(robot_mode_) << "."); - updateRobotState(); - if (!is_started_) { - startActionServer(); + if (in_action_) { + updateRobotState(); + if (!is_started_) { + startActionServer(); + } } } } @@ -72,14 +93,14 @@ void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::Sh } } -void RobotStateHelper::doTransition(bool called_from_thread) +void RobotStateHelper::doTransition() { if (static_cast(goal_->target_robot_mode) < robot_mode_) { - safeDashboardTrigger(this->power_off_srv_, called_from_thread); + safeDashboardTrigger(this->power_off_srv_); } else { switch (safety_mode_) { case urcl::SafetyMode::PROTECTIVE_STOP: - safeDashboardTrigger(this->unlock_protective_stop_srv_, called_from_thread); + safeDashboardTrigger(this->unlock_protective_stop_srv_); break; case urcl::SafetyMode::SYSTEM_EMERGENCY_STOP:; case urcl::SafetyMode::ROBOT_EMERGENCY_STOP: @@ -89,7 +110,7 @@ void RobotStateHelper::doTransition(bool called_from_thread) break; case urcl::SafetyMode::VIOLATION:; case urcl::SafetyMode::FAULT: - safeDashboardTrigger(this->restart_safety_srv_, called_from_thread); + safeDashboardTrigger(this->restart_safety_srv_); break; default: switch (robot_mode_) { @@ -106,7 +127,7 @@ void RobotStateHelper::doTransition(bool called_from_thread) "booted up..."); break; case urcl::RobotMode::POWER_OFF: - safeDashboardTrigger(this->power_on_srv_, called_from_thread); + safeDashboardTrigger(this->power_on_srv_); break; case urcl::RobotMode::POWER_ON: RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " @@ -116,7 +137,7 @@ void RobotStateHelper::doTransition(bool called_from_thread) << robotModeString(urcl::RobotMode::IDLE)); break; case urcl::RobotMode::IDLE: - safeDashboardTrigger(this->brake_release_srv_, called_from_thread); + safeDashboardTrigger(this->brake_release_srv_); break; case urcl::RobotMode::BACKDRIVE: RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), @@ -139,32 +160,17 @@ void RobotStateHelper::doTransition(bool called_from_thread) } } -bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv, - bool called_from_thread) +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) { - RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "trigger"); assert(srv != nullptr); auto request = std::make_shared(); auto future = srv->async_send_request(request); - if (called_from_thread) { - RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "called from thread"); - future.wait(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), - "Service response received: " << future.get()->message); - } else { - RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "not from thread"); - if (rclcpp::spin_until_future_complete(node_, future) == rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), - "Service response received: " << future.get()->message); - } else { - RCLCPP_ERROR(rclcpp::get_logger("robot_state_helper"), "Failed to call dashboard trigger service"); - return false; - } - } + future.wait(); + RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << future.get()->message); return true; } -void RobotStateHelper::updateRobotState(bool called_from_thread) +void RobotStateHelper::updateRobotState() { if (is_started_) { // Update action feedback @@ -177,7 +183,6 @@ void RobotStateHelper::updateRobotState(bool called_from_thread) if (robot_mode_ < static_cast(goal_->target_robot_mode) || safety_mode_ > urcl::SafetyMode::REDUCED) { // Transition to next mode - RCLCPP_INFO(rclcpp::get_logger("robot_state_helper"), "transition"); RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_robot_state_helper"), "Current robot mode is " << robotModeString(robot_mode_) << " while target mode is " @@ -185,12 +190,13 @@ void RobotStateHelper::updateRobotState(bool called_from_thread) doTransition(); } else if (robot_mode_ == static_cast(goal_->target_robot_mode)) { // Final mode reached + in_action_ = false; result_->success = true; result_->message = "Reached target robot mode."; if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program) { // The dashboard denies playing immediately after switching the mode to RUNNING sleep(1); - safeDashboardTrigger(this->play_program_srv_, called_from_thread); + safeDashboardTrigger(this->play_program_srv_); } current_goal_handle_->succeed(result_); } else { @@ -222,6 +228,7 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptr goal_handle) { + in_action_ = true; current_goal_handle_ = goal_handle; const auto goal = current_goal_handle_->get_goal(); this->goal_ = goal; @@ -240,11 +247,11 @@ void RobotStateHelper::setModeExecute(const std::shared_ptr(goal_->target_robot_mode) || safety_mode_ > urcl::SafetyMode::REDUCED) { if (goal_->stop_program) { - safeDashboardTrigger(this->stop_program_srv_, true); + safeDashboardTrigger(this->stop_program_srv_); } - doTransition(true); + doTransition(); } else { - updateRobotState(true); + updateRobotState(); } break; case urcl::RobotMode::NO_CONTROLLER: diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp index eae1d15fa..93eec28a0 100755 --- a/ur_robot_driver/src/robot_state_helper_node.cpp +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -7,7 +7,11 @@ int main(int argc, char** argv) rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("robot_state_helper"); ur_robot_driver::RobotStateHelper state_helper(node); - rclcpp::spin(node); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + // rclcpp::spin(node); return 0; } From 5c05ccee01a453a28c320a6eec21db734a93ac1b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 10:18:35 +0100 Subject: [PATCH 04/13] Code formatting and license headers --- .../ur_robot_driver/robot_state_helper.hpp | 34 +++++++++++++++++-- ur_robot_driver/src/robot_state_helper.cpp | 28 +++++++++++++++ .../src/robot_state_helper_node.cpp | 31 +++++++++++++++-- 3 files changed, 87 insertions(+), 6 deletions(-) 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 index 4de0e0c3e..94bd07634 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -1,7 +1,36 @@ +// 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" @@ -19,7 +48,7 @@ class RobotStateHelper public: using SetModeGoalHandle = rclcpp_action::ServerGoalHandle; - RobotStateHelper(const rclcpp::Node::SharedPtr& node); + explicit RobotStateHelper(const rclcpp::Node::SharedPtr& node); RobotStateHelper() = delete; virtual ~RobotStateHelper() = default; @@ -38,8 +67,7 @@ class RobotStateHelper 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); + rclcpp_action::CancelResponse setModeCancelCallback(const std::shared_ptr goal_handle); void setModeExecute(const std::shared_ptr goal_handle); diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index d49b8322b..e3786be18 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -1,3 +1,31 @@ +// 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 "rclcpp/rclcpp.hpp" diff --git a/ur_robot_driver/src/robot_state_helper_node.cpp b/ur_robot_driver/src/robot_state_helper_node.cpp index 93eec28a0..37bf9a902 100755 --- a/ur_robot_driver/src/robot_state_helper_node.cpp +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -1,5 +1,32 @@ +// 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" -#include "rclcpp/rclcpp.hpp" int main(int argc, char** argv) { @@ -11,7 +38,5 @@ int main(int argc, char** argv) executor.add_node(node); executor.spin(); - // rclcpp::spin(node); - return 0; } From 6f6820b439b7a2f365574c89f5fcc0a78bd12b73 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 12:08:38 +0100 Subject: [PATCH 05/13] Move action to node's private namespace --- ur_robot_driver/src/robot_state_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index e3786be18..ff93ff638 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -243,7 +243,7 @@ void RobotStateHelper::startActionServer() is_started_ = true; } set_mode_as_ = rclcpp_action::create_server( - node_, "set_mode", + 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)); From b88e763394919ead4517f2a2940c5ac5f27b671b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 12:09:43 +0100 Subject: [PATCH 06/13] Rename node --- ur_robot_driver/launch/ur_control.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 08b23fb4f..c19e30e61 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -94,7 +94,7 @@ def launch_setup(context): robot_state_helper_node = Node( package="ur_robot_driver", executable="robot_state_helper", - name="robot_state_helper", + name="ur_robot_state_helper", output="screen", ) From 257488da8b779337bf14c662480984254518b35b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 12:18:55 +0100 Subject: [PATCH 07/13] Add support for headless mode --- .../ur_robot_driver/robot_state_helper.hpp | 3 +++ ur_robot_driver/launch/ur_control.launch.py | 3 +++ ur_robot_driver/src/robot_state_helper.cpp | 17 ++++++++++++++--- 3 files changed, 20 insertions(+), 3 deletions(-) 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 index 94bd07634..8efe55bd7 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -75,6 +75,8 @@ class RobotStateHelper bool is_started_; bool in_action_; + bool headless_mode_; + std::shared_ptr result_; std::shared_ptr feedback_; std::shared_ptr goal_; @@ -105,6 +107,7 @@ class RobotStateHelper 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 diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c19e30e61..4c317808b 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -96,6 +96,9 @@ def launch_setup(context): executable="robot_state_helper", name="ur_robot_state_helper", output="screen", + parameters=[ + {"headless_mode": headless_mode}, + ], ) tool_communication_node = Node( diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index ff93ff638..b12d72cd1 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -66,6 +66,9 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) stop_program_cb_ = unlock_cb_; play_program_cb_ = unlock_cb_; + 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", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); @@ -89,6 +92,10 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) rclcpp::QoS(rclcpp::KeepLast(10)), play_program_cb_); play_program_srv_->wait_for_service(); + resend_robot_program_srv_ = node_->create_client( + "io_and_status_controller/resend_robot_program", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); + resend_robot_program_srv_->wait_for_service(); + feedback_ = std::make_shared(); result_ = std::make_shared(); } @@ -222,9 +229,13 @@ void RobotStateHelper::updateRobotState() result_->success = true; result_->message = "Reached target robot mode."; if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program) { - // The dashboard denies playing immediately after switching the mode to RUNNING - sleep(1); - safeDashboardTrigger(this->play_program_srv_); + if (headless_mode_) { + safeDashboardTrigger(this->resend_robot_program_srv_); + } else { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + safeDashboardTrigger(this->play_program_srv_); + } } current_goal_handle_->succeed(result_); } else { From bb6aa1cb365832fb8d8badf9881fd9e54c16eeeb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 12:24:14 +0100 Subject: [PATCH 08/13] Allow stopping also when target mode == mode == RUNNING --- ur_robot_driver/src/robot_state_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index b12d72cd1..68eff5698 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -283,11 +283,11 @@ void RobotStateHelper::setModeExecute(const std::shared_ptrstop_program) { + safeDashboardTrigger(this->stop_program_srv_); + } if (robot_mode_ != static_cast(goal_->target_robot_mode) || safety_mode_ > urcl::SafetyMode::REDUCED) { - if (goal_->stop_program) { - safeDashboardTrigger(this->stop_program_srv_); - } doTransition(); } else { updateRobotState(); From 769f6b55fd13ee0654bdbf026b422bd12ff82bab Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 6 Feb 2025 12:24:32 +0100 Subject: [PATCH 09/13] Use only one service callback group --- .../ur_robot_driver/robot_state_helper.hpp | 8 +----- ur_robot_driver/src/robot_state_helper.cpp | 28 ++++++++----------- 2 files changed, 12 insertions(+), 24 deletions(-) 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 index 8efe55bd7..21385981c 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -92,13 +92,7 @@ class RobotStateHelper rclcpp::Subscription::SharedPtr robot_mode_sub_; rclcpp::Subscription::SharedPtr safety_mode_sub_; - rclcpp::CallbackGroup::SharedPtr unlock_cb_; - rclcpp::CallbackGroup::SharedPtr restart_cb_; - rclcpp::CallbackGroup::SharedPtr power_on_cb_; - rclcpp::CallbackGroup::SharedPtr power_off_cb_; - rclcpp::CallbackGroup::SharedPtr brake_release_cb_; - rclcpp::CallbackGroup::SharedPtr stop_program_cb_; - rclcpp::CallbackGroup::SharedPtr play_program_cb_; + rclcpp::CallbackGroup::SharedPtr service_cb_grp_; rclcpp::Client::SharedPtr unlock_protective_stop_srv_; rclcpp::Client::SharedPtr restart_safety_srv_; diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 68eff5698..2a41610eb 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -58,42 +58,36 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) "io_and_status_controller/safety_mode", 1, std::bind(&RobotStateHelper::safetyModeCallback, this, std::placeholders::_1)); - unlock_cb_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - restart_cb_ = unlock_cb_; - power_on_cb_ = unlock_cb_; - power_off_cb_ = unlock_cb_; - brake_release_cb_ = unlock_cb_; - stop_program_cb_ = unlock_cb_; - play_program_cb_ = unlock_cb_; + 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", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); + "dashboard_client/unlock_protective_stop", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to restart safety - restart_safety_srv_ = node_->create_client("dashboard_client/restart_safety", - rclcpp::QoS(rclcpp::KeepLast(10)), restart_cb_); + restart_safety_srv_ = node_->create_client( + "dashboard_client/restart_safety", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to power on the robot power_on_srv_ = node_->create_client("dashboard_client/power_on", - rclcpp::QoS(rclcpp::KeepLast(10)), power_on_cb_); + rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to power off the robot power_off_srv_ = node_->create_client("dashboard_client/power_off", - rclcpp::QoS(rclcpp::KeepLast(10)), power_off_cb_); + rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to release the robot's brakes - brake_release_srv_ = node_->create_client( - "dashboard_client/brake_release", rclcpp::QoS(rclcpp::KeepLast(10)), brake_release_cb_); + brake_release_srv_ = node_->create_client("dashboard_client/brake_release", + rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to stop UR program execution on the robot stop_program_srv_ = node_->create_client("dashboard_client/stop", - rclcpp::QoS(rclcpp::KeepLast(10)), stop_program_cb_); + rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); // Service to start UR program execution on the robot play_program_srv_ = node_->create_client("dashboard_client/play", - rclcpp::QoS(rclcpp::KeepLast(10)), play_program_cb_); + rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); play_program_srv_->wait_for_service(); resend_robot_program_srv_ = node_->create_client( - "io_and_status_controller/resend_robot_program", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); + "io_and_status_controller/resend_robot_program", rclcpp::QoS(rclcpp::KeepLast(10)), service_cb_grp_); resend_robot_program_srv_->wait_for_service(); feedback_ = std::make_shared(); From 01aa6e8b4e215241f1f093582fb9d2ddf98fce83 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 10:08:46 +0100 Subject: [PATCH 10/13] Remove catkin libraries --- ur_robot_driver/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 82297cc11..f8ac46f39 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -84,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}) # @@ -103,7 +103,7 @@ add_executable(robot_state_helper src/robot_state_helper.cpp src/robot_state_helper_node.cpp ) -target_link_libraries(robot_state_helper ${catkin_LIBRARIES} ur_client_library::urcl) +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 From 557645d9fe3a3285e1e1225b9896d516399123f7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 15:19:11 +0100 Subject: [PATCH 11/13] Add documentation for robot_state_helper --- ur_dashboard_msgs/action/SetMode.action | 5 +- ur_robot_driver/doc/index.rst | 1 + ur_robot_driver/doc/robot_state_helper.rst | 59 ++++++++++++++++++++++ 3 files changed, 64 insertions(+), 1 deletion(-) create mode 100644 ur_robot_driver/doc/robot_state_helper.rst 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/doc/index.rst b/ur_robot_driver/doc/index.rst index 83d915f09..30140fe2a 100644 --- a/ur_robot_driver/doc/index.rst +++ b/ur_robot_driver/doc/index.rst @@ -17,3 +17,4 @@ ur_robot_driver setup_tool_communication hardware_interface_parameters dashboard_client + robot_state_helper 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`. From 80666b89265b68f75f5027bef6563b7f6f260cd9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 7 Feb 2025 15:32:14 +0100 Subject: [PATCH 12/13] Add controller_stopper documentation --- ur_robot_driver/doc/controller_stopper.rst | 29 ++++++++++++++++++++++ ur_robot_driver/doc/index.rst | 1 + ur_robot_driver/doc/usage/startup.rst | 7 ++++-- 3 files changed, 35 insertions(+), 2 deletions(-) create mode 100644 ur_robot_driver/doc/controller_stopper.rst 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 30140fe2a..c28b54377 100644 --- a/ur_robot_driver/doc/index.rst +++ b/ur_robot_driver/doc/index.rst @@ -18,3 +18,4 @@ ur_robot_driver hardware_interface_parameters dashboard_client robot_state_helper + controller_stopper diff --git a/ur_robot_driver/doc/usage/startup.rst b/ur_robot_driver/doc/usage/startup.rst index 6103121d4..6b0644563 100644 --- a/ur_robot_driver/doc/usage/startup.rst +++ b/ur_robot_driver/doc/usage/startup.rst @@ -71,12 +71,12 @@ Depending on the :ref:`robot control mode` 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.`` From def86508eba872094c6185456c048356ab9393d2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 12 Feb 2025 18:55:31 +0100 Subject: [PATCH 13/13] Refactor code and check all calls --- .../ur_robot_driver/robot_state_helper.hpp | 20 +- ur_robot_driver/src/robot_state_helper.cpp | 366 +++++++++++------- 2 files changed, 231 insertions(+), 155 deletions(-) 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 index 21385981c..d3c03189b 100644 --- a/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -34,6 +34,7 @@ #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" @@ -60,10 +61,14 @@ class RobotStateHelper void updateRobotState(); - void doTransition(); + 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); @@ -71,10 +76,6 @@ class RobotStateHelper void setModeExecute(const std::shared_ptr goal_handle); - void startActionServer(); - bool is_started_; - bool in_action_; - bool headless_mode_; std::shared_ptr result_; @@ -82,8 +83,12 @@ class RobotStateHelper std::shared_ptr goal_; std::shared_ptr current_goal_handle_; - urcl::RobotMode robot_mode_; - urcl::SafetyMode safety_mode_; + 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_; @@ -91,6 +96,7 @@ class RobotStateHelper rclcpp::Subscription::SharedPtr robot_mode_sub_; rclcpp::Subscription::SharedPtr safety_mode_sub_; + rclcpp::Subscription::SharedPtr program_running_sub; rclcpp::CallbackGroup::SharedPtr service_cb_grp_; diff --git a/ur_robot_driver/src/robot_state_helper.cpp b/ur_robot_driver/src/robot_state_helper.cpp index 2a41610eb..cd3d4ef29 100755 --- a/ur_robot_driver/src/robot_state_helper.cpp +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -26,6 +26,7 @@ // 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" @@ -41,10 +42,9 @@ namespace ur_robot_driver { RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) : node_(node) - , is_started_(false) - , in_action_(false) , 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; @@ -57,6 +57,9 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) 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); @@ -92,6 +95,11 @@ RobotStateHelper::RobotStateHelper(const rclcpp::Node::SharedPtr& node) 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) @@ -101,10 +109,10 @@ void RobotStateHelper::robotModeCallback(ur_dashboard_msgs::msg::RobotMode::Shar RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "The robot is currently in mode " << robotModeString(robot_mode_) << "."); if (in_action_) { - updateRobotState(); - if (!is_started_) { - startActionServer(); - } + std::scoped_lock lock(goal_mutex_); + feedback_->current_robot_mode = + static_cast(robot_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); } } } @@ -115,143 +123,121 @@ void RobotStateHelper::safetyModeCallback(ur_dashboard_msgs::msg::SafetyMode::Sh 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_) << "."); - updateRobotState(); - if (!is_started_) { - startActionServer(); + if (in_action_) { + std::scoped_lock lock(goal_mutex_); + feedback_->current_safety_mode = + static_cast(safety_mode_.load()); + current_goal_handle_->publish_feedback(feedback_); } } } -void RobotStateHelper::doTransition() +bool RobotStateHelper::recoverFromSafety() { - if (static_cast(goal_->target_robot_mode) < robot_mode_) { - safeDashboardTrigger(this->power_off_srv_); - } else { - switch (safety_mode_) { - case urcl::SafetyMode::PROTECTIVE_STOP: - safeDashboardTrigger(this->unlock_protective_stop_srv_); - break; - 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."); - break; - case urcl::SafetyMode::VIOLATION:; - case urcl::SafetyMode::FAULT: - safeDashboardTrigger(this->restart_safety_srv_); - break; - default: - 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: - safeDashboardTrigger(this->power_on_srv_); - break; - 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: - safeDashboardTrigger(this->brake_release_srv_); - 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: - 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."); - } - } + 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::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) +bool RobotStateHelper::jumpToRobotMode(const urcl::RobotMode target_mode) { - assert(srv != nullptr); - auto request = std::make_shared(); - auto future = srv->async_send_request(request); - future.wait(); - RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), "Service response received: " << future.get()->message); - return true; + 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; } -void RobotStateHelper::updateRobotState() +bool RobotStateHelper::doTransition(const urcl::RobotMode target_mode) { - if (is_started_) { - // Update action feedback - feedback_->current_robot_mode = - static_cast(robot_mode_); - feedback_->current_safety_mode = - static_cast(safety_mode_); - current_goal_handle_->publish_feedback(feedback_); - - if (robot_mode_ < static_cast(goal_->target_robot_mode) || - safety_mode_ > urcl::SafetyMode::REDUCED) { - // Transition to next mode - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("ur_robot_state_helper"), - "Current robot mode is " - << robotModeString(robot_mode_) << " while target mode is " - << robotModeString(static_cast(goal_->target_robot_mode))); - doTransition(); - } else if (robot_mode_ == static_cast(goal_->target_robot_mode)) { - // Final mode reached - in_action_ = false; - result_->success = true; - result_->message = "Reached target robot mode."; - if (robot_mode_ == urcl::RobotMode::RUNNING && goal_->play_program) { - if (headless_mode_) { - safeDashboardTrigger(this->resend_robot_program_srv_); - } else { - // The dashboard denies playing immediately after switching the mode to RUNNING - sleep(1); - safeDashboardTrigger(this->play_program_srv_); + 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; } } - current_goal_handle_->succeed(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.)"; - current_goal_handle_->abort(result_); - } + 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; } -void RobotStateHelper::startActionServer() +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) { - if (robot_mode_ != urcl::RobotMode::UNKNOWN && safety_mode_ != urcl::SafetyMode::UNDEFINED_SAFETY_MODE) { - is_started_ = true; - } - 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)); + 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) @@ -259,32 +245,53 @@ void RobotStateHelper::setModeAcceptCallback(const std::shared_ptrstop_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; - current_goal_handle_ = goal_handle; - const auto goal = current_goal_handle_->get_goal(); + const auto goal = goal_handle->get_goal(); this->goal_ = goal; - if (goal_->target_robot_mode > 8 || goal_->target_robot_mode < -1) { - result_->message = "Requested illegal mode."; - result_->success = false; - current_goal_handle_->abort(result_); - } else { - RCLCPP_INFO_STREAM(rclcpp::get_logger("robot_state_helper"), - "Target mode was set to " - << robotModeString(static_cast(goal_->target_robot_mode)) << "."); - switch (static_cast(goal_->target_robot_mode)) { + 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) { - safeDashboardTrigger(this->stop_program_srv_); + 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_ != static_cast(goal_->target_robot_mode) || - safety_mode_ > urcl::SafetyMode::REDUCED) { - doTransition(); - } else { - updateRobotState(); + 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: @@ -294,18 +301,70 @@ void RobotStateHelper::setModeExecute(const std::shared_ptrmessage = "Requested target mode " + - robotModeString(static_cast(goal_->target_robot_mode)) + - " which cannot be explicitly selected."; + result_->message = + "Requested target mode " + robotModeString(target_mode) + " which cannot be explicitly selected."; result_->success = false; - current_goal_handle_->abort(result_); + { + std::scoped_lock lock(goal_mutex_); + current_goal_handle_->abort(result_); + } + return; break; default: result_->message = "Requested illegal mode."; result_->success = false; - current_goal_handle_->abort(result_); + { + 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_); + } } } @@ -313,6 +372,17 @@ 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; }