From ad2467e59574cc31470b203b7af644c108fd9db4 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 20 May 2026 08:12:08 +0200 Subject: [PATCH 1/6] refactor: TCPSocket::setup to support timeouts and improve error handling --- include/ur_client_library/comm/tcp_socket.h | 4 +- src/comm/tcp_socket.cpp | 92 +++++++++++++++++++-- 2 files changed, 90 insertions(+), 6 deletions(-) diff --git a/include/ur_client_library/comm/tcp_socket.h b/include/ur_client_library/comm/tcp_socket.h index 5a1d468c5..62a152427 100644 --- a/include/ur_client_library/comm/tcp_socket.h +++ b/include/ur_client_library/comm/tcp_socket.h @@ -62,12 +62,14 @@ class TCPSocket } 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 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 */ diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 6af840e0d..25c5bdb5a 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -28,6 +28,8 @@ #ifndef _WIN32 # include # include +# include +# include #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,96 @@ 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; + 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 + int flags = ::fcntl(socket_fd_, F_GETFL, 0); + ::fcntl(socket_fd_, F_SETFL, flags | O_NONBLOCK); + + int connect_res = ::connect(socket_fd_, p->ai_addr, static_cast(p->ai_addrlen)); + bool connect_success = false; + + if (connect_res == 0) + { + connect_success = true; + } + else if (errno == EINPROGRESS) + { + auto timeout_ms = std::chrono::duration_cast(timeout).count(); + struct timeval tv; + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000; + + fd_set write_fds; + FD_ZERO(&write_fds); + FD_SET(socket_fd_, &write_fds); + + int select_res = ::select(socket_fd_ + 1, nullptr, &write_fds, nullptr, &tv); + + if (select_res > 0) + { + int so_error = 0; + socklen_t len = sizeof(so_error); + ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, &so_error, &len); + if (so_error == 0) + { + connect_success = true; + } + else + { + socket_error = std::error_code(so_error, std::generic_category()); + } + } + else if (select_res == 0) + { + socket_error = std::make_error_code(std::errc::timed_out); + } + else + { + socket_error = getLastSocketErrorCode(); + } + } + else + { + socket_error = getLastSocketErrorCode(); + } + + ::fcntl(socket_fd_, F_SETFL, flags); + + if (connect_success) + { + connected = true; + break; + } + else + { + ::ur_close(socket_fd_); + socket_fd_ = INVALID_SOCKET; + } +#else + if (open(socket_fd_, p->ai_addr, p->ai_addrlen)) { connected = true; break; } + else + { + socket_error = getLastSocketErrorCode(); + ::ur_close(socket_fd_); + socket_fd_ = INVALID_SOCKET; + } +#endif } freeaddrinfo(result); @@ -136,8 +218,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>(reconnection_time_resolved).count() << " seconds."; URCL_LOG_ERROR("%s", ss.str().c_str()); From 4ab5c0716ac385edcfd407195adf1111378a5062 Mon Sep 17 00:00:00 2001 From: srvald Date: Wed, 20 May 2026 11:14:05 +0200 Subject: [PATCH 2/6] refactor: add Windows timeout compatibility and fcntl flags checking --- src/comm/tcp_socket.cpp | 51 ++++++++++++++++++++++++++--------------- 1 file changed, 33 insertions(+), 18 deletions(-) diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 25c5bdb5a..74b69e3b7 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -126,35 +126,58 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ continue; } + 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(p->ai_addrlen)); - bool connect_success = false; + bool is_in_progress = (connect_res != 0 && errno == EINPROGRESS); +#else + int connect_res = ::connect(socket_fd_, p->ai_addr, static_cast(p->ai_addrlen)); + bool is_in_progress = (connect_res != 0 && ::WSAGetLastError() == WSAEWOULDBLOCK); +#endif if (connect_res == 0) { connect_success = true; } - else if (errno == EINPROGRESS) + else if (is_in_progress) { auto timeout_ms = std::chrono::duration_cast(timeout).count(); struct timeval tv; - tv.tv_sec = timeout_ms / 1000; - tv.tv_usec = (timeout_ms % 1000) * 1000; + tv.tv_sec = static_cast(timeout_ms / 1000); + tv.tv_usec = static_cast((timeout_ms % 1000) * 1000); fd_set write_fds; FD_ZERO(&write_fds); FD_SET(socket_fd_, &write_fds); - int select_res = ::select(socket_fd_ + 1, nullptr, &write_fds, nullptr, &tv); + int select_res = ::select(static_cast(socket_fd_ + 1), nullptr, &write_fds, nullptr, &tv); if (select_res > 0) { int so_error = 0; +#ifndef _WIN32 socklen_t len = sizeof(so_error); ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, &so_error, &len); +#else + int len = sizeof(so_error); + ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, reinterpret_cast(&so_error), &len); +#endif if (so_error == 0) { connect_success = true; @@ -178,7 +201,12 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ socket_error = getLastSocketErrorCode(); } +#ifndef _WIN32 ::fcntl(socket_fd_, F_SETFL, flags); +#else + mode = 0; + ::ioctlsocket(socket_fd_, FIONBIO, &mode); +#endif if (connect_success) { @@ -190,19 +218,6 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ ::ur_close(socket_fd_); socket_fd_ = INVALID_SOCKET; } -#else - if (open(socket_fd_, p->ai_addr, p->ai_addrlen)) - { - connected = true; - break; - } - else - { - socket_error = getLastSocketErrorCode(); - ::ur_close(socket_fd_); - socket_fd_ = INVALID_SOCKET; - } -#endif } freeaddrinfo(result); From aa09e5d52a9e5f95234f30bc777c931aae010a0f Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 21 May 2026 08:39:18 +0200 Subject: [PATCH 3/6] fix: Windows async connection issues, error reporting, and resource leaks --- src/comm/tcp_socket.cpp | 43 +++++++++++++++++++++++++++++++++++------ 1 file changed, 37 insertions(+), 6 deletions(-) diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 74b69e3b7..f1ee42325 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -50,6 +50,9 @@ TCPSocket::TCPSocket() TCPSocket::~TCPSocket() { close(); +#ifdef _WIN32 + ::WSACleanup(); +#endif } void TCPSocket::setupOptions() @@ -114,7 +117,7 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ return false; } - std::error_code socket_error; + 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) { @@ -125,6 +128,15 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ 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; @@ -166,25 +178,44 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ FD_ZERO(&write_fds); FD_SET(socket_fd_, &write_fds); - int select_res = ::select(static_cast(socket_fd_ + 1), nullptr, &write_fds, nullptr, &tv); + fd_set except_fds; + FD_ZERO(&except_fds); + FD_SET(socket_fd_, &except_fds); + + int select_res = ::select(static_cast(socket_fd_ + 1), nullptr, &write_fds, &except_fds, &tv); if (select_res > 0) { int so_error = 0; #ifndef _WIN32 socklen_t len = sizeof(so_error); - ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, &so_error, &len); + int opt_res = ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, &so_error, &len); #else int len = sizeof(so_error); - ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, reinterpret_cast(&so_error), &len); + int opt_res = ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, reinterpret_cast(&so_error), &len); #endif - if (so_error == 0) + if (opt_res != 0) + { + socket_error = getLastSocketErrorCode(); + } + else if (so_error == 0 && FD_ISSET(socket_fd_, &write_fds)) { connect_success = true; } else { - socket_error = std::error_code(so_error, std::generic_category()); + 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) From 1fb52cd45611434bd1dc93a02c7d65a178037628 Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 21 May 2026 08:53:20 +0200 Subject: [PATCH 4/6] refactor: remove unused protected status open() method --- include/ur_client_library/comm/tcp_socket.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/include/ur_client_library/comm/tcp_socket.h b/include/ur_client_library/comm/tcp_socket.h index 62a152427..9bda281db 100644 --- a/include/ur_client_library/comm/tcp_socket.h +++ b/include/ur_client_library/comm/tcp_socket.h @@ -56,11 +56,6 @@ 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(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 timeout = DEFAULT_CONNECTION_TIME); From 6618266ca1fa713299cea998eee61063e23d627f Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 21 May 2026 09:34:11 +0200 Subject: [PATCH 5/6] fix: remove WSACleanup from TCPSocket destructor --- src/comm/tcp_socket.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index f1ee42325..22ff4d582 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -50,9 +50,6 @@ TCPSocket::TCPSocket() TCPSocket::~TCPSocket() { close(); -#ifdef _WIN32 - ::WSACleanup(); -#endif } void TCPSocket::setupOptions() From 6b46158a2b92273be8bc6d3f667c90cf04e4f30c Mon Sep 17 00:00:00 2001 From: srvald Date: Thu, 21 May 2026 10:09:11 +0200 Subject: [PATCH 6/6] fix: use local copy of atomic socket_fd in FD_SET macros --- src/comm/tcp_socket.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 22ff4d582..cf469661e 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -171,31 +171,33 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ tv.tv_sec = static_cast(timeout_ms / 1000); tv.tv_usec = static_cast((timeout_ms % 1000) * 1000); + socket_t local_fd = socket_fd_.load(); + fd_set write_fds; FD_ZERO(&write_fds); - FD_SET(socket_fd_, &write_fds); + FD_SET(local_fd, &write_fds); fd_set except_fds; FD_ZERO(&except_fds); - FD_SET(socket_fd_, &except_fds); + FD_SET(local_fd, &except_fds); - int select_res = ::select(static_cast(socket_fd_ + 1), nullptr, &write_fds, &except_fds, &tv); + int select_res = ::select(static_cast(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(socket_fd_, SOL_SOCKET, SO_ERROR, &so_error, &len); + int opt_res = ::getsockopt(local_fd, SOL_SOCKET, SO_ERROR, &so_error, &len); #else int len = sizeof(so_error); - int opt_res = ::getsockopt(socket_fd_, SOL_SOCKET, SO_ERROR, reinterpret_cast(&so_error), &len); + int opt_res = ::getsockopt(local_fd, SOL_SOCKET, SO_ERROR, reinterpret_cast(&so_error), &len); #endif if (opt_res != 0) { socket_error = getLastSocketErrorCode(); } - else if (so_error == 0 && FD_ISSET(socket_fd_, &write_fds)) + else if (so_error == 0 && FD_ISSET(local_fd, &write_fds)) { connect_success = true; }