From ac9b4ef73a1cee3174d400ba5ee11c22dff14905 Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Tue, 11 Jun 2019 11:35:17 +0200
Subject: [PATCH] added ros-publishing of changes in the state of the program
 on the robot

---
 .../include/ur_rtde_driver/ros/hardware_interface.h         | 2 ++
 ur_rtde_driver/src/ros/hardware_interface.cpp               | 6 +++++-
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index 8a8baff..efe0995 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -32,6 +32,7 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <algorithm>
+#include <std_msgs/Bool.h>
 
 #include <ur_controllers/speed_scaling_interface.h>
 #include <ur_controllers/scaled_joint_command_interface.h>
@@ -101,6 +102,7 @@ protected:
 
   std::string tcp_link_;
   bool robot_program_running_;
+  ros::Publisher program_state_pub_;
 };
 
 }  // namespace ur_driver
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 5d690dd..0bc19e0 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -64,6 +64,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   }
 
   tcp_link_ = robot_hw_nh.param<std::string>("tcp_link", "tool0");
+  program_state_pub_ = robot_hw_nh.advertise<std_msgs::Bool>("robot_program_running", 10, true);
 
   ROS_INFO_STREAM("Initializing urdriver");
   ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
@@ -107,6 +108,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
 
   ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
 
+
   return true;
 }
 
@@ -272,8 +274,10 @@ bool HardwareInterface ::isRobotProgramRunning() const
 
 void HardwareInterface ::handleRobotProgramStop(bool program_running)
 {
-  // TODO add ROS publisher to call for controller stop
   robot_program_running_ = program_running;
+  std_msgs::Bool msg;
+  msg.data = robot_program_running_;
+  program_state_pub_.publish(msg);
 }
 
 }  // namespace ur_driver