Allow configuring the connection and initialization attempts for the UrDriver's RTDE client

Author: urfeex

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/rtde/rtde_client.cpp

@@ -76,28 +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;
 
-    if (++attempts < MAX_INITIALIZE_ATTEMPTS)
+    if (++attempts < max_initialization_attempts)
     {
       URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds");
-      std::this_thread::sleep_for(std::chrono::seconds(10));
+      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());
 }
 

src/ur/ur_driver.cpp

@@ -60,6 +60,8 @@ 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;
 
   URCL_LOG_DEBUG("Initializing urdriver");
   URCL_LOG_DEBUG("Initializing RTDE client");
@@ -691,7 +693,7 @@ 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_))
   {
     throw UrException("Initialization of RTDE client went wrong.");
   }