Parametrize reconnection time for UrDriver (#266)

It would be beneficial to allow reconfiguring the
connection attempts and timeout of the UrDriver / RTDEClient.

---------

Co-authored-by: Dominic Reber <[email protected]>

Author: urfeex

include/ur_client_library/primary/primary_client.h

@@ -66,7 +66,8 @@ class PrimaryClient
    * \param primary_consumer Primary consumer that should be removed from the list
    */
   void removePrimaryConsumer(std::shared_ptr<comm::IConsumer<PrimaryPackage>> primary_consumer);
-  void start();
+  void start(const size_t max_connection_attempts = 0,
+             const std::chrono::milliseconds reconnection_timeout = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME);
 
   /*!
    * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be

include/ur_client_library/rtde/rtde_client.h

@@ -53,7 +53,6 @@ namespace rtde_interface
 {
 static const uint16_t MAX_RTDE_PROTOCOL_VERSION = 2;
 static const unsigned MAX_REQUEST_RETRIES = 5;
-static const unsigned MAX_INITIALIZE_ATTEMPTS = 10;
 
 enum class UrRtdeRobotStatusBits
 {
@@ -131,14 +130,21 @@ class RTDEClient
    * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
    * used protocol version and setting of input and output recipes.
    *
-   * \param max_num_tries Maximum number of connection attempts before counting the connection as
+   * \param max_connection_attempts Maximum number of (socket) connection attempts before counting the connection as
    * failed. Unlimited number of attempts when set to 0.
-   * \param reconnection_time time in between connection attempts to the server
+   * \param reconnection_timeout Time in between connection attempts to the socket
+   * \param max_initialization_attempts Maximum number of initialization attempts before counting
+   * the initialization as failed. Initialization can
+   * fail given an established socket connection e.g. when the connected socket does not implement
+   * an RTDE interface.
+   * \param initialization_timeout Time in between initialization attempts of the RTDE interface
    *
    * \returns Success of the handshake
    */
-  bool init(const size_t max_num_tries = 0,
-            const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
+  bool init(const size_t max_connection_attempts = 0,
+            const std::chrono::milliseconds reconnection_timeout = comm::TCPSocket::DEFAULT_RECONNECTION_TIME,
+            const size_t max_initialization_attempts = 3,
+            const std::chrono::milliseconds initialization_timeout = std::chrono::seconds(1));
   /*!
    * \brief Triggers the robot to start sending RTDE data packages in the negotiated format.
    *

include/ur_client_library/ur/ur_driver.h

@@ -28,6 +28,7 @@
 #ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
 #define UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED
 
+#include <chrono>
 #include <functional>
 
 #include "ur_client_library/rtde/rtde_client.h"
@@ -107,6 +108,30 @@ struct UrDriverConfiguration
    */
   double servoj_lookahead_time = 0.03;
 
+  /*!
+   * \brief Number of attempts to reconnect to sockets such as the primary or RTDE interface.
+   *
+   * If set to 0, the driver will try to reconnect indefinitely.
+   */
+  size_t socket_reconnect_attempts = 0;
+
+  /*!
+   * \brief Time in between connection attempts to sockets such as the primary or RTDE interface.
+   */
+  std::chrono::milliseconds socket_reconnection_timeout = std::chrono::seconds(10);
+
+  /*!
+   * \brief Number of attempts to initialize (given a successful socket connection) the RTDE interface.
+   *
+   * If set to 0, the driver will try to initialize the RTDE interface indefinitely.
+   */
+  size_t rtde_initialization_attempts_ = 3;
+
+  /*!
+   * \brief Time in between initialization attempts of the RTDE interface.
+   */
+  std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::seconds(5);
+
   bool non_blocking_read = false;
 
   // TODO: Remove on 2027-05
@@ -873,6 +898,12 @@ class UrDriver
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> primary_stream_;
   std::unique_ptr<comm::URStream<primary_interface::PrimaryPackage>> secondary_stream_;
 
+  size_t socket_connection_attempts_ = 0;
+  std::chrono::milliseconds socket_reconnection_timeout_ = std::chrono::milliseconds(10000);
+
+  size_t rtde_initialization_attempts_ = 0;
+  std::chrono::milliseconds rtde_initialization_timeout_ = std::chrono::milliseconds(10000);
+
   double force_mode_gain_scale_factor_ = 0.5;
   double force_mode_damping_factor_ = 0.025;
 

src/comm/tcp_socket.cpp

@@ -109,25 +109,24 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_
 
     freeaddrinfo(result);
 
-    if (max_num_tries > 0)
+    if (!connected)
     {
-      if (connect_counter++ >= max_num_tries)
+      state_ = SocketState::Invalid;
+      if (++connect_counter >= max_num_tries && max_num_tries > 0)
       {
         URCL_LOG_ERROR("Failed to establish connection for %s:%d after %d tries", host.c_str(), port, max_num_tries);
-        state_ = SocketState::Invalid;
         return false;
       }
-    }
-
-    if (!connected)
-    {
-      state_ = SocketState::Invalid;
-      std::stringstream ss;
-      ss << "Failed to connect to robot on IP " << host_name
-         << ". Please check that the robot is booted and reachable on " << host_name << ". Retrying in "
-         << std::chrono::duration_cast<std::chrono::duration<float>>(reconnection_time_resolved).count() << " seconds";
-      URCL_LOG_ERROR("%s", ss.str().c_str());
-      std::this_thread::sleep_for(reconnection_time_resolved);
+      else
+      {
+        std::stringstream ss;
+        ss << "Failed to connect to robot on IP " << host_name << ":" << port
+           << ". Please check that the robot is booted and reachable on " << host_name << ". Retrying in "
+           << std::chrono::duration_cast<std::chrono::duration<float>>(reconnection_time_resolved).count()
+           << " seconds.";
+        URCL_LOG_ERROR("%s", ss.str().c_str());
+        std::this_thread::sleep_for(reconnection_time_resolved);
+      }
     }
   }
   setupOptions();

src/primary/primary_client.cpp

@@ -60,10 +60,10 @@ PrimaryClient::~PrimaryClient()
   pipeline_->stop();
 }
 
-void PrimaryClient::start()
+void PrimaryClient::start(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time)
 {
   URCL_LOG_INFO("Starting primary client pipeline");
-  pipeline_->init();
+  pipeline_->init(max_num_tries, reconnection_time);
   pipeline_->run();
 }
 

src/rtde/rtde_client.cpp

@@ -28,7 +28,9 @@
 
 #include "ur_client_library/rtde/rtde_client.h"
 #include "ur_client_library/exceptions.h"
+#include "ur_client_library/log.h"
 #include <algorithm>
+#include <chrono>
 
 namespace urcl
 {
@@ -74,26 +76,34 @@ RTDEClient::~RTDEClient()
   disconnect();
 }
 
-bool RTDEClient::init(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time)
+bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::milliseconds reconnection_timeout,
+                      const size_t max_initialization_attempts, const std::chrono::milliseconds initialization_timeout)
 {
+  if (max_initialization_attempts <= 0)
+  {
+    throw UrException("The number of initialization attempts has to be greater than 0.");
+  }
+
   if (client_state_ > ClientState::UNINITIALIZED)
   {
     return true;
   }
 
   unsigned int attempts = 0;
-  while (attempts < MAX_INITIALIZE_ATTEMPTS)
+  while (attempts < max_initialization_attempts)
   {
-    setupCommunication(max_num_tries, reconnection_time);
+    setupCommunication(max_connection_attempts, reconnection_timeout);
     if (client_state_ == ClientState::INITIALIZED)
       return true;
 
-    URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds");
-    std::this_thread::sleep_for(std::chrono::seconds(10));
-    attempts++;
+    if (++attempts < max_initialization_attempts)
+    {
+      URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000);
+      std::this_thread::sleep_for(initialization_timeout);
+    }
   }
   std::stringstream ss;
-  ss << "Failed to initialize RTDE client after " << MAX_INITIALIZE_ATTEMPTS << " attempts";
+  ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts";
   throw UrException(ss.str());
 }
 
@@ -241,7 +251,7 @@ void RTDEClient::queryURControlVersion()
     else
     {
       std::stringstream ss;
-      ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl
+      ss << "Did not receive URControl version from robot. Message received instead: " << std::endl
          << package->toString() << ". Retrying...";
       num_retries++;
       URCL_LOG_WARN("%s", ss.str().c_str());

src/ur/ur_driver.cpp

@@ -60,6 +60,10 @@ void UrDriver::init(const UrDriverConfiguration& config)
   servoj_lookahead_time_ = config.servoj_lookahead_time;
   handle_program_state_ = config.handle_program_state;
   in_headless_mode_ = config.headless_mode;
+  socket_connection_attempts_ = config.socket_reconnect_attempts;
+  socket_reconnection_timeout_ = config.socket_reconnection_timeout;
+  rtde_initialization_attempts_ = config.rtde_initialization_attempts_;
+  rtde_initialization_timeout_ = config.rtde_initialization_timeout_;
 
   URCL_LOG_DEBUG("Initializing urdriver");
   URCL_LOG_DEBUG("Initializing RTDE client");
@@ -70,7 +74,7 @@ void UrDriver::init(const UrDriverConfiguration& config)
       new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT));
   secondary_stream_.reset(
       new comm::URStream<primary_interface::PrimaryPackage>(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT));
-  secondary_stream_->connect();
+  secondary_stream_->connect(socket_connection_attempts_, socket_reconnection_timeout_);
 
   primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_));
 
@@ -691,7 +695,8 @@ void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const
 
 void UrDriver::initRTDE()
 {
-  if (!rtde_client_->init())
+  if (!rtde_client_->init(socket_connection_attempts_, socket_reconnection_timeout_, rtde_initialization_attempts_,
+                          rtde_initialization_timeout_))
   {
     throw UrException("Initialization of RTDE client went wrong.");
   }
@@ -706,7 +711,7 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port)
 
 void UrDriver::startPrimaryClientCommunication()
 {
-  primary_client_->start();
+  primary_client_->start(socket_connection_attempts_, socket_reconnection_timeout_);
 }
 
 std::deque<urcl::primary_interface::ErrorCode> UrDriver::getErrorCodes()

tests/test_ur_driver.cpp

@@ -32,6 +32,7 @@
 
 #include <ur_client_library/ur/dashboard_client.h>
 #include <thread>
+#include "ur_client_library/log.h"
 #define private public
 #include <ur_client_library/ur/ur_driver.h>
 #include <ur_client_library/example_robot_wrapper.h>
@@ -268,6 +269,19 @@ TEST_F(UrDriverTest, read_error_code)
   EXPECT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop());
 }
 
+TEST(UrDriverInitTest, setting_connection_limits_works_correctly)
+{
+  UrDriverConfiguration config;
+  config.socket_reconnect_attempts = 1;
+  config.socket_reconnection_timeout = std::chrono::milliseconds(200);
+  config.robot_ip = "192.168.56.100";  // That IP address should not exist on the test network
+  config.input_recipe_file = INPUT_RECIPE;
+  config.output_recipe_file = OUTPUT_RECIPE;
+  config.headless_mode = g_HEADLESS;
+
+  EXPECT_THROW(UrDriver ur_driver(config), UrException);
+}
+
 // TODO we should add more tests for the UrDriver class.
 
 int main(int argc, char* argv[])