From 5310b8afa2105b10da7ebb105f38ec99a3f5111b Mon Sep 17 00:00:00 2001 From: Robert Bevec Date: Sat, 21 Dec 2019 18:46:17 +0100 Subject: [PATCH] Added a topic that shows program data for any program --- ur_robot_driver/doc/ROS_INTERFACE.md | 4 ++++ .../ur_robot_driver/ros/hardware_interface.h | 3 +++ ur_robot_driver/src/ros/hardware_interface.cpp | 15 +++++++++++++++ 3 files changed, 22 insertions(+) diff --git a/ur_robot_driver/doc/ROS_INTERFACE.md b/ur_robot_driver/doc/ROS_INTERFACE.md index 06410f116..0a1d021a4 100644 --- a/ur_robot_driver/doc/ROS_INTERFACE.md +++ b/ur_robot_driver/doc/ROS_INTERFACE.md @@ -823,6 +823,10 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu Whenever the runtime state of the "External Control" program node in the UR-program changes, a message gets published here. So this is equivalent to the information whether the robot accepts commands from ROS side. + * "**runtime_state**" ([std_msgs/UInt32](http://docs.ros.org/api/std_msgs/html/msg/UInt32.html)) + + State of *any* active program on the controller, as defined in the RTDE. (0 - stopping, 1 - stopped, 2 - playing, 3 - pausing, 4 - paused, 5 - resuming) + #### Subscribed topics * "**script_command**" ([std_msgs/String](http://docs.ros.org/api/std_msgs/html/msg/String.html)) diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h index ff380f0f4..0eb108e20 100644 --- a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h +++ b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include "tf2_msgs/TFMessage.h" @@ -178,6 +179,7 @@ class HardwareInterface : public hardware_interface::RobotHW void publishIOData(); void publishToolData(); void publishRobotAndSafetyMode(); + void publishRuntimeState(); bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res); @@ -239,6 +241,7 @@ class HardwareInterface : public hardware_interface::RobotHW std::unique_ptr> tool_data_pub_; std::unique_ptr> robot_mode_pub_; std::unique_ptr> safety_mode_pub_; + std::unique_ptr> runtime_state_pub_; ros::ServiceServer set_speed_slider_srv_; ros::ServiceServer set_io_srv_; diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp index 692130b74..819c39729 100644 --- a/ur_robot_driver/src/ros/hardware_interface.cpp +++ b/ur_robot_driver/src/ros/hardware_interface.cpp @@ -302,6 +302,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw new realtime_tools::RealtimePublisher(robot_hw_nh, "robot_mode", 1, true)); safety_mode_pub_.reset( new realtime_tools::RealtimePublisher(robot_hw_nh, "safety_mode", 1, true)); + runtime_state_pub_.reset( + new realtime_tools::RealtimePublisher(robot_hw_nh, "runtime_state", 1)); // Set the speed slider fraction used by the robot's execution. Values should be between 0 and 1. // Only set this smaller than 1 if you are using the scaled controllers (as by default) or you know what you're @@ -393,6 +395,7 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period) transformForceTorque(); publishPose(); publishRobotAndSafetyMode(); + publishRuntimeState(); // pausing state follows runtime state when pausing if (runtime_state_ == static_cast(rtde_interface::RUNTIME_STATE::PAUSED)) @@ -526,6 +529,18 @@ bool HardwareInterface::isRobotProgramRunning() const return robot_program_running_; } +void HardwareInterface::publishRuntimeState() +{ + if (runtime_state_pub_) + { + if (runtime_state_pub_->trylock()) + { + runtime_state_pub_->msg_.data = runtime_state_; + runtime_state_pub_->unlockAndPublish(); + } + } +} + void HardwareInterface::handleRobotProgramState(bool program_running) { if (robot_program_running_ == false && program_running)