From 7fdff1c66dc9bc1d4975551733745b7a6b8e1279 Mon Sep 17 00:00:00 2001
From: Simon <sjansen@smart-robotics.com>
Date: Thu, 8 Oct 2015 15:06:04 +0200
Subject: [PATCH] Publish robot_state

---
 src/ur_ros_wrapper.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp
index c782bde..28b8add 100644
--- a/src/ur_ros_wrapper.cpp
+++ b/src/ur_ros_wrapper.cpp
@@ -38,6 +38,7 @@
 #include "ur_msgs/Digital.h"
 #include "ur_msgs/Analog.h"
 #include "std_msgs/String.h"
+#include "std_msgs/Int32.h"
 #include <controller_manager/controller_manager.h>
 
 class RosWrapper {
@@ -54,8 +55,8 @@ protected:
 	ros::Subscriber urscript_sub_;
 	ros::ServiceServer io_srv_;
 	ros::ServiceServer payload_srv_;
-	std::thread* rt_publish_thread_;
-	std::thread* mb_publish_thread_;
+    std::thread* rt_publish_thread_;
+    std::thread* mb_publish_thread_;
 	double io_flag_delay_;
 	double max_velocity_;
 	std::vector<double> joint_offsets_;
@@ -423,6 +424,8 @@ private:
 				"joint_states", 1);
 		ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
 				"wrench", 1);
+        ros::Publisher robot_state_pub = nh_.advertise<std_msgs::Int32>(
+                "ur_driver/robot_state", 1);
 		while (ros::ok()) {
 			sensor_msgs::JointState joint_msg;
 			joint_msg.name = robot_.getJointNames();
@@ -453,6 +456,11 @@ private:
 			wrench_msg.wrench.torque.z = tcp_force[5];
 			wrench_pub.publish(wrench_msg);
 
+            //Publish robot state
+            std_msgs::Int32 robot_state_msg;
+            robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode();
+            robot_state_pub.publish(robot_state_msg);
+
 			robot_.rt_interface_->robot_state_->setDataPublished();
 
 		}