added support for timeout and max_num_tries

Author: michaelaeriksen

include/ur_client_library/comm/pipeline.h

@@ -173,7 +173,7 @@ class IProducer
   /*!
    * \brief Set-up functionality of the producers.
    */
-  virtual void setupProducer()
+  virtual void setupProducer(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10))
   {
   }
   /*!
@@ -287,9 +287,9 @@ class Pipeline
     stop();
   }
 
-  void init()
+  void init(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10))
   {
-    producer_.setupProducer();
+    producer_.setupProducer(max_num_tries, reconnection_time);
     if (consumer_ != nullptr)
       consumer_->setupConsumer();
   }

include/ur_client_library/comm/producer.h

@@ -60,13 +60,14 @@ class URProducer : public IProducer<T>
   /*!
    * \brief Triggers the stream to connect to the robot.
    */
-  void setupProducer() override
+  void setupProducer(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10)) override
   {
+    stream_.setReconnectionTime(reconnection_time);
     timeval tv;
     tv.tv_sec = 1;
     tv.tv_usec = 0;
     stream_.setReceiveTimeout(tv);
-    if (!stream_.connect())
+    if (!stream_.connect(max_num_tries))
     {
       throw UrException("Failed to connect to robot. Please check if the robot is booted and connected.");
     }

include/ur_client_library/comm/stream.h

@@ -58,9 +58,9 @@ class URStream : public TCPSocket
    *
    * \returns True on success, false if connection could not be established
    */
-  bool connect()
+  bool connect(size_t max_num_tries = 0)
   {
-    return TCPSocket::setup(host_, port_);
+    return TCPSocket::setup(host_, port_, max_num_tries);
   }
 
   /*!

include/ur_client_library/comm/tcp_socket.h

@@ -50,7 +50,7 @@ class TCPSocket
 private:
   std::atomic<int> socket_fd_;
   std::atomic<SocketState> state_;
-  std::chrono::seconds reconnection_time_;
+  std::chrono::milliseconds reconnection_time_;
 
 protected:
   virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
@@ -59,7 +59,7 @@ class TCPSocket
   }
   virtual void setOptions(int socket_fd);
 
-  bool setup(std::string& host, int port);
+  bool setup(std::string& host, int port, size_t max_num_tries = 0);
 
   std::unique_ptr<timeval> recv_timeout_;
 
@@ -146,7 +146,7 @@ class TCPSocket
    *
    * \param reconnection_time time in between connection attempts to the server
    */
-  void setReconnectionTime(std::chrono::seconds reconnection_time)
+  void setReconnectionTime(std::chrono::milliseconds reconnection_time)
   {
     reconnection_time_ = reconnection_time;
   }

include/ur_client_library/rtde/rtde_client.h

@@ -125,7 +125,7 @@ class RTDEClient
    *
    * \returns Success of the handshake
    */
-  bool init();
+  bool init(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
   /*!
    * \brief Triggers the robot to start sending RTDE data packages in the negotiated format.
    *
@@ -228,7 +228,7 @@ class RTDEClient
   // the robot is booted.
   std::vector<std::string> ensureTimestampIsPresent(const std::vector<std::string>& output_recipe) const;
 
-  void setupCommunication();
+  void setupCommunication(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
   bool negotiateProtocolVersion(const uint16_t protocol_version);
   void queryURControlVersion();
   void setupOutputs(const uint16_t protocol_version);

include/ur_client_library/ur/dashboard_client.h

@@ -55,15 +55,15 @@ class DashboardClient : public comm::TCPSocket
   DashboardClient() = delete;
   virtual ~DashboardClient() = default;
 
-  const int DASHBOARD_SERVER_PORT = 29999;
+  static constexpr int DASHBOARD_SERVER_PORT = 29999;
 
   /*!
    * \brief Opens a connection to the dashboard server on the host as specified in the constructor.
    *
    * \returns True on successful connection, false otherwise.
    */
-  bool connect();
-
+  bool connect(size_t max_num_tries = 0, std::chrono::milliseconds reconnection_time = std::chrono::seconds(10));
+  
   /*!
    * \brief Makes sure no connection to the dashboard server is held inside the object.
    */

src/comm/tcp_socket.cpp

@@ -55,7 +55,7 @@ void TCPSocket::setOptions(int socket_fd)
   }
 }
 
-bool TCPSocket::setup(std::string& host, int port)
+bool TCPSocket::setup(std::string& host, int port, size_t max_num_tries)
 {
   if (state_ == SocketState::Connected)
     return false;
@@ -74,6 +74,7 @@ bool TCPSocket::setup(std::string& host, int port)
   hints.ai_socktype = SOCK_STREAM;
   hints.ai_flags = AI_PASSIVE;
 
+  size_t connect_counter = 0;
   bool connected = false;
   while (!connected)
   {
@@ -96,13 +97,23 @@ bool TCPSocket::setup(std::string& host, int port)
 
     freeaddrinfo(result);
 
+    if (max_num_tries > 0)
+    {
+      if (connect_counter++ >= max_num_tries)
+      {
+        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 "
-         << reconnection_time_.count() << " seconds";
+         << std::chrono::duration_cast<std::chrono::duration<float>>(reconnection_time_).count() << " seconds";
       URCL_LOG_ERROR("%s", ss.str().c_str());
       std::this_thread::sleep_for(reconnection_time_);
     }

src/rtde/rtde_client.cpp

@@ -69,7 +69,7 @@ RTDEClient::~RTDEClient()
   disconnect();
 }
 
-bool RTDEClient::init()
+bool RTDEClient::init(size_t max_num_tries, std::chrono::milliseconds reconnection_time)
 {
   if (client_state_ > ClientState::UNINITIALIZED)
   {
@@ -79,7 +79,7 @@ bool RTDEClient::init()
   unsigned int attempts = 0;
   while (attempts < MAX_INITIALIZE_ATTEMPTS)
   {
-    setupCommunication();
+    setupCommunication(max_num_tries, reconnection_time);
     if (client_state_ == ClientState::INITIALIZED)
       return true;
 
@@ -92,11 +92,11 @@ bool RTDEClient::init()
   throw UrException(ss.str());
 }
 
-void RTDEClient::setupCommunication()
+void RTDEClient::setupCommunication(size_t max_num_tries, std::chrono::milliseconds reconnection_time)
 {
   client_state_ = ClientState::INITIALIZING;
   // A running pipeline is needed inside setup
-  pipeline_.init();
+  pipeline_.init(max_num_tries, reconnection_time);
   pipeline_.run();
 
   uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION;

src/ur/dashboard_client.cpp

@@ -47,7 +47,7 @@ void DashboardClient::rtrim(std::string& str, const std::string& chars)
   str.erase(str.find_last_not_of(chars) + 1);
 }
 
-bool DashboardClient::connect()
+bool DashboardClient::connect(size_t max_num_tries, std::chrono::milliseconds reconnection_time)
 {
   if (getState() == comm::SocketState::Connected)
   {
@@ -61,17 +61,22 @@ bool DashboardClient::connect()
 
   while (not ret_val)
   {
+    TCPSocket::setReconnectionTime(reconnection_time);
     // The first read after connection can take more time.
     tv.tv_sec = 10;
     tv.tv_usec = 0;
     TCPSocket::setReceiveTimeout(tv);
     try
     {
-      if (TCPSocket::setup(host_, port_))
+      if (TCPSocket::setup(host_, port_, max_num_tries))
       {
         URCL_LOG_INFO("%s", read().c_str());
         ret_val = true;
       }
+      else
+      {
+        return false;
+      }
     }
     catch (const TimeoutException&)
     {