From 4f646bb88895adb056ec8fe7b8320d771b0e87f1 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Thu, 23 May 2019 17:44:54 +0200
Subject: [PATCH] Only send robot commands, when robot is in runtime states
 PLAYING and PAUSING

---
 .../include/ur_rtde_driver/ros/hardware_interface.h    |  1 +
 .../include/ur_rtde_driver/rtde/data_package.h         | 10 ++++++++++
 ur_rtde_driver/resources/rtde_recipe.txt               |  1 +
 ur_rtde_driver/src/ros/hardware_interface.cpp          |  8 +++++++-
 4 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
index af2a173..9ac295a 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h
@@ -74,6 +74,7 @@ protected:
   double speed_scaling_combined_;
   std::vector<std::string> joint_names_;
 
+  uint32_t runtime_state_;
   bool position_controller_running_;
 };
 
diff --git a/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h b/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
index 636b80a..2906a2b 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/rtde/data_package.h
@@ -38,6 +38,16 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
+enum class RUNTIME_STATE : uint32_t
+{
+  STOPPING = 0,
+  STOPPED = 1,
+  PLAYING = 2,
+  PAUSING = 3,
+  PAUSED = 4,
+  RESUMING = 5
+};
+
 struct ParseVisitor : public boost::static_visitor<>
 {
   template <typename T>
diff --git a/ur_rtde_driver/resources/rtde_recipe.txt b/ur_rtde_driver/resources/rtde_recipe.txt
index 24588e3..e4ef814 100644
--- a/ur_rtde_driver/resources/rtde_recipe.txt
+++ b/ur_rtde_driver/resources/rtde_recipe.txt
@@ -3,3 +3,4 @@ actual_q
 actual_qd
 speed_scaling
 target_speed_fraction
+runtime_state
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index 0e8349f..c6ca63f 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -35,6 +35,7 @@ HardwareInterface::HardwareInterface()
   , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_names_(6)
+  , runtime_state_(static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::STOPPED))
   , position_controller_running_(false)
 
 {
@@ -124,6 +125,11 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
       // This throwing should never happen unless misconfigured
       throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!");
     }
+    if (!data_pkg->getData("runtime_state", runtime_state_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find speed_scaling in data sent from robot. This should not happen!");
+    }
 
     speed_scaling_combined_ = speed_scaling_ * target_speed_fraction_;
   }
@@ -135,7 +141,7 @@ void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period
 
 void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
 {
-  if (position_controller_running_)
+  if (position_controller_running_ && (runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PLAYING) || runtime_state_ == static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::PAUSING)))
   {
     // ROS_INFO_STREAM("Writing command: " << joint_position_command_);
     ur_driver_->writeJointCommand(joint_position_command_);