Refactor TCPSocket::setup Timeout and Error Handling

include/ur_client_library/comm/tcp_socket.h

@@ -56,18 +56,15 @@ class TCPSocket
   void setupOptions();
 
 protected:
-  static bool open(socket_t socket_fd, struct sockaddr* address, size_t address_len)
-  {
-    return ::connect(socket_fd, address, static_cast<socklen_t>(address_len)) == 0;
-  }
-
   bool setup(const std::string& host, const int port, const size_t max_num_tries = 0,
-             const std::chrono::milliseconds reconnection_time = DEFAULT_RECONNECTION_TIME);
+             const std::chrono::milliseconds reconnection_time = DEFAULT_RECONNECTION_TIME,
+             const std::chrono::milliseconds timeout = DEFAULT_CONNECTION_TIME);
 
   std::unique_ptr<timeval> recv_timeout_;
 
 public:
   static constexpr std::chrono::milliseconds DEFAULT_RECONNECTION_TIME{ 10000 };
+  static constexpr std::chrono::milliseconds DEFAULT_CONNECTION_TIME{ 500 };
   /*!
    * \brief Creates a TCPSocket object
    */

src/comm/tcp_socket.cpp

@@ -28,6 +28,8 @@
 #ifndef _WIN32
 #  include <arpa/inet.h>
 #  include <netinet/tcp.h>
+#  include <fcntl.h>
+#  include <sys/select.h>
 #endif
 
 #include "ur_client_library/log.h"
@@ -73,7 +75,7 @@ void TCPSocket::setupOptions()
 }
 
 bool TCPSocket::setup(const std::string& host, const int port, const size_t max_num_tries,
-                      const std::chrono::milliseconds reconnection_time)
+                      const std::chrono::milliseconds reconnection_time, const std::chrono::milliseconds timeout)
 {
   // This can be removed once we remove the setReconnectionTime() method
   auto reconnection_time_resolved = reconnection_time;
@@ -111,16 +113,141 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_
       URCL_LOG_ERROR("Failed to get address for %s:%d", host.c_str(), port);
       return false;
     }
-    // loop through the list of addresses untill we find one that's connectable
+
+    std::error_code socket_error = std::make_error_code(std::errc::address_not_available);
+
     for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
     {
       socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
 
-      if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
+      if (socket_fd_ == -1)
+      {
+        socket_error = getLastSocketErrorCode();
+        continue;
+      }
+#ifndef _WIN32
+      if (socket_fd_ >= FD_SETSIZE)
+      {
+        socket_error = std::make_error_code(std::errc::value_too_large);
+        ::ur_close(socket_fd_);
+        socket_fd_ = INVALID_SOCKET;
+        continue;
+      }
+#endif
+
+      bool connect_success = false;
+
+#ifndef _WIN32
+      int flags = ::fcntl(socket_fd_, F_GETFL, 0);
+      if (flags < 0)
+      {
+        socket_error = getLastSocketErrorCode();
+        ::ur_close(socket_fd_);
+        socket_fd_ = INVALID_SOCKET;
+        continue;
+      }
+      ::fcntl(socket_fd_, F_SETFL, flags | O_NONBLOCK);
+#else
+      unsigned long mode = 1;
+      ::ioctlsocket(socket_fd_, FIONBIO, &mode);
+#endif
+
+#ifndef _WIN32
+      int connect_res = ::connect(socket_fd_, p->ai_addr, static_cast<socklen_t>(p->ai_addrlen));
+      bool is_in_progress = (connect_res != 0 && errno == EINPROGRESS);
+#else
+      int connect_res = ::connect(socket_fd_, p->ai_addr, static_cast<int>(p->ai_addrlen));
+      bool is_in_progress = (connect_res != 0 && ::WSAGetLastError() == WSAEWOULDBLOCK);
+#endif
+
+      if (connect_res == 0)
+      {
+        connect_success = true;
+      }
+      else if (is_in_progress)
+      {
+        auto timeout_ms = std::chrono::duration_cast<std::chrono::milliseconds>(timeout).count();
+        struct timeval tv;
+        tv.tv_sec = static_cast<long>(timeout_ms / 1000);
+        tv.tv_usec = static_cast<long>((timeout_ms % 1000) * 1000);
+
+        socket_t local_fd = socket_fd_.load();
+
+        fd_set write_fds;
+        FD_ZERO(&write_fds);
+        FD_SET(local_fd, &write_fds);
+
+        fd_set except_fds;
+        FD_ZERO(&except_fds);
+        FD_SET(local_fd, &except_fds);
+
+        int select_res = ::select(static_cast<int>(local_fd + 1), nullptr, &write_fds, &except_fds, &tv);
+
+        if (select_res > 0)
+        {
+          int so_error = 0;
+#ifndef _WIN32
+          socklen_t len = sizeof(so_error);
+          int opt_res = ::getsockopt(local_fd, SOL_SOCKET, SO_ERROR, &so_error, &len);
+#else
+          int len = sizeof(so_error);
+          int opt_res = ::getsockopt(local_fd, SOL_SOCKET, SO_ERROR, reinterpret_cast<char*>(&so_error), &len);
+#endif
+          if (opt_res != 0)
+          {
+            socket_error = getLastSocketErrorCode();
+          }
+          else if (so_error == 0 && FD_ISSET(local_fd, &write_fds))
+          {
+            connect_success = true;
+          }
+          else
+          {
+            if (so_error != 0)
+            {
+#ifdef _WIN32
+              socket_error = std::error_code(so_error, std::system_category());
+#else
+              socket_error = std::error_code(so_error, std::generic_category());
+#endif
+            }
+            else
+            {
+              socket_error = std::make_error_code(std::errc::connection_refused);
+            }
+          }
+        }
+        else if (select_res == 0)
+        {
+          socket_error = std::make_error_code(std::errc::timed_out);
+        }
+        else
+        {
+          socket_error = getLastSocketErrorCode();
+        }
+      }
+      else
+      {
+        socket_error = getLastSocketErrorCode();
+      }
+
+#ifndef _WIN32
+      ::fcntl(socket_fd_, F_SETFL, flags);
+#else
+      mode = 0;
+      ::ioctlsocket(socket_fd_, FIONBIO, &mode);
+#endif
+
+      if (connect_success)
       {
         connected = true;
         break;
       }
+      else
+      {
+        ::ur_close(socket_fd_);
+        socket_fd_ = INVALID_SOCKET;
+      }
     }
 
     freeaddrinfo(result);
@@ -136,8 +263,8 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_
       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 "
+        ss << "Failed to connect to robot on IP " << host_name << ":" << port << ". Reason: " << socket_error.message()
+           << ". Retrying in "
            << std::chrono::duration_cast<std::chrono::duration<float>>(reconnection_time_resolved).count()
            << " seconds.";
         URCL_LOG_ERROR("%s", ss.str().c_str());