diff --git a/include/ur_client_library/comm/tcp_socket.h b/include/ur_client_library/comm/tcp_socket.h index 5a1d468c5..9bda281db 100644 --- a/include/ur_client_library/comm/tcp_socket.h +++ b/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(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 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..cf469661e 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,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(p->ai_addrlen)); + 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 (is_in_progress) + { + auto timeout_ms = std::chrono::duration_cast(timeout).count(); + struct timeval tv; + 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(local_fd, &write_fds); + + fd_set except_fds; + FD_ZERO(&except_fds); + FD_SET(local_fd, &except_fds); + + 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(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(&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>(reconnection_time_resolved).count() << " seconds."; URCL_LOG_ERROR("%s", ss.str().c_str());