Tcp socket improvements (UniversalRobots#222)

Couple of improvements related to TCP communication:

1. the error handling in `tcp_server::readData` seems wrong since the
error case should be when the number of bytes received is strictly
negative and not equal to 0
2. added early disconnection in `tcp_socket::read` in case of permanent
failure in order to ease failure detection
3. added robustness to `UrDriver::sendScript`: the program sending is
now retried once after closing and reconnecting the secondary stream.
This is the workaround described in
UniversalRobots/Universal_Robots_ROS_Driver#459.
It helps mitigate program sending issues when working with the UR ROS2
driver in headless mode.

New tests added for 2 and 3. For 1 the debug output of
`TCPServerTest::client_connections` seems to confirm the behaviour makes
sense. We now see `client from FD <xx> sent a connection reset package`
which is what I'd expect given the client willingly terminates
connection.

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/comm/tcp_server.cpp

@@ -296,12 +296,13 @@ void TCPServer::readData(const int fd)
   }
   else
   {
-    if (0 < nbytesrecv)
+    if (nbytesrecv < 0)
     {
       if (errno == ECONNRESET)  // if connection gets reset by client, we want to suppress this output
       {
-        URCL_LOG_DEBUG("client from FD %s sent a connection reset package.", fd);
+        URCL_LOG_DEBUG("client from FD %d sent a connection reset package.", fd);
       }
+      else
       {
         URCL_LOG_ERROR("recv() on FD %d failed.", fd);
       }

src/comm/tcp_socket.cpp

@@ -187,7 +187,14 @@ bool TCPSocket::read(uint8_t* buf, const size_t buf_len, size_t& read)
     return false;
   }
   else if (res < 0)
+  {
+    if (!(errno == EAGAIN || errno == EWOULDBLOCK))
+    {
+      // any permanent error should be detected early
+      state_ = SocketState::Disconnected;
+    }
     return false;
+  }
 
   read = static_cast<size_t>(res);
   return true;

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
@@ -556,12 +555,28 @@ bool UrDriver::sendScript(const std::string& program)
   const uint8_t* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
   size_t written;
 
-  if (secondary_stream_->write(data, len, written))
+  const auto send_script_contents = [this, program_with_newline, data, len,
+                                     &written](const std::string&& description) -> bool {
+    if (secondary_stream_->write(data, len, written))
+    {
+      URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str());
+      return true;
+    }
+    const std::string error_message = "Could not send program to robot: " + description;
+    URCL_LOG_ERROR(error_message.c_str());
+    return false;
+  };
+
+  if (send_script_contents("initial attempt"))
   {
-    URCL_LOG_DEBUG("Sent program to robot:\n%s", program_with_newline.c_str());
     return true;
   }
-  URCL_LOG_ERROR("Could not send program to robot");
+
+  if (reconnectSecondaryStream())
+  {
+    return send_script_contents("after reconnecting secondary stream");
+  }
+
   return false;
 }
 
@@ -578,6 +593,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 secondary stream!");
+  return false;
+}
+
 std::vector<std::string> UrDriver::getRTDEOutputRecipe()
 {
   return rtde_client_->getOutputRecipe();

tests/test_tcp_socket.cpp

@@ -360,6 +360,28 @@ TEST_F(TCPSocketTest, test_deprecated_reconnection_time_interface)
   EXPECT_TRUE(client_->setup(2));
 }
 
+TEST_F(TCPSocketTest, test_read_on_socket_abruptly_closed)
+{
+  client_->setup();
+
+  // Make sure the client has connected to the server, before writing to the client
+  EXPECT_TRUE(waitForConnectionCallback());
+
+  std::string send_message = "test message";
+  size_t len = send_message.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(send_message.c_str());
+  size_t written;
+  server_->write(client_fd_, data, len, written);
+
+  // Simulate socket failure
+  close(client_->getSocketFD());
+
+  char characters;
+  size_t read_chars = 0;
+  EXPECT_FALSE(client_->read((uint8_t*)&characters, 1, read_chars));
+  EXPECT_EQ(client_->getState(), comm::SocketState::Disconnected);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);

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[])