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..4de0e0c3e --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/robot_state_helper.hpp @@ -0,0 +1,83 @@ +#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(); + + void doTransition(); + + bool safeDashboardTrigger(rclcpp::Client::SharedPtr srv); + + 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_; + bool in_action_; + + 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::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_; + 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_ diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 4a8d51370..46ba61b9d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -236,6 +236,13 @@ 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 +361,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..d49b8322b --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper.cpp @@ -0,0 +1,294 @@ +#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) + , 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", 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", rclcpp::QoS(rclcpp::KeepLast(10)), unlock_cb_); + // Service to 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", + rclcpp::QoS(rclcpp::KeepLast(10)), power_on_cb_); + // Service to power off the robot + 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", 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", + 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", + rclcpp::QoS(rclcpp::KeepLast(10)), play_program_cb_); + 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_) << "."); + if (in_action_) { + 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() +{ + 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."); + } + } + } +} + +bool RobotStateHelper::safeDashboardTrigger(rclcpp::Client::SharedPtr srv) +{ + 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; +} + +void RobotStateHelper::updateRobotState() +{ + 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) { + // The dashboard denies playing immediately after switching the mode to RUNNING + sleep(1); + safeDashboardTrigger(this->play_program_srv_); + } + 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) +{ + in_action_ = true; + 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_); + } + doTransition(); + } else { + updateRobotState(); + } + 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 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..93eec28a0 --- /dev/null +++ b/ur_robot_driver/src/robot_state_helper_node.cpp @@ -0,0 +1,17 @@ +#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::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + + // rclcpp::spin(node); + + return 0; +}