Publish industrial_msgs::RobotStatus (#5)

include/ur_modern_driver/ros/mb_publisher.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <industrial_msgs/RobotStatus.h>
 #include <ros/ros.h>
 #include <ur_msgs/Analog.h>
 #include <ur_msgs/Digital.h>
@@ -14,6 +15,7 @@ class MBPublisher : public URStatePacketConsumer
 private:
   NodeHandle nh_;
   Publisher io_pub_;
+  Publisher status_pub_;
 
   template <size_t N>
   inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
@@ -28,9 +30,13 @@ private:
   }
 
   void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
+  void publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const;
+  void publishRobotStatus(const RobotModeData_V1_X& data) const;
+  void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
 
 public:
   MBPublisher() : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
+                , status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
   {
   }
 
@@ -51,4 +57,4 @@ public:
   virtual void stopConsumer()
   {
   }
-};
+};

include/ur_modern_driver/ros/rt_publisher.h

@@ -71,4 +71,4 @@ public:
   virtual void stopConsumer()
   {
   }
-};
+};