Merge pull request #141 from isys-vision/robot_status

fmauch · web-flow · commit ad3cb7c2cf91 · 2020-04-21T14:31:29.000+02:00
Robot status topic via controller
diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt
@@ -15,6 +15,7 @@ find_package(catkin REQUIRED
     controller_manager
     geometry_msgs
     hardware_interface
+    industrial_robot_status_interface
     pluginlib
     roscpp
     sensor_msgs
diff --git a/ur_robot_driver/config/ur10_controllers.yaml b/ur_robot_driver/config/ur10_controllers.yaml
@@ -139,3 +139,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/config/ur10e_controllers.yaml b/ur_robot_driver/config/ur10e_controllers.yaml
@@ -139,3 +139,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/config/ur3_controllers.yaml b/ur_robot_driver/config/ur3_controllers.yaml
@@ -140,3 +140,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/config/ur3e_controllers.yaml b/ur_robot_driver/config/ur3e_controllers.yaml
@@ -139,3 +139,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/config/ur5_controllers.yaml b/ur_robot_driver/config/ur5_controllers.yaml
@@ -139,3 +139,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/config/ur5e_controllers.yaml b/ur_robot_driver/config/ur5e_controllers.yaml
@@ -139,3 +139,8 @@ vel_traj_controller:
 joint_group_vel_controller:
    type: velocity_controllers/JointGroupVelocityController
    joints: *robot_joints
+
+robot_status_controller:
+   type: industrial_robot_status_controller/IndustrialRobotStatusController
+   handle_name: industrial_robot_status_handle
+   publish_rate: 10
diff --git a/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h b/ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h
@@ -54,6 +54,8 @@
 #include <ur_dashboard_msgs/RobotMode.h>
 #include <ur_dashboard_msgs/SafetyMode.h>
 
+#include <industrial_robot_status_interface/industrial_robot_status_interface.h>
+
 namespace ur_driver
 {
 /*!
@@ -179,6 +181,12 @@ class HardwareInterface : public hardware_interface::RobotHW
   void publishToolData();
   void publishRobotAndSafetyMode();
 
+  /*!
+   * \brief Read and evaluate data in order to set robot status properties for industrial
+   *        robot status interface
+   */
+  void extractRobotStatus();
+
   bool stopControl(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& res);
 
   template <typename T>
@@ -242,6 +250,8 @@ class HardwareInterface : public hardware_interface::RobotHW
   std::vector<std::string> joint_names_;
   int32_t robot_mode_;
   int32_t safety_mode_;
+  std::bitset<4> robot_status_bits_;
+  std::bitset<11> safety_status_bits_;
 
   std::unique_ptr<realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>> tcp_pose_pub_;
   std::unique_ptr<realtime_tools::RealtimePublisher<ur_msgs::IOStates>> io_pub_;
@@ -254,6 +264,9 @@ class HardwareInterface : public hardware_interface::RobotHW
   ros::ServiceServer resend_robot_program_srv_;
   ros::Subscriber command_sub_;
 
+  industrial_robot_status_interface::RobotStatus robot_status_resource_{};
+  industrial_robot_status_interface::IndustrialRobotStatusInterface robot_status_interface_{};
+
   uint32_t runtime_state_;
   bool position_controller_running_;
   bool velocity_controller_running_;
diff --git a/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h b/ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h
@@ -48,6 +48,29 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
+enum class UrRtdeRobotStatusBits
+{
+  IS_POWER_ON = 0,
+  IS_PROGRAM_RUNNING = 1,
+  IS_TEACH_BUTTON_PRESSED = 2,
+  IS_POWER_BUTTON_PRESSED = 3
+};
+
+enum class UrRtdeSafetyStatusBits
+{
+  IS_NORMAL_MODE = 0,
+  IS_REDUCED_MODE = 1,
+  IS_PROTECTIVE_STOPPED = 2,
+  IS_RECOVERY_MODE = 3,
+  IS_SAFEGUARD_STOPPED = 4,
+  IS_SYSTEM_EMERGENCY_STOPPED = 5,
+  IS_ROBOT_EMERGENCY_STOPPED = 6,
+  IS_EMERGENCY_STOPPED = 7,
+  IS_VIOLATION = 8,
+  IS_FAULT = 9,
+  IS_STOPPED_DUE_TO_SAFETY = 10
+};
+
 /*!
  * \brief The RTDEClient class manages communication over the RTDE interface. It contains the RTDE
  * handshake and read and write functionality to and from the robot.
diff --git a/ur_robot_driver/launch/ur_common.launch b/ur_robot_driver/launch/ur_common.launch
@@ -8,7 +8,7 @@
   <arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
   <arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
-  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
+  <arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller robot_status_controller" doc="Controllers that are activated by default."/>
   <arg name="stopped_controllers" default="pos_traj_controller joint_group_vel_controller" doc="Controllers that are initally loaded, but not started."/>
   <arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
   <arg name="tool_parity" default="0" doc="Parity configuration used for tool communication. Only used, when `use_tool_communication` is set to true."/>
diff --git a/ur_robot_driver/launch/ur_control.launch b/ur_robot_driver/launch/ur_control.launch
@@ -13,7 +13,7 @@
   <arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
   <arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description. Pass the same config file that is passed to the robot_description."/>
   <arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
-  <arg name="controllers" default="joint_state_controller vel_based_pos_traj_controller force_torque_sensor_controller"/>
+  <arg name="controllers" default="joint_state_controller vel_based_pos_traj_controller force_torque_sensor_controller robot_status_controller"/>
   <arg name="stopped_controllers" default="joint_group_vel_controller"/>
   <arg name="urscript_file" default="$(find ur_robot_driver)/resources/ros_control.urscript" doc="Path to URScript that will be sent to the robot and that forms the main control program."/>
   <arg name="rtde_output_recipe_file" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt" doc="Recipe file used for the RTDE-outputs. Only change this if you know what you're doing."/>
@@ -76,6 +76,7 @@
        - "joint_state_controller"
        - "speed_scaling_state_controller"
        - "force_torque_sensor_controller"
+       - "robot_status_controller"
     </rosparam>
   </node>
 
diff --git a/ur_robot_driver/package.xml b/ur_robot_driver/package.xml
@@ -21,6 +21,7 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
+  <build_depend>industrial_robot_status_interface</build_depend>
 
   <depend>actionlib</depend>
   <depend>control_msgs</depend>
@@ -41,6 +42,7 @@
 
   <exec_depend>controller_stopper</exec_depend>
   <exec_depend>force_torque_sensor_controller</exec_depend>
+  <exec_depend>industrial_robot_status_controller</exec_depend>
   <exec_depend>joint_state_controller</exec_depend>
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>robot_state_publisher</exec_depend>
diff --git a/ur_robot_driver/resources/rtde_output_recipe.txt b/ur_robot_driver/resources/rtde_output_recipe.txt
@@ -22,3 +22,5 @@ tool_output_current
 tool_temperature
 robot_mode
 safety_mode
+robot_status_bits
+safety_status_bits
diff --git a/ur_robot_driver/src/ros/hardware_interface.cpp b/ur_robot_driver/src/ros/hardware_interface.cpp
@@ -31,8 +31,20 @@
 
 #include <Eigen/Geometry>
 
+using industrial_robot_status_interface::RobotMode;
+using industrial_robot_status_interface::TriState;
+using namespace ur_driver::rtde_interface;
+
 namespace ur_driver
 {
+// bitset mask is applied to robot safety status bits in order to determine 'in_error' state
+static const std::bitset<11> in_error_bitset_(1 << toUnderlying(UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED) |
+                                              1 << toUnderlying(UrRtdeSafetyStatusBits::IS_ROBOT_EMERGENCY_STOPPED) |
+                                              1 << toUnderlying(UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED) |
+                                              1 << toUnderlying(UrRtdeSafetyStatusBits::IS_VIOLATION) |
+                                              1 << toUnderlying(UrRtdeSafetyStatusBits::IS_FAULT) |
+                                              1 << toUnderlying(UrRtdeSafetyStatusBits::IS_STOPPED_DUE_TO_SAFETY));
+
 HardwareInterface::HardwareInterface()
   : joint_position_command_({ 0, 0, 0, 0, 0, 0 })
   , joint_velocity_command_({ 0, 0, 0, 0, 0, 0 })
@@ -299,6 +311,9 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   fts_interface_.registerHandle(hardware_interface::ForceTorqueSensorHandle(
       "wrench", tf_prefix_ + "tool0_controller", fts_measurements_.begin(), fts_measurements_.begin() + 3));
 
+  robot_status_interface_.registerHandle(industrial_robot_status_interface::IndustrialRobotStatusHandle(
+      "industrial_robot_status_handle", robot_status_resource_));
+
   // Register interfaces
   registerInterface(&js_interface_);
   registerInterface(&spj_interface_);
@@ -307,6 +322,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   registerInterface(&svj_interface_);
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);
+  registerInterface(&robot_status_interface_);
 
   tcp_pose_pub_.reset(new realtime_tools::RealtimePublisher<tf2_msgs::TFMessage>(root_nh, "/tf", 100));
   io_pub_.reset(new realtime_tools::RealtimePublisher<ur_msgs::IOStates>(robot_hw_nh, "io_states", 1));
@@ -392,6 +408,15 @@ void HardwareInterface::readBitsetData(const std::unique_ptr<rtde_interface::Dat
 
 void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
 {
+  // set defaults
+  robot_status_resource_.mode = RobotMode::UNKNOWN;
+  robot_status_resource_.e_stopped = TriState::UNKNOWN;
+  robot_status_resource_.drives_powered = TriState::UNKNOWN;
+  robot_status_resource_.motion_possible = TriState::UNKNOWN;
+  robot_status_resource_.in_motion = TriState::UNKNOWN;
+  robot_status_resource_.in_error = TriState::UNKNOWN;
+  robot_status_resource_.error_code = 0;
+
   std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
   if (data_pkg)
   {
@@ -415,11 +440,15 @@ void HardwareInterface::read(const ros::Time& time, const ros::Duration& period)
     readData(data_pkg, "tool_temperature", tool_temperature_);
     readData(data_pkg, "robot_mode", robot_mode_);
     readData(data_pkg, "safety_mode", safety_mode_);
+    readBitsetData<uint32_t>(data_pkg, "robot_status_bits", robot_status_bits_);
+    readBitsetData<uint32_t>(data_pkg, "safety_status_bits", safety_status_bits_);
     readBitsetData<uint64_t>(data_pkg, "actual_digital_input_bits", actual_dig_in_bits_);
     readBitsetData<uint64_t>(data_pkg, "actual_digital_output_bits", actual_dig_out_bits_);
     readBitsetData<uint32_t>(data_pkg, "analog_io_types", analog_io_types_);
     readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
 
+    extractRobotStatus();
+
     publishIOData();
     publishToolData();
 
@@ -664,6 +693,59 @@ void HardwareInterface::publishPose()
   }
 }
 
+void HardwareInterface::extractRobotStatus()
+{
+  using namespace rtde_interface;
+
+  robot_status_resource_.mode = robot_status_bits_[toUnderlying(UrRtdeRobotStatusBits::IS_TEACH_BUTTON_PRESSED)] ?
+                                    RobotMode::MANUAL :
+                                    RobotMode::AUTO;
+
+  robot_status_resource_.e_stopped = safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED)] ?
+                                         TriState::TRUE :
+                                         TriState::FALSE;
+
+  // Note that this is true as soon as the drives are powered,
+  // even if the brakes are still closed
+  // which is in slight contrast to the comments in the
+  // message definition
+  robot_status_resource_.drives_powered =
+      robot_status_bits_[toUnderlying(UrRtdeRobotStatusBits::IS_POWER_ON)] ? TriState::TRUE : TriState::FALSE;
+
+  // I found no way to reliably get information if the robot is moving
+  robot_status_resource_.in_motion = TriState::UNKNOWN;
+
+  if ((safety_status_bits_ & in_error_bitset_).any())
+  {
+    robot_status_resource_.in_error = TriState::TRUE;
+  }
+  else
+  {
+    robot_status_resource_.in_error = TriState::FALSE;
+  }
+
+  // Motion is not possible if controller is either in error or in safeguard stop.
+  // TODO: Check status of robot program "external control" here as well
+  if (robot_status_resource_.in_error == TriState::TRUE ||
+      safety_status_bits_[toUnderlying(UrRtdeSafetyStatusBits::IS_SAFEGUARD_STOPPED)])
+  {
+    robot_status_resource_.motion_possible = TriState::FALSE;
+  }
+  else if (robot_mode_ == ur_dashboard_msgs::RobotMode::RUNNING)
+
+  {
+    robot_status_resource_.motion_possible = TriState::TRUE;
+  }
+  else
+  {
+    robot_status_resource_.motion_possible = TriState::FALSE;
+  }
+
+  // the error code, if any, is not transmitted by this protocol
+  // it can and should be fetched separately
+  robot_status_resource_.error_code = 0;
+}
+
 void HardwareInterface::publishIOData()
 {
   if (io_pub_)
