Add robot mode publisher, masterboard publisher and waitForURProgram …

…example function (ros-industrial#247)

Squashed commits:

 * Add "program_running" status publisher
 * Add documentation and example
 * Add isProgramRunning() example function
 * Publish robot internal data
 * Change error return value in isProgramRunning
 * Throw exception in isProgramRunning
 * Insert missed timeout check, prettify
 * Decrease verbosity
 * Move RobotModeDataMsg to ur_msgs
 * clang format
 * Correct typos, add tested environment
 * Remove convenience header
 * Correct typo

CMakeLists.txt

@@ -24,7 +24,6 @@ find_package(catkin REQUIRED
     ur_msgs
 )
 
-
 catkin_package(
   INCLUDE_DIRS
     include

README.md

@@ -20,10 +20,11 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
 
   * Service call to set outputs and payload - Again, the string */ur\_driver* has been prepended compared to the old driver (Note: I am not sure if setting the payload actually works, as the robot GUI does not update. This is also true for the old ur\_driver  )
 
+  * Additionally, the masterboard state and robot mode are published to */ur\_driver/masterboard\_state* and */ur\_driver/robot\_mode\_state*. (Note: Only the parameters that are shared between all robot firmwares are published. The `digital_input_bits` and `digital_output_bits` fields of the MasterboardDataMsg are not set. Use the IOStates message for these.)
 
 * Besides this, the driver subscribes to two new topics:
 
-  * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like.
+  * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like. Completion can be checked with the */ur\_driver/robot_mode_state* topic. Include *ur_modern_driver/wait_for_program.h* for a convenience function.
 
   * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
 
@@ -34,7 +35,7 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
     * The velocity based controller sends joint speed commands to the robot, using the speedj command
     * The position based controller sends joint position commands to the robot, using the servoj command
     * I have so far only used the velocity based controller, but which one is optimal depends on the application.
-  * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant:
+  * As ros_control continuously controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant:
     * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ```
     * The running position trajectory controller can be stopped with a call to  ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this)
 
@@ -168,6 +169,8 @@ Should be compatible with all robots and control boxes with the newest firmware.
 
 ### Tested with:
 
+* Real UR5 with 3.9.0.46176 (Mar 01 2019)
+* Real UR5 and UR3 with 3.5.1.10661 (Dec 13 2017)
 * Real UR10 with CB2 running 1.8.14035
 * Real UR5 with CB2 running 1.8.14035
 * Simulated UR3 running 3.1.18024
@@ -176,7 +179,6 @@ Should be compatible with all robots and control boxes with the newest firmware.
 * Simulated UR5 running 1.7.10857
 * Simulated UR5 running 1.6.08725
 
-
 # Credits
 Please cite the following report if using this driver
 

include/ur_modern_driver/ros/mb_publisher.h

@@ -20,9 +20,12 @@
 
 #include <industrial_msgs/RobotStatus.h>
 #include <ros/ros.h>
+#include <std_msgs/Bool.h>
 #include <ur_msgs/Analog.h>
 #include <ur_msgs/Digital.h>
 #include <ur_msgs/IOStates.h>
+#include <ur_msgs/MasterboardDataMsg.h>
+#include <ur_msgs/RobotModeDataMsg.h>
 
 #include "ur_modern_driver/ur/consumer.h"
 
@@ -34,6 +37,8 @@ class MBPublisher : public URStatePacketConsumer
   NodeHandle nh_;
   Publisher io_pub_;
   Publisher status_pub_;
+  Publisher masterboard_state_pub_;
+  Publisher robot_mode_state_pub_;
 
   template <size_t N>
   inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
@@ -56,6 +61,8 @@ class MBPublisher : public URStatePacketConsumer
   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))
+    , masterboard_state_pub_(nh_.advertise<ur_msgs::MasterboardDataMsg>("ur_driver/masterboard_state", 1))
+    , robot_mode_state_pub_(nh_.advertise<ur_msgs::RobotModeDataMsg>("ur_driver/robot_mode_state", 1))
   {
   }
 

src/ros/mb_publisher.cpp

@@ -34,12 +34,30 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data
   appendAnalog(io_msg.analog_out_states, data.analog_output1, 1);
 
   io_pub_.publish(io_msg);
+
+  // --- Publishes the data in robot-specific format.
+  ur_msgs::MasterboardDataMsg msg;
+  msg.digital_input_bits = 0;   // TODO: Set this.
+  msg.digital_output_bits = 0;  // TODO: Set this.
+  msg.analog_input_range0 = data.analog_input_range0;
+  msg.analog_input_range1 = data.analog_input_range1;
+  msg.analog_input0 = data.analog_input0;
+  msg.analog_input1 = data.analog_input1;
+  msg.analog_output_domain0 = data.analog_output_domain0;
+  msg.analog_output_domain1 = data.analog_output_domain1;
+  msg.analog_output0 = data.analog_output0;
+  msg.analog_output1 = data.analog_output1;
+  msg.masterboard_temperature = data.master_board_temperature;
+  msg.robot_voltage_48V = data.robot_voltage_48V;
+  msg.robot_current = data.robot_current;
+  msg.master_io_current = data.master_IO_current;
+  masterboard_state_pub_.publish(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
+  // even if the brakes are still closed
   // which is in slight contrast to the comments in the
   // message definition
   status.drives_powered.val = data.robot_power_on;
@@ -60,6 +78,18 @@ void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const
   status.in_error.val = data.protective_stopped;
 
   status_pub_.publish(status);
+
+  // --- Publishes the data in robot-specific format.
+  ur_msgs::RobotModeDataMsg msg;
+  msg.timestamp = data.timestamp;
+  msg.is_robot_connected = data.physical_robot_connected;
+  msg.is_real_robot_enabled = data.real_robot_enabled;
+  msg.is_power_on_robot = data.robot_power_on;
+  msg.is_emergency_stopped = data.emergency_stopped;
+  msg.is_protective_stopped = data.protective_stopped;
+  msg.is_program_running = data.program_running;
+  msg.is_program_paused = data.program_paused;
+  robot_mode_state_pub_.publish(msg);
 }
 
 void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const