Made sendRobotProgram robust to secondary stream disconnected

include/ur_client_library/ur/ur_driver.h

@@ -489,6 +489,14 @@ class UrDriver
 
 private:
   static std::string readScriptFile(const std::string& filename);
+  /*!
+   * \brief Reconnects the secondary stream used to send program to the robot.
+   *
+   * Only for use in headless mode, as it replaces the use of the URCaps program.
+   *
+   * \returns true of on successful reconnection, false otherwise
+   */
+  bool reconnectSecondaryStream();
 
   int rtde_frequency_;
   comm::INotifier notifier_;

src/ur/ur_driver.cpp

@@ -542,9 +542,8 @@ bool UrDriver::sendScript(const std::string& program)
 {
   if (secondary_stream_ == nullptr)
   {
-    throw std::runtime_error("Sending script to robot requested while there is no primary interface established. "
-                             "This "
-                             "should not happen.");
+    throw std::runtime_error("Sending script to robot requested while there is no secondary interface established. "
+                             "This should not happen.");
   }
 
   // urscripts (snippets) must end with a newline, or otherwise the controller's runtime will
@@ -562,6 +561,16 @@ bool UrDriver::sendScript(const std::string& program)
     return true;
   }
   URCL_LOG_ERROR("Could not send program to robot");
+
+  URCL_LOG_INFO("Reconnecting secondary stream to retry sending program...");
+  secondary_stream_->close();
+  if (secondary_stream_->connect() && secondary_stream_->write(data, len, written))
+  {
+    URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str());
+    return true;
+  }
+  URCL_LOG_ERROR("Retry sending program failed!");
+
   return false;
 }
 
@@ -578,6 +587,19 @@ bool UrDriver::sendRobotProgram()
   }
 }
 
+bool UrDriver::reconnectSecondaryStream()
+{
+  URCL_LOG_DEBUG("Closing secondary stream...");
+  secondary_stream_->close();
+  if (secondary_stream_->connect())
+  {
+    URCL_LOG_DEBUG("Secondary stream connected");
+    return true;
+  }
+  URCL_LOG_ERROR("Failed to reconnect secodary stream!");
+  return false;
+}
+
 std::vector<std::string> UrDriver::getRTDEOutputRecipe()
 {
   return rtde_client_->getOutputRecipe();

tests/test_ur_driver.cpp

@@ -359,6 +359,19 @@ TEST_F(UrDriverTest, target_outside_limits_pose)
   waitForProgramNotRunning(1000);
 }
 
+TEST_F(UrDriverTest, send_robot_program_retry_on_failure)
+{
+  // Start robot program
+  g_ur_driver_->sendRobotProgram();
+  EXPECT_TRUE(waitForProgramRunning(1000));
+
+  // Check that sendRobotProgram is robust to the secondary stream being disconnected. This is what happens when
+  // switching from Remote to Local and back to Remote mode for example.
+  g_ur_driver_->secondary_stream_->close();
+
+  EXPECT_TRUE(g_ur_driver_->sendRobotProgram());
+}
+
 // TODO we should add more tests for the UrDriver class.
 
 int main(int argc, char* argv[])