implemented watchdog

it receives keepalive signals by the program running on the robot and monitors the socket connection to reopen the server when necessary

ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h

@@ -77,6 +77,23 @@ public:
     return server_.write(buffer, sizeof(buffer), written);
   }
 
+  std::string readKeepalive()
+  {
+    size_t buf_len = 16;
+    char buffer[buf_len];
+
+    bool read_successful = server_.readLine(buffer, buf_len);
+
+    if (read_successful)
+    {
+      return std::string(buffer);
+    }
+    else
+    {
+      return std::string("");
+    }
+  }
+
   template <typename T>
   size_t append(uint8_t* buffer, T& val)
   {

ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h

@@ -62,6 +62,10 @@ public:
 
   uint32_t getControlFrequency() const;
 
+  bool isRobotProgramRunning() const;
+
+  void handleRobotProgramStop(bool program_running);
+
 protected:
   /*!
    * \brief Transforms force-torque measurements reported from the robot from base to tool frame
@@ -96,6 +100,7 @@ protected:
   double pausing_ramp_up_increment_;
 
   std::string tcp_link_;
+  bool robot_program_running_;
 };
 
 }  // namespace ur_driver

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -48,7 +48,8 @@ public:
    * \param robot_ip IP-address under which the robot is reachable.
    * \param script_file URScript file that should be sent to the robot
    */
-  UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file);
+  UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file,
+           std::function<void(bool)> handle_program_state);
   virtual ~UrDriver() = default;
 
   /*!
@@ -67,8 +68,11 @@ public:
 
   bool writeJointCommand(const vector6d_t& values);
 
+  void startWatchdog();
+
 private:
   std::string readScriptFile(const std::string& filename);
+  std::string readKeepalive();
 
   int rtde_frequency_;
   comm::INotifier notifier_;
@@ -79,6 +83,11 @@ private:
   double servoj_time_;
   uint32_t servoj_gain_;
   double servoj_lookahead_time_;
+
+  std::thread watchdog_thread_;
+  bool reverse_interface_active_;
+  uint32_t reverse_port_;
+  std::function<void(bool)> handle_program_state_;
 };
 }  // namespace ur_driver
 #endif  // ifndef UR_RTDE_DRIVER_UR_UR_DRIVER_H_INCLUDED

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -66,7 +66,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h
   tcp_link_ = robot_hw_nh.param<std::string>("tcp_link", "tool0");
 
   ROS_INFO_STREAM("Initializing urdriver");
-  ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename));
+  ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename,
+                                std::bind(&HardwareInterface::handleRobotProgramStop, this, std::placeholders::_1)));
 
   if (!root_nh.getParam("hardware_interface/joints", joint_names_))
   {
@@ -263,4 +264,16 @@ void HardwareInterface ::transform_force_torque()
 
   fts_measurements_ = { tcp_force[0], tcp_force[1], tcp_force[2], tcp_torque[0], tcp_torque[1], tcp_torque[2] };
 }
+
+bool HardwareInterface ::isRobotProgramRunning() const
+{
+  return robot_program_running_;
+}
+
+void HardwareInterface ::handleRobotProgramStop(bool program_running)
+{
+  // TODO add ROS publisher to call for controller stop
+  robot_program_running_ = program_running;
+}
+
 }  // namespace ur_driver

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -44,8 +44,12 @@ static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
 static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
 
 ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file,
-                              const std::string& recipe_file)
-  : servoj_time_(0.008), servoj_gain_(2000), servoj_lookahead_time_(0.03)
+                              const std::string& recipe_file, std::function<void(bool)> handle_program_state)
+  : servoj_time_(0.008)
+  , servoj_gain_(2000)
+  , servoj_lookahead_time_(0.03)
+  , reverse_interface_active_(false)
+  , handle_program_state_(handle_program_state)
 {
   LOG_INFO("Initializing RTDE client");
   rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip, notifier_, recipe_file));
@@ -79,9 +83,9 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
   script_sender_.reset(new comm::ScriptSender(script_sender_port, prog));
   script_sender_->start();
   LOG_INFO("Created script sender");
-  reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
 
-  LOG_INFO("Created reverse interface");
+  reverse_port_ = reverse_port;
+  watchdog_thread_ = std::thread(&UrDriver::startWatchdog, this);
 
   rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
   LOG_INFO("Initialization done");
@@ -107,7 +111,7 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
 
 bool UrDriver::writeJointCommand(const vector6d_t& values)
 {
-  if (reverse_interface_)
+  if (reverse_interface_active_)
   {
     reverse_interface_->write(values);
   }
@@ -119,6 +123,34 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
   return true;
 }
 
+void UrDriver::startWatchdog()
+{
+  handle_program_state_(false);
+  reverse_interface_.reset(new comm::ReverseInterface(reverse_port_));
+  reverse_interface_active_ = true;
+  LOG_INFO("Created reverse interface");
+
+  while (true)
+  {
+    handle_program_state_(true);
+    while (reverse_interface_active_ == true)
+    {
+      std::string keepalive = readKeepalive();
+
+      if (keepalive == std::string(""))
+      {
+        reverse_interface_active_ = false;
+      }
+    }
+
+    LOG_INFO("Connection to robot dropped, waiting for new connection.");
+    handle_program_state_(false);
+    reverse_interface_->~ReverseInterface();
+    reverse_interface_.reset(new comm::ReverseInterface(reverse_port_));
+    reverse_interface_active_ = true;
+  }
+}
+
 std::string UrDriver::readScriptFile(const std::string& filename)
 {
   std::ifstream ifs(filename);
@@ -126,4 +158,15 @@ std::string UrDriver::readScriptFile(const std::string& filename)
 
   return content;
 }
+std::string UrDriver::readKeepalive()
+{
+  if (reverse_interface_active_)
+  {
+    return reverse_interface_->readKeepalive();
+  }
+  else
+  {
+    return std::string("");
+  }
+}
 }  // namespace ur_driver