From 14b9dd4f1b0e04f3087879163600f4f21329b0c7 Mon Sep 17 00:00:00 2001
From: Felix Mauch <mauch@fzi.de>
Date: Wed, 26 Jun 2019 08:48:24 +0200
Subject: [PATCH] send keepalive signal when no (commanding) controller is
 active

Otherwise the hardware disconnects as soon as we stop a controller by hand.
---
 .../include/ur_rtde_driver/ur/ur_driver.h      | 18 ++++++++++++++++++
 ur_rtde_driver/src/ros/hardware_interface.cpp  |  4 ++++
 ur_rtde_driver/src/ur/ur_driver.cpp            | 17 +++++++++++------
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
index a376286..ce29f37 100644
--- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
+++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
@@ -81,8 +81,26 @@ public:
     return rtde_frequency_;
   }
 
+  /*!
+   * \brief Writes a joint command together with a keepalive signal onto the socket being sent to
+   * the robot.
+   *
+   * \param values Desired joint positions
+   *
+   * \returns True on successful write.
+   */
   bool writeJointCommand(const vector6d_t& values);
 
+  /*!
+   * \brief Write a keepalive signal only.
+   *
+   * This signals the robot that the connection is still
+   * active in times when no commands are to be sent (e.g. no controller is active.)
+   *
+   * \returns True on successful write.
+   */
+  bool writeKeepalive();
+
   void startWatchdog();
 
 private:
diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp
index c78cc03..1384455 100644
--- a/ur_rtde_driver/src/ros/hardware_interface.cpp
+++ b/ur_rtde_driver/src/ros/hardware_interface.cpp
@@ -292,6 +292,10 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio
   {
     ur_driver_->writeJointCommand(joint_position_command_);
   }
+  else
+  {
+    ur_driver_->writeKeepalive();
+  }
 }
 
 bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp
index 479fe09..d8df01b 100644
--- a/ur_rtde_driver/src/ur/ur_driver.cpp
+++ b/ur_rtde_driver/src/ur/ur_driver.cpp
@@ -138,14 +138,19 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
 {
   if (reverse_interface_active_)
   {
-    reverse_interface_->write(values);
-  }
-  else
-  {
-    return false;
+    return reverse_interface_->write(&values);
   }
+  return false;
+}
 
-  return true;
+bool UrDriver::writeKeepalive()
+{
+  if (reverse_interface_active_)
+  {
+    vector6d_t* fake = nullptr;
+    return reverse_interface_->write(fake, 1);
+  }
+  return false;
 }
 
 void UrDriver::startWatchdog()