From d3e91d96fb9fa087dafcef668f834fe87ad75a73 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 11 Feb 2025 12:02:14 +0100 Subject: [PATCH 1/8] Do not wait after last attempt to establish a connection --- src/comm/tcp_socket.cpp | 27 +++++++++++++-------------- src/rtde/rtde_client.cpp | 12 ++++++++---- 2 files changed, 21 insertions(+), 18 deletions(-) diff --git a/src/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 8803664a8..5f56bdb8a 100644 --- a/src/comm/tcp_socket.cpp +++ b/src/comm/tcp_socket.cpp @@ -109,25 +109,24 @@ bool TCPSocket::setup(const std::string& host, const int port, const size_t max_ freeaddrinfo(result); - if (max_num_tries > 0) + if (!connected) { - if (connect_counter++ >= max_num_tries) + state_ = SocketState::Invalid; + if (++connect_counter >= max_num_tries && max_num_tries > 0) { 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 " - << std::chrono::duration_cast>(reconnection_time_resolved).count() << " seconds"; - URCL_LOG_ERROR("%s", ss.str().c_str()); - std::this_thread::sleep_for(reconnection_time_resolved); + 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 " + << std::chrono::duration_cast>(reconnection_time_resolved).count() + << " seconds."; + URCL_LOG_ERROR("%s", ss.str().c_str()); + std::this_thread::sleep_for(reconnection_time_resolved); + } } } setupOptions(); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 602088724..4391c9c1d 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -28,7 +28,9 @@ #include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/exceptions.h" +#include "ur_client_library/log.h" #include +#include namespace urcl { @@ -88,9 +90,11 @@ bool RTDEClient::init(const size_t max_num_tries, const std::chrono::millisecond if (client_state_ == ClientState::INITIALIZED) return true; - URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds"); - std::this_thread::sleep_for(std::chrono::seconds(10)); - attempts++; + if (++attempts < MAX_INITIALIZE_ATTEMPTS) + { + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds"); + std::this_thread::sleep_for(std::chrono::seconds(10)); + } } std::stringstream ss; ss << "Failed to initialize RTDE client after " << MAX_INITIALIZE_ATTEMPTS << " attempts"; @@ -241,7 +245,7 @@ void RTDEClient::queryURControlVersion() else { std::stringstream ss; - ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl + ss << "Did not receive URControl version from robot. Message received instead: " << std::endl << package->toString() << ". Retrying..."; num_retries++; URCL_LOG_WARN("%s", ss.str().c_str()); From d57df329a2fa7fdbcc4540e8379b7485ae745d7e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 11 Feb 2025 13:07:58 +0100 Subject: [PATCH 2/8] Allow configuring the connection and initialization attempts for the UrDriver's RTDE client --- include/ur_client_library/rtde/rtde_client.h | 16 ++++++---- include/ur_client_library/ur/ur_driver.h | 31 ++++++++++++++++++++ src/rtde/rtde_client.cpp | 18 ++++++++---- src/ur/ur_driver.cpp | 4 ++- 4 files changed, 57 insertions(+), 12 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 17dfd0fae..d94faa5d1 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/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. * diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 73048fc91..56dbc6175 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/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 #include #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> primary_stream_; std::unique_ptr> 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; diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 4391c9c1d..316f7c344 100644 --- a/src/rtde/rtde_client.cpp +++ b/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()); } diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index f200b0308..cfe4b8040 100644 --- a/src/ur/ur_driver.cpp +++ b/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."); } From b22c4edb77a51bf5b37371805918d14643766b19 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 11 Feb 2025 13:08:23 +0100 Subject: [PATCH 3/8] Use socket connection parameters also for primary client --- include/ur_client_library/primary/primary_client.h | 3 ++- src/primary/primary_client.cpp | 4 ++-- src/ur/ur_driver.cpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index d7fd7d58c..e90823d4a 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -66,7 +66,8 @@ class PrimaryClient * \param primary_consumer Primary consumer that should be removed from the list */ void removePrimaryConsumer(std::shared_ptr> primary_consumer); - void start(); + void start(const size_t max_num_tries = 0, + const std::chrono::milliseconds reconnection_time = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME); /*! * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be diff --git a/src/primary/primary_client.cpp b/src/primary/primary_client.cpp index 38ad47277..54b4872e2 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -60,10 +60,10 @@ PrimaryClient::~PrimaryClient() pipeline_->stop(); } -void PrimaryClient::start() +void PrimaryClient::start(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) { URCL_LOG_INFO("Starting primary client pipeline"); - pipeline_->init(); + pipeline_->init(max_num_tries, reconnection_time); pipeline_->run(); } diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index cfe4b8040..b97ce71ef 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -708,7 +708,7 @@ void UrDriver::setupReverseInterface(const uint32_t reverse_port) void UrDriver::startPrimaryClientCommunication() { - primary_client_->start(); + primary_client_->start(socket_connection_attempts_, socket_reconnection_timeout_); } std::deque UrDriver::getErrorCodes() From 69823df454652a49fdffb38a6b435c7d8dd938cd Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 12 Feb 2025 15:57:00 +0100 Subject: [PATCH 4/8] Use socket connection restrictions also for secondary stream With the current implementation this will be connected before the RTDE connection. --- src/ur/ur_driver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index b97ce71ef..ed4ddf48b 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -72,7 +72,7 @@ void UrDriver::init(const UrDriverConfiguration& config) new comm::URStream(robot_ip_, urcl::primary_interface::UR_PRIMARY_PORT)); secondary_stream_.reset( new comm::URStream(robot_ip_, urcl::primary_interface::UR_SECONDARY_PORT)); - secondary_stream_->connect(); + secondary_stream_->connect(socket_connection_attempts_, socket_reconnection_timeout_); primary_client_.reset(new urcl::primary_interface::PrimaryClient(robot_ip_, notifier_)); From 03d3a7f1652fbba5e32860bfca26b8ae62e3eb50 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 12 Feb 2025 15:57:36 +0100 Subject: [PATCH 5/8] Add integration tests for setting connection attempts and timeout --- tests/test_ur_driver.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 3e8c24fe1..6b7eae43a 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -32,6 +32,7 @@ #include #include +#include "ur_client_library/log.h" #define private public #include #include @@ -268,6 +269,19 @@ TEST_F(UrDriverTest, read_error_code) EXPECT_TRUE(g_my_robot->dashboard_client_->commandUnlockProtectiveStop()); } +TEST(UrDriverInitTest, setting_connection_limits_works_correctly) +{ + UrDriverConfiguration config; + config.socket_reconnect_attempts = 1; + config.socket_reconnection_timeout = std::chrono::milliseconds(200); + config.robot_ip = "192.168.56.100"; // That IP address should not exist on the test network + config.input_recipe_file = INPUT_RECIPE; + config.output_recipe_file = OUTPUT_RECIPE; + config.headless_mode = g_HEADLESS; + + EXPECT_THROW(UrDriver ur_driver(config), UrException); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[]) From 1bf825c7ecf94331577fb86ecc157e61ccd03dee Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 13 Feb 2025 10:53:50 +0100 Subject: [PATCH 6/8] Update include/ur_client_library/primary/primary_client.h Co-authored-by: Dominic Reber <71256590+domire8@users.noreply.github.com> --- include/ur_client_library/primary/primary_client.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index e90823d4a..d8a1bf153 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -66,8 +66,8 @@ class PrimaryClient * \param primary_consumer Primary consumer that should be removed from the list */ void removePrimaryConsumer(std::shared_ptr> primary_consumer); - void start(const size_t max_num_tries = 0, - const std::chrono::milliseconds reconnection_time = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME); + void start(const size_t max_connection_attempts = 0, + const std::chrono::milliseconds reconnection_timeout = urcl::comm::TCPSocket::DEFAULT_RECONNECTION_TIME); /*! * \brief Retrieves previously raised error codes from PrimaryClient. After calling this, recorded errors will be From 9ed3b698a6bc8827f7699643a1eef7ecf458355c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 13 Feb 2025 10:58:33 +0100 Subject: [PATCH 7/8] Use rtde initialization attempts and timeouts from UrDriver --- src/ur/ur_driver.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index ed4ddf48b..52fe45ed4 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -62,6 +62,8 @@ void UrDriver::init(const UrDriverConfiguration& config) in_headless_mode_ = config.headless_mode; socket_connection_attempts_ = config.socket_reconnect_attempts; socket_reconnection_timeout_ = config.socket_reconnection_timeout; + rtde_initialization_attempts_ = config.rtde_initialization_attempts_; + rtde_initialization_timeout_ = config.rtde_initialization_timeout_; URCL_LOG_DEBUG("Initializing urdriver"); URCL_LOG_DEBUG("Initializing RTDE client"); @@ -693,7 +695,8 @@ void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const void UrDriver::initRTDE() { - if (!rtde_client_->init(socket_connection_attempts_, socket_reconnection_timeout_)) + if (!rtde_client_->init(socket_connection_attempts_, socket_reconnection_timeout_, rtde_initialization_attempts_, + rtde_initialization_timeout_)) { throw UrException("Initialization of RTDE client went wrong."); } From 4839392b8aa5822516ac5c60d83dc86d47cbd69b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 13 Feb 2025 11:06:13 +0100 Subject: [PATCH 8/8] Correct output of initialization timeout for RTDE client --- src/rtde/rtde_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 316f7c344..8667d4661 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -98,7 +98,7 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m if (++attempts < max_initialization_attempts) { - URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds"); + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); std::this_thread::sleep_for(initialization_timeout); } }