diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index d7fd7d58..d8a1bf15 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_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 diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 17dfd0fa..d94faa5d 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 73048fc9..56dbc617 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/comm/tcp_socket.cpp b/src/comm/tcp_socket.cpp index 8803664a..5f56bdb8 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/primary/primary_client.cpp b/src/primary/primary_client.cpp index 38ad4727..54b4872e 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/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 60208872..8667d466 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 { @@ -74,26 +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; - 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_initialization_attempts) + { + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); + 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()); } @@ -241,7 +251,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()); diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index f200b030..52fe45ed 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -60,6 +60,10 @@ 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; + 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"); @@ -70,7 +74,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_)); @@ -691,7 +695,8 @@ 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_, rtde_initialization_attempts_, + rtde_initialization_timeout_)) { throw UrException("Initialization of RTDE client went wrong."); } @@ -706,7 +711,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() diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 3e8c24fe..6b7eae43 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[])