From 174955d388dd60c4f47df7d072b44a684ffa95f9 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Fri, 10 Oct 2025 06:38:22 +0200 Subject: [PATCH 1/7] Fixed auto reconnection to the RTDE server. When reconnecting to RTDE it requires you to set the input and output recipes again, this is now done properly. --- include/ur_client_library/comm/pipeline.h | 8 +- include/ur_client_library/comm/producer.h | 31 +++ include/ur_client_library/comm/stream.h | 32 +++ include/ur_client_library/rtde/rtde_client.h | 20 +- include/ur_client_library/rtde/rtde_writer.h | 11 +- src/rtde/rtde_client.cpp | 277 ++++++++++++------- src/rtde/rtde_writer.cpp | 13 + 7 files changed, 274 insertions(+), 118 deletions(-) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index f18f17410..5e77ff2ac 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -299,6 +299,7 @@ class Pipeline , queue_{ 32 } , running_{ false } , producer_fifo_scheduling_(producer_fifo_scheduling) + , threads_stopped_(true) { } /*! @@ -318,6 +319,7 @@ class Pipeline , queue_{ 32 } , running_{ false } , producer_fifo_scheduling_(producer_fifo_scheduling) + , threads_stopped_(true) { } @@ -354,6 +356,7 @@ class Pipeline return; running_ = true; + threads_stopped_ = false; producer_.startProducer(); pThread_ = std::thread(&Pipeline::runProducer, this); if (consumer_ != nullptr) @@ -366,13 +369,13 @@ class Pipeline */ void stop() { - if (!running_) + if (threads_stopped_) return; URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); running_ = false; - + threads_stopped_ = true; producer_.stopProducer(); if (pThread_.joinable()) { @@ -417,6 +420,7 @@ class Pipeline std::atomic running_; std::thread pThread_, cThread_; bool producer_fifo_scheduling_; + bool threads_stopped_; void runProducer() { diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 1a9887506..e3d7945c4 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -43,6 +43,7 @@ class URProducer : public IProducer URStream& stream_; Parser& parser_; std::chrono::seconds timeout_; + std::function on_rtde_reconnect_cb_; bool running_; @@ -124,9 +125,28 @@ class URProducer : public IProducer if (!running_) return true; + if (stream_.getState() == SocketState::Connected) + { + continue; + } + if (stream_.closed()) return false; + if (stream_.getStreamType() == URStreamType::RTDE) + { + if (on_rtde_reconnect_cb_) + { + URCL_LOG_WARN("Failed to read from RTDE stream, invoking on reconnect callback and stopping the producer"); + on_rtde_reconnect_cb_(); + } + else + { + URCL_LOG_ERROR("Failed to read from RTDE stream without a reconnect handler stopping the producer"); + } + return false; + } + URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); std::this_thread::sleep_for(timeout_); @@ -140,6 +160,17 @@ class URProducer : public IProducer return false; } + + /*! + * \brief Sets the RTDE reconnection callback. RTDE requires setting up the communication again upon reconnection + * it is not enough to just reconnect to the stream. + * + * \param on_rtde_reconnect_cb Callback to be invoked when connection is lost to the RTDE stream. + */ + void setRTDEReconnectionCallback(std::function on_rtde_reconnect_cb) + { + on_rtde_reconnect_cb_ = on_rtde_reconnect_cb; + } }; } // namespace comm } // namespace urcl diff --git a/include/ur_client_library/comm/stream.h b/include/ur_client_library/comm/stream.h index 8a573492e..7d4c62516 100644 --- a/include/ur_client_library/comm/stream.h +++ b/include/ur_client_library/comm/stream.h @@ -30,6 +30,18 @@ namespace urcl { namespace comm { + +/*! + * \brief Different types of UR streams + */ +enum class URStreamType +{ + Primary = 30001, ///< Stream connected to the primary interface + Secondary = 30002, ///< Stream connected to the secondary interface + RTDE = 30004, ///< Stream connected to the RTDE interface + UNKNOWN = -1, ///< Stream type is fetched from the port, this is to handle unknown ports +}; + /*! * \brief The stream is an abstraction of the TCPSocket that offers reading a full UR data package * out of the socket. This means, it has to have some knowledge about the package structure to @@ -117,6 +129,26 @@ class URStream : public TCPSocket return host_; } + /*! + * \brief Get the stream type + * + * \returns The stream type + */ + URStreamType getStreamType() + { + switch (port_) + { + case static_cast(URStreamType::Primary): + return URStreamType::Primary; + case static_cast(URStreamType::Secondary): + return URStreamType::Secondary; + case static_cast(URStreamType::RTDE): + return URStreamType::RTDE; + default: + return URStreamType::UNKNOWN; + } + } + private: std::string host_; int port_; diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index d94faa5d1..eef1d01bc 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -231,6 +231,9 @@ class RTDEClient comm::INotifier notifier_; std::unique_ptr> pipeline_; RTDEWriter writer_; + bool reconnecting_; + std::mutex reconnect_mutex_; + std::thread reconnecting_thread_; VersionInformation urcontrol_version_; @@ -249,12 +252,13 @@ class RTDEClient // the robot is booted. std::vector ensureTimestampIsPresent(const std::vector& output_recipe) const; - void setupCommunication(const size_t max_num_tries = 0, - const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10)); + bool setupCommunication(); + uint16_t setProtocolVersion(); bool negotiateProtocolVersion(const uint16_t protocol_version); - void queryURControlVersion(); - void setupOutputs(const uint16_t protocol_version); - void setupInputs(); + bool queryURControlVersion(); + bool setTargetFrequency(); + bool setupOutputs(const uint16_t protocol_version); + bool setupInputs(); void disconnect(); /*! @@ -288,6 +292,12 @@ class RTDEClient * \returns A vector of variable variable_names */ std::vector splitVariableTypes(const std::string& variable_types) const; + + /*! + * \brief Reconnects to the RTDE interface and set the input and output recipes again. + */ + void reconnect(); + void reconnectCallback(); }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 77a562d08..00f3da616 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -61,11 +61,7 @@ class RTDEWriter ~RTDEWriter() { - running_ = false; - if (writer_thread_.joinable()) - { - writer_thread_.join(); - } + stop(); } /*! * \brief Starts the writer thread, which periodically clears the queue to write packages to the @@ -79,6 +75,11 @@ class RTDEWriter */ void run(); + /*! + * \brief Stops the writer thread loop. + */ + void stop(); + /*! * \brief Creates a package to request setting a new value for the speed slider. * diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 425dc981d..afc5fe3bf 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -47,6 +47,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , notifier_(notifier) , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) + , reconnecting_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) , client_state_(ClientState::UNINITIALIZED) @@ -65,6 +66,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , notifier_(notifier) , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) + , reconnecting_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) , client_state_(ClientState::UNINITIALIZED) @@ -89,93 +91,87 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m return true; } + prod_->setRTDEReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); + std::cout << "what is happening here" << std::endl; + unsigned int attempts = 0; std::stringstream ss; - while (attempts < max_initialization_attempts) + + // A running pipeline is needed inside setup. + pipeline_->init(max_connection_attempts, reconnection_timeout); + pipeline_->run(); + + // The state initializing is used inside disconnect to stop the pipeline again. + client_state_ = ClientState::INITIALIZING; + + while (!setupCommunication()) { - try - { - setupCommunication(max_connection_attempts, reconnection_timeout); - } - catch (const UrException& exc) - { - ss << exc.what() << std::endl; - } - if (client_state_ == ClientState::INITIALIZED) + if (++attempts >= max_initialization_attempts) { - return true; - } - 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); + disconnect(); + ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts"; + throw UrException(ss.str()); } + // disconnect to start on a clean slate when trying to set up communication again + disconnect(); + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); + std::this_thread::sleep_for(initialization_timeout); + + // A running pipeline is needed inside setup + pipeline_->init(max_connection_attempts, reconnection_timeout); + pipeline_->run(); } - ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts"; - throw UrException(ss.str()); + // Stop pipeline again + pipeline_->stop(); + client_state_ = ClientState::INITIALIZED; + return true; } -void RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) +bool RTDEClient::setupCommunication() { - client_state_ = ClientState::INITIALIZING; - // A running pipeline is needed inside setup - pipeline_->init(max_num_tries, reconnection_time); - pipeline_->run(); + uint16_t protocol_version = setProtocolVersion(); + // Protocol version must be above zero + if (protocol_version == 0) + { + return false; + } + + bool is_rtde_comm_setup = true; + is_rtde_comm_setup = queryURControlVersion(); + + is_rtde_comm_setup = is_rtde_comm_setup && setTargetFrequency(); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); + + is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted(); + + return is_rtde_comm_setup && setupInputs(); +} + +uint16_t RTDEClient::setProtocolVersion() +{ uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION; - while (!negotiateProtocolVersion(protocol_version) && client_state_ == ClientState::INITIALIZING) + while (!negotiateProtocolVersion(protocol_version)) { + if (stream_.getState() != comm::SocketState::Connected) + { + URCL_LOG_ERROR("Protocol version for RTDE communication could not be established, because the RTDE client is " + "disconnected from the server."); + return 0; + } URCL_LOG_INFO("Robot did not accept RTDE protocol version '%hu'. Trying lower protocol version", protocol_version); protocol_version--; if (protocol_version == 0) { - throw UrException("Protocol version for RTDE communication could not be established. Robot didn't accept any of " - "the suggested versions."); + URCL_LOG_ERROR("Protocol version for RTDE communication could not be established. Robot didn't accept any of " + "the suggested versions."); + return 0; } } - if (client_state_ == ClientState::UNINITIALIZED) - return; URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); parser_.setProtocolVersion(protocol_version); - - queryURControlVersion(); - if (client_state_ == ClientState::UNINITIALIZED) - return; - - if (urcontrol_version_.major < 5) - { - max_frequency_ = CB3_MAX_FREQUENCY; - } - - if (target_frequency_ == 0) - { - // Default to maximum frequency - target_frequency_ = max_frequency_; - } - else if (target_frequency_ <= 0.0 || target_frequency_ > max_frequency_) - { - // Target frequency outside valid range - throw UrException("Invalid target frequency of RTDE connection"); - } - - setupOutputs(protocol_version); - if (client_state_ == ClientState::UNINITIALIZED) - return; - - if (!isRobotBooted()) - { - disconnect(); - return; - } - - setupInputs(); - if (client_state_ == ClientState::UNINITIALIZED) - return; - - // We finished communication for now - pipeline_->stop(); - client_state_ = ClientState::INITIALIZED; + return protocol_version; } bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) @@ -189,8 +185,7 @@ bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); if (!stream_.write(buffer, size, written)) { - URCL_LOG_ERROR("Sending protocol version query to robot failed, disconnecting"); - disconnect(); + URCL_LOG_ERROR("Sending protocol version query to robot failed"); return false; } @@ -199,8 +194,7 @@ bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) std::unique_ptr package; if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { - URCL_LOG_ERROR("failed to get package from rtde interface, disconnecting"); - disconnect(); + URCL_LOG_ERROR("failed to get package from rtde interface"); return false; } if (rtde_interface::RequestProtocolVersion* tmp_version = @@ -223,10 +217,10 @@ bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) ss << "Could not negotiate RTDE protocol version after " << MAX_REQUEST_RETRIES << " tries. Please check the output of the " "negotiation attempts above to get a hint what could be wrong."; - throw UrException(ss.str()); + return false; } -void RTDEClient::queryURControlVersion() +bool RTDEClient::queryURControlVersion() { unsigned int num_retries = 0; uint8_t buffer[4096]; @@ -235,9 +229,8 @@ void RTDEClient::queryURControlVersion() size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer); if (!stream_.write(buffer, size, written)) { - URCL_LOG_ERROR("Sending urcontrol version query request to robot failed, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("Sending urcontrol version query request to robot failed"); + return false; } std::unique_ptr package; @@ -245,16 +238,15 @@ void RTDEClient::queryURControlVersion() { if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { - URCL_LOG_ERROR("No answer to urcontrol version query was received from robot, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("No answer to urcontrol version query was received from robot"); + return false; } if (rtde_interface::GetUrcontrolVersion* tmp_urcontrol_version = dynamic_cast(package.get())) { urcontrol_version_ = tmp_urcontrol_version->version_information_; - return; + return true; } else { @@ -269,7 +261,27 @@ void RTDEClient::queryURControlVersion() ss << "Could not query urcontrol version after " << MAX_REQUEST_RETRIES << " tries. Please check the output of the " "negotiation attempts above to get a hint what could be wrong."; - throw UrException(ss.str()); + return false; +} + +bool RTDEClient::setTargetFrequency() +{ + if (urcontrol_version_.major < 5) + { + max_frequency_ = CB3_MAX_FREQUENCY; + } + + if (target_frequency_ == 0) + { + // Default to maximum frequency + target_frequency_ = max_frequency_; + } + else if (target_frequency_ <= 0.0 || target_frequency_ > max_frequency_) + { + // Target frequency outside valid range + throw UrException("Invalid target frequency of RTDE connection"); + } + return true; } void RTDEClient::resetOutputRecipe(const std::vector new_recipe) @@ -282,7 +294,7 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true); } -void RTDEClient::setupOutputs(const uint16_t protocol_version) +bool RTDEClient::setupOutputs(const uint16_t protocol_version) { unsigned int num_retries = 0; size_t size; @@ -310,17 +322,15 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) // Send output recipe to robot if (!stream_.write(buffer, size, written)) { - URCL_LOG_ERROR("Could not send RTDE output recipe to robot, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("Could not send RTDE output recipe to robot"); + return false; } std::unique_ptr package; if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { - URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe"); + return false; } if (rtde_interface::ControlPackageSetupOutputs* tmp_output = @@ -364,7 +374,7 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) // Some variables are not available so retry setting up the communication with a stripped-down output recipe resetOutputRecipe(available_variables); - return; + return false; } else { @@ -375,7 +385,7 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) else { // All variables are accounted for in the RTDE package - return; + return true; } } else @@ -391,10 +401,11 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version) ss << "Could not setup RTDE outputs after " << MAX_REQUEST_RETRIES << " tries. Please check the output of the " "negotiation attempts above to get a hint what could be wrong."; - throw UrException(ss.str()); + URCL_LOG_ERROR(ss.str().c_str()); + return false; } -void RTDEClient::setupInputs() +bool RTDEClient::setupInputs() { unsigned int num_retries = 0; size_t size; @@ -403,9 +414,8 @@ void RTDEClient::setupInputs() size = ControlPackageSetupInputsRequest::generateSerializedRequest(buffer, input_recipe_); if (!stream_.write(buffer, size, written)) { - URCL_LOG_ERROR("Could not send RTDE input recipe to robot, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("Could not send RTDE input recipe to robot"); + return false; } while (num_retries < MAX_REQUEST_RETRIES) @@ -413,11 +423,9 @@ void RTDEClient::setupInputs() std::unique_ptr package; if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { - URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe, disconnecting"); - disconnect(); - return; + URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe"); + return false; } - if (rtde_interface::ControlPackageSetupInputs* tmp_input = dynamic_cast(package.get())) @@ -443,7 +451,7 @@ void RTDEClient::setupInputs() } writer_.init(tmp_input->input_recipe_id_); - return; + return true; } else { @@ -458,7 +466,8 @@ void RTDEClient::setupInputs() ss << "Could not setup RTDE inputs after " << MAX_REQUEST_RETRIES << " tries. Please check the output of the " "negotiation attempts above to get a hint what could be wrong."; - throw UrException(ss.str()); + URCL_LOG_ERROR(ss.str().c_str()); + return false; } void RTDEClient::disconnect() @@ -475,6 +484,7 @@ void RTDEClient::disconnect() if (client_state_ > ClientState::UNINITIALIZED) { stream_.disconnect(); + writer_.stop(); } client_state_ = ClientState::UNINITIALIZED; } @@ -682,16 +692,29 @@ std::vector RTDEClient::ensureTimestampIsPresent(const std::vector< std::unique_ptr RTDEClient::getDataPackage(std::chrono::milliseconds timeout) { - std::unique_ptr urpackage; - if (pipeline_->getLatestProduct(urpackage, timeout)) + // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages + if (reconnect_mutex_.try_lock()) { - rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get()); - if (tmp != nullptr) + std::unique_ptr urpackage; + if (pipeline_->getLatestProduct(urpackage, timeout)) { - urpackage.release(); - return std::unique_ptr(tmp); + rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get()); + if (tmp != nullptr) + { + urpackage.release(); + reconnect_mutex_.unlock(); + return std::unique_ptr(tmp); + } } + reconnect_mutex_.unlock(); } + else + { + URCL_LOG_WARN("Unable to get data package while reconnecting to the RTDE interface"); + auto period = std::chrono::duration(1.0 / target_frequency_); + std::this_thread::sleep_for(period); + } + return std::unique_ptr(nullptr); } @@ -716,5 +739,47 @@ std::vector RTDEClient::splitVariableTypes(const std::string& varia } return result; } + +void RTDEClient::reconnect() +{ + URCL_LOG_INFO("Reconnecting to the RTDE interface"); + // Locking mutex to ensure that calling getDataPackage doesn't influence the communication needed for reconfiguring + // the RTDE connection + std::lock_guard lock(reconnect_mutex_); + ClientState cur_client_state = client_state_; + disconnect(); + try + { + init(); + } + catch (const UrException& exc) + { + URCL_LOG_ERROR("Caught exception while reconnecting to the RTDE interface %s. Unable to reconnect", exc.what()); + reconnecting_ = false; + return; + } + + start(); + if (cur_client_state == ClientState::PAUSED) + { + pause(); + } + reconnecting_ = false; +} + +void RTDEClient::reconnectCallback() +{ + if (reconnecting_) + { + return; + } + if (reconnecting_thread_.joinable()) + { + reconnecting_thread_.join(); + } + reconnecting_ = true; + reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this); +} + } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index ea543ab80..65f4672aa 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -39,6 +39,10 @@ RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector 1.0 || speed_slider_fraction < 0.0) From ce23e6d5b2d4f87a582cc5a4413e7501f9053af5 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Mon, 13 Oct 2025 14:38:44 +0200 Subject: [PATCH 2/7] Apply suggestions from code review Also fixed that we can destroy the rtdeClient while reconnecting --- include/ur_client_library/comm/pipeline.h | 8 - include/ur_client_library/rtde/rtde_client.h | 8 +- src/rtde/rtde_client.cpp | 222 ++++++++++++------- tests/test_pipeline.cpp | 2 + 4 files changed, 142 insertions(+), 98 deletions(-) diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index 5e77ff2ac..aafd0f754 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -299,7 +299,6 @@ class Pipeline , queue_{ 32 } , running_{ false } , producer_fifo_scheduling_(producer_fifo_scheduling) - , threads_stopped_(true) { } /*! @@ -319,7 +318,6 @@ class Pipeline , queue_{ 32 } , running_{ false } , producer_fifo_scheduling_(producer_fifo_scheduling) - , threads_stopped_(true) { } @@ -356,7 +354,6 @@ class Pipeline return; running_ = true; - threads_stopped_ = false; producer_.startProducer(); pThread_ = std::thread(&Pipeline::runProducer, this); if (consumer_ != nullptr) @@ -369,13 +366,9 @@ class Pipeline */ void stop() { - if (threads_stopped_) - return; - URCL_LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str()); running_ = false; - threads_stopped_ = true; producer_.stopProducer(); if (pThread_.joinable()) { @@ -420,7 +413,6 @@ class Pipeline std::atomic running_; std::thread pThread_, cThread_; bool producer_fifo_scheduling_; - bool threads_stopped_; void runProducer() { diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index eef1d01bc..e35e6b95a 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -252,11 +252,11 @@ class RTDEClient // the robot is booted. std::vector ensureTimestampIsPresent(const std::vector& output_recipe) const; - bool setupCommunication(); - uint16_t setProtocolVersion(); - bool negotiateProtocolVersion(const uint16_t protocol_version); + bool setupCommunication(const size_t max_num_tries = 0, + const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10)); + std::pair setProtocolVersion(); bool queryURControlVersion(); - bool setTargetFrequency(); + void setTargetFrequency(); bool setupOutputs(const uint16_t protocol_version); bool setupInputs(); void disconnect(); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index afc5fe3bf..1f47b1323 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -75,6 +75,11 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st RTDEClient::~RTDEClient() { + reconnecting_ = false; + if (reconnecting_thread_.joinable()) + { + reconnecting_thread_.join(); + } disconnect(); } @@ -92,19 +97,11 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m } prod_->setRTDEReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); - std::cout << "what is happening here" << std::endl; unsigned int attempts = 0; std::stringstream ss; - // A running pipeline is needed inside setup. - pipeline_->init(max_connection_attempts, reconnection_timeout); - pipeline_->run(); - - // The state initializing is used inside disconnect to stop the pipeline again. - client_state_ = ClientState::INITIALIZING; - - while (!setupCommunication()) + while (!setupCommunication(max_connection_attempts, reconnection_timeout)) { if (++attempts >= max_initialization_attempts) { @@ -116,10 +113,6 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m disconnect(); URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); std::this_thread::sleep_for(initialization_timeout); - - // A running pipeline is needed inside setup - pipeline_->init(max_connection_attempts, reconnection_timeout); - pipeline_->run(); } // Stop pipeline again pipeline_->stop(); @@ -127,11 +120,25 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m return true; } -bool RTDEClient::setupCommunication() +bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) { - uint16_t protocol_version = setProtocolVersion(); + // The state initializing is used inside disconnect to stop the pipeline again. + client_state_ = ClientState::INITIALIZING; + // A running pipeline is needed inside setup. + try + { + pipeline_->init(max_num_tries, reconnection_time); + } + catch (const UrException& exc) + { + URCL_LOG_ERROR("Caught exception %s, while trying to initialize pipeline", exc.what()); + return false; + } + pipeline_->run(); + + std::pair protocol_version = setProtocolVersion(); // Protocol version must be above zero - if (protocol_version == 0) + if (protocol_version.first == false) { return false; } @@ -139,85 +146,71 @@ bool RTDEClient::setupCommunication() bool is_rtde_comm_setup = true; is_rtde_comm_setup = queryURControlVersion(); - is_rtde_comm_setup = is_rtde_comm_setup && setTargetFrequency(); + if (is_rtde_comm_setup) + { + setTargetFrequency(); + } - is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version.second); is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted(); return is_rtde_comm_setup && setupInputs(); } -uint16_t RTDEClient::setProtocolVersion() +std::pair RTDEClient::setProtocolVersion() { uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION; - while (!negotiateProtocolVersion(protocol_version)) - { - if (stream_.getState() != comm::SocketState::Connected) - { - URCL_LOG_ERROR("Protocol version for RTDE communication could not be established, because the RTDE client is " - "disconnected from the server."); - return 0; - } - URCL_LOG_INFO("Robot did not accept RTDE protocol version '%hu'. Trying lower protocol version", protocol_version); - protocol_version--; - if (protocol_version == 0) + while (protocol_version > 0) + { + // Protocol version should always be 1 before starting negotiation + parser_.setProtocolVersion(1); + unsigned int num_retries = 0; + uint8_t buffer[4096]; + size_t size; + size_t written; + size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); + if (!stream_.write(buffer, size, written)) { - URCL_LOG_ERROR("Protocol version for RTDE communication could not be established. Robot didn't accept any of " - "the suggested versions."); - return 0; + URCL_LOG_ERROR("Sending protocol version query to robot failed"); + return { false, 0 }; } - } - URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); - parser_.setProtocolVersion(protocol_version); - return protocol_version; -} - -bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version) -{ - // Protocol version should always be 1 before starting negotiation - parser_.setProtocolVersion(1); - unsigned int num_retries = 0; - uint8_t buffer[4096]; - size_t size; - size_t written; - size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); - if (!stream_.write(buffer, size, written)) - { - URCL_LOG_ERROR("Sending protocol version query to robot failed"); - return false; - } - - while (num_retries < MAX_REQUEST_RETRIES) - { - std::unique_ptr package; - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) - { - URCL_LOG_ERROR("failed to get package from rtde interface"); - return false; - } - if (rtde_interface::RequestProtocolVersion* tmp_version = - dynamic_cast(package.get())) + while (num_retries < MAX_REQUEST_RETRIES) { - // Reset the num_tries variable in case we have to try with another protocol version. - num_retries = 0; - return tmp_version->accepted_; - } - else - { - std::stringstream ss; - ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl - << package->toString() << ". Retrying..."; - num_retries++; - URCL_LOG_WARN("%s", ss.str().c_str()); + std::unique_ptr package; + if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + { + URCL_LOG_ERROR("failed to get package from RTDE interface"); + return { false, 0 }; + } + if (rtde_interface::RequestProtocolVersion* tmp_version = + dynamic_cast(package.get())) + { + if (tmp_version->accepted_) + { + URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); + parser_.setProtocolVersion(protocol_version); + return { true, protocol_version }; + } + break; + } + else + { + std::stringstream ss; + ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl + << package->toString() << ". Retrying..."; + num_retries++; + URCL_LOG_WARN("%s", ss.str().c_str()); + } } + + URCL_LOG_INFO("Robot did not accept RTDE protocol version '%hu'. Trying lower protocol version", protocol_version); + protocol_version--; } - std::stringstream ss; - ss << "Could not negotiate RTDE protocol version after " << MAX_REQUEST_RETRIES - << " tries. Please check the output of the " - "negotiation attempts above to get a hint what could be wrong."; - return false; + URCL_LOG_ERROR("Protocol version for RTDE communication could not be established. Robot didn't accept any of " + "the suggested versions."); + return { false, 0 }; } bool RTDEClient::queryURControlVersion() @@ -264,7 +257,7 @@ bool RTDEClient::queryURControlVersion() return false; } -bool RTDEClient::setTargetFrequency() +void RTDEClient::setTargetFrequency() { if (urcontrol_version_.major < 5) { @@ -281,7 +274,6 @@ bool RTDEClient::setTargetFrequency() // Target frequency outside valid range throw UrException("Invalid target frequency of RTDE connection"); } - return true; } void RTDEClient::resetOutputRecipe(const std::vector new_recipe) @@ -748,13 +740,71 @@ void RTDEClient::reconnect() std::lock_guard lock(reconnect_mutex_); ClientState cur_client_state = client_state_; disconnect(); - try + + const size_t max_initialization_attempts = 3; + size_t cur_initialization_attempt = 0; + bool client_reconnected = false; + while (cur_initialization_attempt < max_initialization_attempts) { - init(); + bool is_communication_setup = false; + try + { + is_communication_setup = setupCommunication(1, std::chrono::milliseconds{ 10000 }); + } + catch (const UrException& exc) + { + URCL_LOG_ERROR("Caught exception while reconnecting to the RTDE interface %s. Unable to reconnect", exc.what()); + disconnect(); + reconnecting_ = false; + return; + } + + const std::string reconnecting_stopped_msg = "Reconnecting has been stopped, because the object is being destroyed"; + if (reconnecting_ == false) + { + URCL_LOG_WARN(reconnecting_stopped_msg.c_str()); + return; + } + + if (is_communication_setup) + { + client_reconnected = true; + break; + } + + auto duration = std::chrono::seconds(1); + if (stream_.getState() != comm::SocketState::Connected) + { + // We don't wanna count it as an initialization attempt if we cannot connect to the socket and we want to wait + // longer before reconnecting. + duration = std::chrono::seconds(10); + URCL_LOG_ERROR("Failed to connect to the RTDE server, retrying in %i seconds", duration.count()); + } + else + { + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %i second", duration.count()); + cur_initialization_attempt += 1; + } + + disconnect(); + + auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < duration) + { + std::this_thread::sleep_for(std::chrono::milliseconds(250)); + if (reconnecting_ == false) + { + URCL_LOG_WARN(reconnecting_stopped_msg.c_str()); + return; + } + } } - catch (const UrException& exc) + + if (client_reconnected == false) { - URCL_LOG_ERROR("Caught exception while reconnecting to the RTDE interface %s. Unable to reconnect", exc.what()); + URCL_LOG_ERROR("Failed to initialize RTDE client after %i attempts, unable to reconnect", + max_initialization_attempts); + disconnect(); reconnecting_ = false; return; } diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index 8da69fbed..ca2368d6d 100644 --- a/tests/test_pipeline.cpp +++ b/tests/test_pipeline.cpp @@ -199,6 +199,7 @@ TEST_F(PipelineTest, stop_pipeline) TEST_F(PipelineTest, consumer_pipeline) { + pipeline_.reset(); stream_.reset(new comm::URStream("127.0.0.1", 60002)); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); TestConsumer consumer; @@ -236,6 +237,7 @@ TEST_F(PipelineTest, consumer_pipeline) TEST_F(PipelineTest, connect_non_connected_robot) { + pipeline_.reset(); stream_.reset(new comm::URStream("127.0.0.1", 12321)); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); TestConsumer consumer; From d7a622a7c87e44f039af12330dadb04de921958b Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Tue, 14 Oct 2025 07:59:42 +0200 Subject: [PATCH 3/7] Fix segfault in resetting output recipe --- src/rtde/rtde_client.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 1f47b1323..f01bd56c4 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -281,8 +281,14 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); + + // Reset pipeline first otherwise we will segfault, if the producer object no longer exists, when destroying the + // pipeline + pipeline_.reset(); + parser_ = RTDEParser(output_recipe_); prod_ = std::make_unique>(stream_, parser_); + prod_->setRTDEReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true); } From e55432db8595c96f26fde3960bf16a7d2ba791ab Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Wed, 15 Oct 2025 16:05:18 +0200 Subject: [PATCH 4/7] Potential fix for the race condition Applied suggestions from code review --- include/ur_client_library/comm/producer.h | 26 +++++++--------- include/ur_client_library/comm/stream.h | 31 -------------------- include/ur_client_library/rtde/rtde_client.h | 1 + src/rtde/rtde_client.cpp | 20 ++++++++----- 4 files changed, 23 insertions(+), 55 deletions(-) diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index e3d7945c4..19a929a70 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -43,7 +43,7 @@ class URProducer : public IProducer URStream& stream_; Parser& parser_; std::chrono::seconds timeout_; - std::function on_rtde_reconnect_cb_; + std::function on_reconnect_cb_; bool running_; @@ -133,17 +133,10 @@ class URProducer : public IProducer if (stream_.closed()) return false; - if (stream_.getStreamType() == URStreamType::RTDE) + if (on_reconnect_cb_) { - if (on_rtde_reconnect_cb_) - { - URCL_LOG_WARN("Failed to read from RTDE stream, invoking on reconnect callback and stopping the producer"); - on_rtde_reconnect_cb_(); - } - else - { - URCL_LOG_ERROR("Failed to read from RTDE stream without a reconnect handler stopping the producer"); - } + URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer"); + on_reconnect_cb_(); return false; } @@ -162,14 +155,15 @@ class URProducer : public IProducer } /*! - * \brief Sets the RTDE reconnection callback. RTDE requires setting up the communication again upon reconnection - * it is not enough to just reconnect to the stream. + * \brief Sets the reconnection callback. Use this to configure a reconnection callback instead of connecting directly + * to the stream again. This is needed for RTDE as it requires setting up the communication again upon reconnection it + * is not enough to just reconnect to the stream. * - * \param on_rtde_reconnect_cb Callback to be invoked when connection is lost to the RTDE stream. + * \param on_reconnect_cb Callback to be invoked when connection is lost to the stream. */ - void setRTDEReconnectionCallback(std::function on_rtde_reconnect_cb) + void setReconnectionCallback(std::function on_reconnect_cb) { - on_rtde_reconnect_cb_ = on_rtde_reconnect_cb; + on_reconnect_cb_ = on_reconnect_cb; } }; } // namespace comm diff --git a/include/ur_client_library/comm/stream.h b/include/ur_client_library/comm/stream.h index 7d4c62516..a6fb57052 100644 --- a/include/ur_client_library/comm/stream.h +++ b/include/ur_client_library/comm/stream.h @@ -31,17 +31,6 @@ namespace urcl namespace comm { -/*! - * \brief Different types of UR streams - */ -enum class URStreamType -{ - Primary = 30001, ///< Stream connected to the primary interface - Secondary = 30002, ///< Stream connected to the secondary interface - RTDE = 30004, ///< Stream connected to the RTDE interface - UNKNOWN = -1, ///< Stream type is fetched from the port, this is to handle unknown ports -}; - /*! * \brief The stream is an abstraction of the TCPSocket that offers reading a full UR data package * out of the socket. This means, it has to have some knowledge about the package structure to @@ -129,26 +118,6 @@ class URStream : public TCPSocket return host_; } - /*! - * \brief Get the stream type - * - * \returns The stream type - */ - URStreamType getStreamType() - { - switch (port_) - { - case static_cast(URStreamType::Primary): - return URStreamType::Primary; - case static_cast(URStreamType::Secondary): - return URStreamType::Secondary; - case static_cast(URStreamType::RTDE): - return URStreamType::RTDE; - default: - return URStreamType::UNKNOWN; - } - } - private: std::string host_; int port_; diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index e35e6b95a..83bcf7913 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -232,6 +232,7 @@ class RTDEClient std::unique_ptr> pipeline_; RTDEWriter writer_; bool reconnecting_; + bool stop_reconnection_; std::mutex reconnect_mutex_; std::thread reconnecting_thread_; diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index dc38d0eb5..d782ea18d 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -47,6 +47,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , reconnecting_(false) + , stop_reconnection_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) , client_state_(ClientState::UNINITIALIZED) @@ -71,6 +72,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , reconnecting_(false) + , stop_reconnection_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) , client_state_(ClientState::UNINITIALIZED) @@ -79,7 +81,8 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st RTDEClient::~RTDEClient() { - reconnecting_ = false; + prod_->setReconnectionCallback(nullptr); + stop_reconnection_ = true; if (reconnecting_thread_.joinable()) { reconnecting_thread_.join(); @@ -100,7 +103,7 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m return true; } - prod_->setRTDEReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); + prod_->setReconnectionCallback(nullptr); unsigned int attempts = 0; std::stringstream ss; @@ -121,6 +124,9 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m // Stop pipeline again pipeline_->stop(); client_state_ = ClientState::INITIALIZED; + // Set reconnection callback after we are initialized to ensure that a disconnect during initialization doesn't + // trigger a reconnect + prod_->setReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); return true; } @@ -296,7 +302,6 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) parser_ = RTDEParser(output_recipe_); prod_ = std::make_unique>(stream_, parser_); - prod_->setRTDEReconnectionCallback(std::bind(&RTDEClient::reconnectCallback, this)); pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true); } @@ -611,7 +616,6 @@ bool RTDEClient::sendStart() ss << "Did not receive answer to RTDE start request. Message received instead: " << std::endl << package->toString(); URCL_LOG_WARN("%s", ss.str().c_str()); - return false; } } std::stringstream ss; @@ -644,7 +648,6 @@ bool RTDEClient::sendPause() } if (rtde_interface::ControlPackagePause* tmp = dynamic_cast(package.get())) { - client_state_ = ClientState::PAUSED; return tmp->accepted_; } } @@ -774,7 +777,7 @@ void RTDEClient::reconnect() } const std::string reconnecting_stopped_msg = "Reconnecting has been stopped, because the object is being destroyed"; - if (reconnecting_ == false) + if (stop_reconnection_) { URCL_LOG_WARN(reconnecting_stopped_msg.c_str()); return; @@ -806,7 +809,7 @@ void RTDEClient::reconnect() while (std::chrono::steady_clock::now() - start_time < duration) { std::this_thread::sleep_for(std::chrono::milliseconds(250)); - if (reconnecting_ == false) + if (stop_reconnection_) { URCL_LOG_WARN(reconnecting_stopped_msg.c_str()); return; @@ -828,12 +831,13 @@ void RTDEClient::reconnect() { pause(); } + URCL_LOG_INFO("Done reconnecting to the RTDE interface"); reconnecting_ = false; } void RTDEClient::reconnectCallback() { - if (reconnecting_) + if (reconnecting_ || stop_reconnection_) { return; } From b64aef35222866e01bf8f9cf493e0f85d2caaccb Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 16 Oct 2025 11:43:30 +0200 Subject: [PATCH 5/7] Update include/ur_client_library/rtde/rtde_client.h Co-authored-by: Felix Exner --- include/ur_client_library/rtde/rtde_client.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 83bcf7913..338b6ba2e 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -231,8 +231,8 @@ class RTDEClient comm::INotifier notifier_; std::unique_ptr> pipeline_; RTDEWriter writer_; - bool reconnecting_; - bool stop_reconnection_; + std::atomic reconnecting_; + std::atomic stop_reconnection_; std::mutex reconnect_mutex_; std::thread reconnecting_thread_; From 676797bfbdbaaec89392657d72cb63346887471b Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 16 Oct 2025 11:43:48 +0200 Subject: [PATCH 6/7] Update include/ur_client_library/comm/stream.h Co-authored-by: Felix Exner --- include/ur_client_library/comm/stream.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/ur_client_library/comm/stream.h b/include/ur_client_library/comm/stream.h index a6fb57052..8a573492e 100644 --- a/include/ur_client_library/comm/stream.h +++ b/include/ur_client_library/comm/stream.h @@ -30,7 +30,6 @@ namespace urcl { namespace comm { - /*! * \brief The stream is an abstraction of the TCPSocket that offers reading a full UR data package * out of the socket. This means, it has to have some knowledge about the package structure to From 2b1197e7603a29b7bec17dd07f7ba7822f51f930 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Thu, 16 Oct 2025 12:00:39 +0200 Subject: [PATCH 7/7] Change function name to negotiateProtocolVersion and return only protocol version --- include/ur_client_library/rtde/rtde_client.h | 2 +- src/rtde/rtde_client.cpp | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 338b6ba2e..7343a4ad8 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -255,7 +255,7 @@ class RTDEClient bool setupCommunication(const size_t max_num_tries = 0, const std::chrono::milliseconds reconnection_time = std::chrono::seconds(10)); - std::pair setProtocolVersion(); + uint16_t negotiateProtocolVersion(); bool queryURControlVersion(); void setTargetFrequency(); bool setupOutputs(const uint16_t protocol_version); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index d782ea18d..b7c1bac73 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -146,9 +146,9 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron } pipeline_->run(); - std::pair protocol_version = setProtocolVersion(); + uint16_t protocol_version = negotiateProtocolVersion(); // Protocol version must be above zero - if (protocol_version.first == false) + if (protocol_version == 0) { return false; } @@ -161,7 +161,7 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron setTargetFrequency(); } - is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version.second); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted(); @@ -172,7 +172,7 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron return is_rtde_comm_setup; } -std::pair RTDEClient::setProtocolVersion() +uint16_t RTDEClient::negotiateProtocolVersion() { uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION; while (protocol_version > 0) @@ -187,7 +187,7 @@ std::pair RTDEClient::setProtocolVersion() if (!stream_.write(buffer, size, written)) { URCL_LOG_ERROR("Sending protocol version query to robot failed"); - return { false, 0 }; + return 0; } while (num_retries < MAX_REQUEST_RETRIES) @@ -196,7 +196,7 @@ std::pair RTDEClient::setProtocolVersion() if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) { URCL_LOG_ERROR("failed to get package from RTDE interface"); - return { false, 0 }; + return 0; } if (rtde_interface::RequestProtocolVersion* tmp_version = dynamic_cast(package.get())) @@ -205,7 +205,7 @@ std::pair RTDEClient::setProtocolVersion() { URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); parser_.setProtocolVersion(protocol_version); - return { true, protocol_version }; + return protocol_version; } break; } @@ -224,7 +224,7 @@ std::pair RTDEClient::setProtocolVersion() } URCL_LOG_ERROR("Protocol version for RTDE communication could not be established. Robot didn't accept any of " "the suggested versions."); - return { false, 0 }; + return 0; } bool RTDEClient::queryURControlVersion()