Publish industrial_msgs::RobotStatus (#5)

CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   actionlib
   control_msgs
   geometry_msgs
+  industrial_msgs
   roscpp
   sensor_msgs
   std_srvs

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

package.xml

@@ -45,6 +45,7 @@
   <build_depend>actionlib</build_depend>
   <build_depend>control_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>industrial_msgs</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>trajectory_msgs</build_depend>
@@ -57,6 +58,7 @@
   <run_depend>actionlib</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>geometry_msgs</run_depend>
+  <run_depend>industrial_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>sensor_msgs</run_depend>
   <run_depend>trajectory_msgs</run_depend>

src/ros/mb_publisher.cpp

@@ -18,6 +18,63 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data
   io_pub_.publish(io_msg);
 }
 
+void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
+{
+  //note that this is true as soon as the drives are powered,
+  //even if the breakes are still closed
+  //which is in slight contrast to the comments in the
+  //message definition
+  status.drives_powered.val = data.robot_power_on;
+
+  status.e_stopped.val = data.emergency_stopped;
+
+  //I found no way to reliably get information if the robot is moving
+  //data.programm_running would be true when using this driver to move the robot
+  //but it would also be true when another programm is running that might not
+  //in fact contain any movement commands
+  status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
+
+  //the error code, if any, is not transmitted by this protocol
+  //it can and should be fetched seperately
+  status.error_code = 0;
+
+  //note that e-stop is handled by a seperate variable
+  status.in_error.val = data.protective_stopped;
+
+  status_pub_.publish(status);
+}
+
+void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
+{
+  industrial_msgs::RobotStatus msg;
+
+  if (data.robot_mode == robot_mode_V1_X::ROBOT_FREEDRIVE_MODE)
+    msg.mode.val = industrial_msgs::RobotMode::MANUAL;
+  else
+    msg.mode.val = industrial_msgs::RobotMode::AUTO;
+
+  //todo: verify that this correct, there is also ROBOT_READY_MODE
+  msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE)
+                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+
+  publishRobotStatus(msg, data);
+}
+
+void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
+{
+  industrial_msgs::RobotStatus msg;
+
+  msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
+                             ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
+
+  if (data.control_mode == robot_control_mode_V3_X::TEACH)
+      msg.mode.val = industrial_msgs::RobotMode::MANUAL;
+  else
+      msg.mode.val = industrial_msgs::RobotMode::AUTO;
+
+  publishRobotStatus(msg, data);
+}
+
 bool MBPublisher::consume(MasterBoardData_V1_X& data)
 {
   ur_msgs::IOStates io_msg;
@@ -42,13 +99,16 @@ bool MBPublisher::consume(MasterBoardData_V3_2& data)
 
 bool MBPublisher::consume(RobotModeData_V1_X& data)
 {
+  publishRobotStatus(data);
   return true;
 }
 bool MBPublisher::consume(RobotModeData_V3_0__1& data)
 {
+  publishRobotStatus(data);
   return true;
 }
 bool MBPublisher::consume(RobotModeData_V3_2& data)
 {
+  publishRobotStatus(data);
   return true;
 }