Added checks for internal robot state machine

Author: MGBla

ur_robot_driver/include/ur_robot_driver/hardware_interface.h

@@ -55,6 +55,13 @@ using hardware_interface::status;
 
 namespace ur_robot_driver
 {
+enum class PausingState
+{
+  PAUSED,
+  RUNNING,
+  RAMPUP
+};
+
 /*!
  * \brief The HardwareInterface class handles the interface between the ROS system and the main
  * driver. It contains the read and write methods of the main control loop and registers various ROS
@@ -86,6 +93,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   return_type read() final;
   return_type write() final;
 
+ /*!
+   * \brief Read and evaluate data in order to set robot status properties for industrial
+   *        robot status interface
+   */
+  void extractRobotStatus();
+
   /*!
    * \brief Callback to handle a change in the current state of the URCaps program running on the
    * robot. Executed only on the state change.
@@ -113,9 +126,40 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   urcl::vector6d_t urcl_joint_efforts_;
   urcl::vector6d_t urcl_ft_sensor_measurements_;
   urcl::vector6d_t urcl_tcp_pose_;
+
+  // TODO exchange for industrial robot status interface
+  enum class TriState : int8_t
+  {
+    UNKNOWN = -1,
+    FALSE = 0,
+    TRUE = 1,
+  };
+  enum class RobotMode : int8_t
+  {
+    UNKNOWN = -1,
+    MANUAL = 1,
+    AUTO = 2,
+  };
+  typedef std::int32_t ErrorCode;
+  struct RobotStatus
+  {
+    RobotMode mode;
+    TriState e_stopped;
+    TriState drives_powered;
+    TriState motion_possible;
+    TriState in_motion;
+    TriState in_error;
+    ErrorCode error_code;
+  };
+  RobotStatus robot_status_resource_{};
+  // END TODO
+
   bool packet_read_;
 
   uint32_t runtime_state_;
+  bool position_controller_running_;
+  bool velocity_controller_running_;
+  bool controllers_initialized_;
 
   std::bitset<18> actual_dig_out_bits_;
   std::bitset<18> actual_dig_in_bits_;
@@ -140,6 +184,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
   bool non_blocking_read_;
   bool position_interface_in_use_;
 
+  PausingState pausing_state_;
+  double pausing_ramp_up_increment_;
+
   std::unique_ptr<urcl::UrDriver> ur_driver_;
 };
 }  // namespace ur_robot_driver

ur_robot_driver/src/hardware_interface.cpp

@@ -21,6 +21,7 @@
  *
  * \author  Lovro Ivanov [email protected]
  * \author  Andy Zelenak [email protected]
+ * \author  Marvin Große Besselmann [email protected]
  * \date    2020-11-9
  *
  */
@@ -37,6 +38,14 @@ namespace rtde = urcl::rtde_interface;
 
 namespace ur_robot_driver
 {
+static const std::bitset<11>
+    in_error_bitset_(1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_PROTECTIVE_STOPPED) |
+                     1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_ROBOT_EMERGENCY_STOPPED) |
+                     1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_EMERGENCY_STOPPED) |
+                     1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_VIOLATION) |
+                     1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_FAULT) |
+                     1 << urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_STOPPED_DUE_TO_SAFETY));
+
 hardware_interface::return_type URPositionHardwareInterface::configure(const HardwareInfo& system_info)
 {
   info_ = system_info;
@@ -49,6 +58,13 @@ hardware_interface::return_type URPositionHardwareInterface::configure(const Har
   urcl_tcp_pose_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_position_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
   urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
+  runtime_state_ = static_cast<uint32_t>(rtde::RUNTIME_STATE::STOPPED);
+  pausing_state_ = PausingState::RUNNING;
+  pausing_ramp_up_increment_ = 0.01;
+  position_controller_running_ = false;
+  velocity_controller_running_ = false;
+  controllers_initialized_ = false;
+
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints)
   {
@@ -338,6 +354,15 @@ void URPositionHardwareInterface::readBitsetData(const std::unique_ptr<rtde::Dat
 
 return_type URPositionHardwareInterface::read()
 {
+  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::DataPackage> data_pkg = ur_driver_->getDataPackage();
 
   if (data_pkg)
@@ -372,6 +397,42 @@ return_type URPositionHardwareInterface::read()
     readBitsetData<uint32_t>(data_pkg, "tool_analog_input_types", tool_analog_input_types_);
 
     // TODO logic for sending other stuff to higher level interface
+    extractRobotStatus();
+
+    // pausing state follows runtime state when pausing
+    if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PAUSED))
+    {
+      pausing_state_ = PausingState::PAUSED;
+    }
+    // When the robot resumed program execution and pausing state was PAUSED, we enter RAMPUP
+    else if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::PLAYING) &&
+             pausing_state_ == PausingState::PAUSED)
+    {
+      speed_scaling_combined_ = 0.0;
+      pausing_state_ = PausingState::RAMPUP;
+    }
+
+    if (pausing_state_ == PausingState::RAMPUP)
+    {
+      double speed_scaling_ramp = speed_scaling_combined_ + pausing_ramp_up_increment_;
+      speed_scaling_combined_ = std::min(speed_scaling_ramp, speed_scaling_ * target_speed_fraction_);
+
+      if (speed_scaling_ramp > speed_scaling_ * target_speed_fraction_)
+      {
+        pausing_state_ = PausingState::RUNNING;
+      }
+    }
+    else if (runtime_state_ == static_cast<uint32_t>(rtde::RUNTIME_STATE::RESUMING))
+    {
+      // We have to keep speed scaling on ROS side at 0 during RESUMING to prevent controllers from
+      // continuing to interpolate
+      speed_scaling_combined_ = 0.0;
+    }
+    else
+    {
+      // Normal case
+      speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_;
+    }
 
     return return_type::OK;
   }
@@ -426,6 +487,58 @@ return_type URPositionHardwareInterface::write()
   }
 }
 
+void URPositionHardwareInterface::extractRobotStatus()
+{
+    robot_status_resource_.mode =
+      robot_status_bits_[urcl::toUnderlying(rtde::UrRtdeRobotStatusBits::IS_TEACH_BUTTON_PRESSED)] ? RobotMode::MANUAL :
+                                                                                                     RobotMode::AUTO;
+
+  robot_status_resource_.e_stopped =
+      safety_status_bits_[urcl::toUnderlying(rtde::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_[urcl::toUnderlying(rtde::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_[urcl::toUnderlying(rtde::UrRtdeSafetyStatusBits::IS_SAFEGUARD_STOPPED)])
+  {
+    robot_status_resource_.motion_possible = TriState::FALSE;
+  }
+  // TODO The hardcoded 7 should be ur_dashboard_msgs::RobotMode::RUNNING exchange when msgs are built
+  else if (robot_mode_ == 7)
+  {
+    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 URPositionHardwareInterface::handleRobotProgramState(bool program_running)
 {
   if (!robot_program_running_ && program_running)