Added a topic that shows program data for any program

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))
 

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -35,6 +35,7 @@
 #include <std_msgs/Bool.h>
 #include <std_msgs/Float64.h>
 #include <std_msgs/String.h>
+#include <std_msgs/UInt32.h>
 #include <std_srvs/Trigger.h>
 #include <realtime_tools/realtime_publisher.h>
 #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<realtime_tools::RealtimePublisher<ur_msgs::ToolDataMsg>> tool_data_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::RobotMode>> robot_mode_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_dashboard_msgs::SafetyMode>> safety_mode_pub_;
+  std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::UInt32>> runtime_state_pub_;
 
   ros::ServiceServer set_speed_slider_srv_;
   ros::ServiceServer set_io_srv_;

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<ur_dashboard_msgs::RobotMode>(robot_hw_nh, "robot_mode", 1, true));
   safety_mode_pub_.reset(
       new realtime_tools::RealtimePublisher<ur_dashboard_msgs::SafetyMode>(robot_hw_nh, "safety_mode", 1, true));
+  runtime_state_pub_.reset(
+      new realtime_tools::RealtimePublisher<std_msgs::UInt32>(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<uint32_t>(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)