From 2ef4ea18860555d2fa9dedcf32096369087e03ca Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 10 Feb 2026 12:54:15 +0100 Subject: [PATCH 1/5] Move splitString to helpers --- include/ur_client_library/helpers.h | 10 +++++++ include/ur_client_library/rtde/rtde_client.h | 10 ------- .../ur/version_information.h | 3 +-- src/helpers.cpp | 18 +++++++++++++ src/rtde/rtde_client.cpp | 17 +++--------- src/ur/version_information.cpp | 19 +++---------- tests/test_helpers.cpp | 27 +++++++++++++++++++ tests/test_version_information.cpp | 8 ------ 8 files changed, 62 insertions(+), 50 deletions(-) diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index 2b898cdfa..470f97e84 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -99,5 +99,15 @@ void waitFor(std::function condition, const std::chrono::milliseconds ti */ bool parseBoolean(const std::string& str); +/*! + * \brief Splits a at each delimiter found + * + * \param string_to_split String containing the delimiter and other characters + * \param delimiter Chars at which the string should be split + * + * \returns A vector of characters that were between the delimiters + */ +std::vector splitString(const std::string& string_to_split, const std::string& delimiter = ","); + } // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 5ebc76ce3..e2b01c50a 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -295,16 +295,6 @@ class RTDEClient bool sendStart(); bool sendPause(); - /*! - * \brief Splits a variable_types string as reported from the robot into single variable type - * strings - * - * \param variable_types String as reported from the robot - * - * \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. */ diff --git a/include/ur_client_library/ur/version_information.h b/include/ur_client_library/ur/version_information.h index 784cdc22e..fa47631c8 100644 --- a/include/ur_client_library/ur/version_information.h +++ b/include/ur_client_library/ur/version_information.h @@ -79,7 +79,6 @@ class VersionInformation uint32_t build; ///< Build number }; -std::vector splitString(std::string input, const std::string& delimiter = "."); } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_UR_VERSION_INFORMATION_H_INCLUDED \ No newline at end of file +#endif // ifndef UR_CLIENT_LIBRARY_UR_VERSION_INFORMATION_H_INCLUDED diff --git a/src/helpers.cpp b/src/helpers.cpp index 5e5daf7c4..6919ee4f9 100644 --- a/src/helpers.cpp +++ b/src/helpers.cpp @@ -140,4 +140,22 @@ bool parseBoolean(const std::string& str) throw UrException(ss.str().c_str()); } } + +std::vector splitString(const std::string& input, const std::string& delimiter) +{ + std::vector result; + size_t pos = 0; + size_t pos_end = pos; + std::string substring; + while ((pos_end = input.find(delimiter, pos)) != std::string::npos) + { + substring = input.substr(pos, pos_end - pos); + result.push_back(substring); + pos = pos_end + delimiter.length(); + } + substring = input.substr(pos); + result.push_back(substring); + return result; +} + } // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 4ed2f3cd6..8f386956a 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -348,7 +348,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version) dynamic_cast(package.get())) { - std::vector variable_types = splitVariableTypes(tmp_output->variable_types_); + std::vector variable_types = splitString(tmp_output->variable_types_, ","); std::vector available_variables; std::vector unavailable_variables; assert(output_recipe_.size() == variable_types.size()); @@ -441,7 +441,7 @@ bool RTDEClient::setupInputs() dynamic_cast(package.get())) { - std::vector variable_types = splitVariableTypes(tmp_input->variable_types_); + std::vector variable_types = splitString(tmp_input->variable_types_, ","); assert(input_recipe_.size() == variable_types.size()); for (std::size_t i = 0; i < variable_types.size(); ++i) { @@ -745,21 +745,10 @@ RTDEWriter& RTDEClient::getWriter() return writer_; } -std::vector RTDEClient::splitVariableTypes(const std::string& variable_types) const -{ - std::vector result; - std::stringstream ss(variable_types); - std::string substr = ""; - while (getline(ss, substr, ',')) - { - result.push_back(substr); - } - return result; -} - void RTDEClient::reconnect() { URCL_LOG_INFO("Reconnecting to the RTDE interface"); + client_state_ = ClientState::UNINITIALIZED; // Locking mutex to ensure that calling getDataPackage doesn't influence the communication needed for reconfiguring // the RTDE connection std::lock_guard lock(reconnect_mutex_); diff --git a/src/ur/version_information.cpp b/src/ur/version_information.cpp index be0b13954..b5850f0bd 100644 --- a/src/ur/version_information.cpp +++ b/src/ur/version_information.cpp @@ -27,24 +27,11 @@ //---------------------------------------------------------------------- #include +#include #include namespace urcl { -std::vector splitString(std::string input, const std::string& delimiter) -{ - std::vector result; - size_t pos = 0; - std::string substring; - while ((pos = input.find(delimiter)) != std::string::npos) - { - substring = input.substr(0, pos); - result.push_back(substring); - input.erase(0, pos + delimiter.length()); - } - result.push_back(input); - return result; -} VersionInformation::VersionInformation() { @@ -58,7 +45,7 @@ VersionInformation::VersionInformation() VersionInformation VersionInformation::fromString(const std::string& str) { - auto components = splitString(str); + auto components = splitString(str, "."); VersionInformation info; if (components.size() >= 2) { @@ -149,4 +136,4 @@ bool operator>=(const VersionInformation& v1, const VersionInformation& v2) { return !(v1 < v2); } -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/tests/test_helpers.cpp b/tests/test_helpers.cpp index dfae50647..7b98061f3 100644 --- a/tests/test_helpers.cpp +++ b/tests/test_helpers.cpp @@ -59,3 +59,30 @@ TEST(TestHelpers, test_parse_boolean) EXPECT_FALSE(parseBoolean("0")); EXPECT_THROW(parseBoolean("notabool"), urcl::UrException); } + +TEST(TestHelpers, splitString) +{ + std::vector test_vec{ "this", "is", "very", "simple" }; + { + std::string combined = test_vec[0]; + for (std::size_t i = 1; i < test_vec.size(); ++i) + { + combined += "," + test_vec[i]; + } + + EXPECT_EQ(test_vec, urcl::splitString(combined, ",")); + } + { + std::string combined = test_vec[0]; + for (std::size_t i = 1; i < test_vec.size(); ++i) + { + combined += "--?--" + test_vec[i]; + } + + EXPECT_EQ(test_vec, urcl::splitString(combined, "--?--")); + } + + const std::string version_string1 = "5.12.0.1101319"; + std::vector expected = { "5", "12", "0", "1101319" }; + EXPECT_EQ(expected, splitString(version_string1, ".")); +} diff --git a/tests/test_version_information.cpp b/tests/test_version_information.cpp index df12dedf8..ec2b70865 100644 --- a/tests/test_version_information.cpp +++ b/tests/test_version_information.cpp @@ -33,14 +33,6 @@ using namespace urcl; -TEST(version_information, test_split) -{ - const std::string version_string1 = "5.12.0.1101319"; - std::vector expected = { "5", "12", "0", "1101319" }; - - EXPECT_EQ(expected, splitString(version_string1)); -} - TEST(version_information, string_parsing) { const std::string version_string_full = "5.12.1.1234"; From 80a395d1679b7ee7f92d338ba0576b80ee5647fa Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 10 Feb 2026 16:44:23 +0100 Subject: [PATCH 2/5] Add fake RTDE server for testing --- tests/CMakeLists.txt | 5 +- tests/fake_rtde_server.cpp | 233 ++++++++++++++++++++++++++++++++ tests/fake_rtde_server.h | 54 ++++++++ tests/fake_rtde_server_main.cpp | 13 ++ 4 files changed, 304 insertions(+), 1 deletion(-) create mode 100644 tests/fake_rtde_server.cpp create mode 100644 tests/fake_rtde_server.h create mode 100644 tests/fake_rtde_server_main.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 5a3e261bc..b60f04e88 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -14,6 +14,9 @@ FetchContent_Declare( set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) FetchContent_MakeAvailable(googletest) +add_executable(fake_rtde_server fake_rtde_server.cpp fake_rtde_server_main.cpp) +target_link_libraries(fake_rtde_server PRIVATE ur_client_library::urcl) + include(GoogleTest) option(INTEGRATION_TESTS "Build the integration tests that require a running robot / URSim" OFF) @@ -25,7 +28,7 @@ if (INTEGRATION_TESTS) find_package(Python3 COMPONENTS Interpreter REQUIRED) add_custom_target(generate_outputs ALL COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/resources/generate_rtde_outputs.py) - add_executable(rtde_tests test_rtde_client.cpp) + add_executable(rtde_tests test_rtde_client.cpp fake_rtde_server.cpp) add_dependencies(rtde_tests generate_outputs) target_link_libraries(rtde_tests PRIVATE ur_client_library::urcl GTest::gtest_main) gtest_add_tests(TARGET rtde_tests diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp new file mode 100644 index 000000000..a7524465a --- /dev/null +++ b/tests/fake_rtde_server.cpp @@ -0,0 +1,233 @@ +#include "fake_rtde_server.h" +#include +#include "ur_client_library/comm/package_serializer.h" +#include "ur_client_library/log.h" + +namespace urcl +{ + +RTDEServer::RTDEServer(const int port) : server_(port) +{ + start_time_ = std::chrono::steady_clock::now(); + server_.setMessageCallback(std::bind(&RTDEServer::messageCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + server_.setConnectCallback(std::bind(&RTDEServer::connectionCallback, this, std::placeholders::_1)); + server_.setDisconnectCallback(std::bind(&RTDEServer::disconnectionCallback, this, std::placeholders::_1)); + server_.setMaxClientsAllowed(1); + server_.start(); +} + +RTDEServer::~RTDEServer() +{ + stopSendingDataPackages(); +} + +void RTDEServer::connectionCallback(const socket_t filedescriptor) +{ + client_socket_ = filedescriptor; + URCL_LOG_INFO("Client connected to RTDE server on FD %d", filedescriptor); +} +void RTDEServer::disconnectionCallback(const socket_t filedescriptor) +{ + URCL_LOG_INFO("Client disconnected from RTDE server on FD %d", filedescriptor); + stopSendingDataPackages(); +} +void RTDEServer::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv) +{ + comm::BinParser bp(reinterpret_cast(buffer), nbytesrecv); + rtde_interface::PackageHeader::_package_size_type size; + rtde_interface::PackageType type; + bp.parse(size); + bp.parse(type); + + switch (type) + { + case rtde_interface::PackageType::RTDE_REQUEST_PROTOCOL_VERSION: + { + bool accepted = true; + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_REQUEST_PROTOCOL_VERSION, sizeof(uint8_t)); + size += serializer.serialize(buffer + size, accepted); + + size_t written; + server_.write(filedescriptor, buffer, size, written); + break; + } + case rtde_interface::PackageType::RTDE_GET_URCONTROL_VERSION: + { + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_GET_URCONTROL_VERSION, 4 * sizeof(uint32_t)); + uint32_t version = 10; + size += serializer.serialize(buffer + size, version); // major + size += serializer.serialize(buffer + size, version); // minor + size += serializer.serialize(buffer + size, version); // bugfix + size += serializer.serialize(buffer + size, version); // build + + size_t written; + server_.write(filedescriptor, buffer, size, written); + break; + } + case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: + { + bp.parse(output_frequency_); + URCL_LOG_DEBUG("Frequency is set to %f", output_frequency_); + std::string variable_names_str; + bp.parseRemainder(variable_names_str); + output_recipe_ = splitString(variable_names_str); + + output_data_package_ = std::make_unique(output_recipe_); + output_data_package_->initEmpty(); + + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS, + variable_names_str.length() + sizeof(uint8_t)); + uint8_t recipe_id = 1; + size += serializer.serialize(buffer + size, recipe_id); + size += serializer.serialize(buffer + size, + variable_names_str); // We return the variable + // names list directly. For the initialization process, it is + // only important, that no field is "NOT_FOUND". + + size_t written; + server_.write(filedescriptor, buffer, size, written); + URCL_LOG_INFO("Output recipe set"); + break; + } + case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS: + { + std::string variable_names_str; + bp.parseRemainder(variable_names_str); + input_recipe_ = splitString(variable_names_str); + + input_data_package_ = std::make_unique(input_recipe_); + + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS, + variable_names_str.length() + sizeof(uint8_t)); + uint8_t recipe_id = 1; + size += serializer.serialize(buffer + size, recipe_id); + size += serializer.serialize(buffer + size, + variable_names_str); // We return the variable + // names list directly. For the initialization process, it is + // only important, that no field is "NOT_FOUND". + + size_t written; + server_.write(filedescriptor, buffer, size, written); + + URCL_LOG_INFO("Input recipe set with %zu variables.", input_recipe_.size()); + break; + } + case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_START: + { + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_START, sizeof(uint8_t)); + bool accepted = true; + size += serializer.serialize(buffer + size, accepted); + + size_t written; + server_.write(filedescriptor, buffer, size, written); + startSendingDataPackages(); + break; + } + case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_PAUSE: + { + comm::PackageSerializer serializer; + uint8_t buffer[4096]; + size_t size = 0; + size += rtde_interface::PackageHeader::serializeHeader( + buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_PAUSE, sizeof(uint8_t)); + bool accepted = true; + size += serializer.serialize(buffer + size, accepted); + + size_t written; + server_.write(filedescriptor, buffer, size, written); + stopSendingDataPackages(); + break; + } + case rtde_interface::PackageType::RTDE_DATA_PACKAGE: + { + if (input_data_package_ == nullptr) + { + throw std::runtime_error("Fake RTDE Server received a data package before input recipe was setup. This should " + "not happen."); + } + input_data_package_->parseWith(bp); + actOnInput(); + break; + } + case rtde_interface::PackageType::RTDE_TEXT_MESSAGE: + { + URCL_LOG_WARN("Received Text message which usually shouldn't be sent to the RTDE server."); + break; + } + default: + { + URCL_LOG_WARN("Received unknown package type %d", static_cast(type)); + break; + } + } +} + +void RTDEServer::startSendingDataPackages() +{ + URCL_LOG_INFO("Start sending data."); + send_loop_running_ = true; + send_thread_ = std::thread(&RTDEServer::sendDataLoop, this); +} + +void RTDEServer::stopSendingDataPackages() +{ + URCL_LOG_INFO("Stop sending data."); + send_loop_running_ = false; + if (send_thread_.joinable()) + { + send_thread_.join(); + } +} + +void RTDEServer::sendDataLoop() +{ + while (send_loop_running_) + { + if (output_data_package_ != nullptr) + { + std::lock_guard data_lock(output_data_mutex_); + double timestamp = std::chrono::duration(std::chrono::steady_clock::now() - start_time_).count(); + output_data_package_->setData("timestamp", timestamp); + uint8_t buffer[65536]; + size_t size = output_data_package_->serializePackage(buffer); + size_t written; + server_.write(client_socket_, buffer, size, written); + } + std::this_thread::sleep_for(std::chrono::duration(1.0 / output_frequency_)); + } +} + +void RTDEServer::actOnInput() +{ + // This is not complete! + if (std::find(input_recipe_.begin(), input_recipe_.end(), "speed_slider_mask") != input_recipe_.end()) + { + double speed_slider_fraction = 0.0; + input_data_package_->getData("speed_slider_fraction", speed_slider_fraction); + std::lock_guard data_lock(output_data_mutex_); + output_data_package_->setData("target_speed_fraction", speed_slider_fraction); + } +} + +} // namespace urcl diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h new file mode 100644 index 000000000..99a072f6c --- /dev/null +++ b/tests/fake_rtde_server.h @@ -0,0 +1,54 @@ +#pragma once + +#include +#include + +#include "ur_client_library/comm/tcp_server.h" +#include "ur_client_library/rtde/rtde_package.h" +#include "ur_client_library/rtde/rtde_parser.h" + +namespace urcl +{ + +class RTDEServer +{ +public: + RTDEServer() = delete; + explicit RTDEServer(const int port); + + ~RTDEServer(); + + void startSendingDataPackages(); + void stopSendingDataPackages(); + +private: + std::vector input_recipe_; + std::vector output_recipe_; + + std::unique_ptr output_data_package_; + std::unique_ptr input_data_package_; + comm::TCPServer server_; + + std::thread send_thread_; + + virtual void connectionCallback(const socket_t filedescriptor); + + virtual void disconnectionCallback(const socket_t filedescriptor); + + virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv); + + void sendDataLoop(); + std::atomic send_loop_running_; + + double output_frequency_; + + std::chrono::steady_clock::time_point start_time_; + + socket_t client_socket_; + + void actOnInput(); + + std::mutex output_data_mutex_; +}; + +} // namespace urcl diff --git a/tests/fake_rtde_server_main.cpp b/tests/fake_rtde_server_main.cpp new file mode 100644 index 000000000..30d5664b6 --- /dev/null +++ b/tests/fake_rtde_server_main.cpp @@ -0,0 +1,13 @@ +#include "fake_rtde_server.h" +#include + +int main() +{ + urcl::RTDEServer server(30004); + + while (true) + { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + return 0; +} From aac3163c80f7c9689d1ec7f01f1702b8254400ae Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 11 Feb 2026 11:19:51 +0100 Subject: [PATCH 3/5] Add port parameter for RTDEClient Before, it was tied to port 30004. However, it might be good to attach it to another port, e.g. to do local testing with a custom server. --- include/ur_client_library/rtde/rtde_client.h | 6 ++++-- src/rtde/rtde_client.cpp | 13 ++++++++----- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index e2b01c50a..17ad86280 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -105,11 +105,12 @@ class RTDEClient * \param input_recipe_file Path to the file containing the input recipe * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency. * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * \param port Optionally specify a different port * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file, double target_frequency = 0.0, - bool ignore_unavailable_outputs = false); + bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT); /*! * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the @@ -121,11 +122,12 @@ class RTDEClient * \param input_recipe Vector containing the input recipe * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency. * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available + * \param port Optionally specify a different port * from the robot: output is silently ignored if true, a UrException is raised otherwise. */ RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector& output_recipe, const std::vector& input_recipe, double target_frequency = 0.0, - bool ignore_unavailable_outputs = false); + bool ignore_unavailable_outputs = false, const uint32_t port = UR_RTDE_PORT); ~RTDEClient(); /*! * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 8f386956a..e37993a39 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -31,14 +31,16 @@ #include "ur_client_library/log.h" #include #include +#include namespace urcl { namespace rtde_interface { RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, - const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs) - : stream_(robot_ip, UR_RTDE_PORT) + const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs, + const uint32_t port) + : stream_(robot_ip, port) , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file))) , ignore_unavailable_outputs_(ignore_unavailable_outputs) , parser_(output_recipe_) @@ -61,8 +63,8 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector& output_recipe, const std::vector& input_recipe, double target_frequency, - bool ignore_unavailable_outputs) - : stream_(robot_ip, UR_RTDE_PORT) + bool ignore_unavailable_outputs, const uint32_t port) + : stream_(robot_ip, port) , output_recipe_(ensureTimestampIsPresent(output_recipe)) , ignore_unavailable_outputs_(ignore_unavailable_outputs) , input_recipe_(input_recipe) @@ -286,7 +288,8 @@ void RTDEClient::setTargetFrequency() else if (target_frequency_ <= 0.0 || target_frequency_ > max_frequency_) { // Target frequency outside valid range - throw UrException("Invalid target frequency of RTDE connection"); + std::string error = "Invalid target frequency of RTDE connection: " + std::to_string(target_frequency_); + throw UrException(error.c_str()); } } From 6f2e80a8cf5b770b40946a3f9ad8d285e0f968fe Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 11 Feb 2026 14:51:15 +0100 Subject: [PATCH 4/5] Added an integration test reconnecting the RTDEClient The integration test uses the fake RTDEServer --- include/ur_client_library/rtde/rtde_client.h | 13 ++- src/rtde/rtde_client.cpp | 30 +++--- tests/fake_rtde_server.cpp | 5 + tests/fake_rtde_server.h | 2 + tests/test_rtde_client.cpp | 102 ++++++++++++++++++- 5 files changed, 136 insertions(+), 16 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 17ad86280..cad076189 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -84,7 +84,8 @@ enum class ClientState INITIALIZING = 1, INITIALIZED = 2, RUNNING = 3, - PAUSED = 4 + PAUSED = 4, + CONNECTION_LOST = 5 }; /*! @@ -237,6 +238,11 @@ class RTDEClient // Reads output or input recipe from a file static std::vector readRecipe(const std::string& recipe_file); + ClientState getClientState() const + { + return client_state_; + } + private: comm::URStream stream_; std::vector output_recipe_; @@ -302,6 +308,11 @@ class RTDEClient */ void reconnect(); void reconnectCallback(); + + size_t max_connection_attempts_; + std::chrono::milliseconds reconnection_timeout_; + size_t max_initialization_attempts_; + std::chrono::milliseconds initialization_timeout_; }; } // namespace rtde_interface diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index e37993a39..02d90819d 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -105,6 +105,11 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m return true; } + max_connection_attempts_ = max_connection_attempts; + reconnection_timeout_ = reconnection_timeout; + max_initialization_attempts_ = max_initialization_attempts; + initialization_timeout_ = initialization_timeout; + prod_->setReconnectionCallback(nullptr); unsigned int attempts = 0; @@ -135,23 +140,25 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) { // The state initializing is used inside disconnect to stop the pipeline again. - client_state_ = ClientState::INITIALIZING; // A running pipeline is needed inside setup. + client_state_ = ClientState::UNINITIALIZED; 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()); + URCL_LOG_ERROR("Caught exception '%s', while trying to initialize pipeline", exc.what()); return false; } pipeline_->run(); + client_state_ = ClientState::INITIALIZING; uint16_t protocol_version = negotiateProtocolVersion(); // Protocol version must be above zero if (protocol_version == 0) { + client_state_ = ClientState::UNINITIALIZED; return false; } @@ -751,22 +758,21 @@ RTDEWriter& RTDEClient::getWriter() void RTDEClient::reconnect() { URCL_LOG_INFO("Reconnecting to the RTDE interface"); - client_state_ = ClientState::UNINITIALIZED; // 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_; + client_state_ = ClientState::CONNECTION_LOST; disconnect(); - const size_t max_initialization_attempts = 3; size_t cur_initialization_attempt = 0; bool client_reconnected = false; - while (cur_initialization_attempt < max_initialization_attempts) + while (cur_initialization_attempt < max_initialization_attempts_) { bool is_communication_setup = false; try { - is_communication_setup = setupCommunication(1, std::chrono::milliseconds{ 10000 }); + is_communication_setup = setupCommunication(max_connection_attempts_, reconnection_timeout_); } catch (const UrException& exc) { @@ -789,24 +795,22 @@ void RTDEClient::reconnect() 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()); + URCL_LOG_ERROR("Failed to connect to the RTDE server, retrying in %i seconds", reconnection_timeout_.count()); } else { - URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %i second", duration.count()); + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %i second", initialization_timeout_.count()); cur_initialization_attempt += 1; } disconnect(); auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < duration) + while (std::chrono::steady_clock::now() - start_time < initialization_timeout_) { std::this_thread::sleep_for(std::chrono::milliseconds(250)); if (stop_reconnection_) @@ -820,12 +824,14 @@ void RTDEClient::reconnect() if (client_reconnected == false) { URCL_LOG_ERROR("Failed to initialize RTDE client after %i attempts, unable to reconnect", - max_initialization_attempts); + max_initialization_attempts_); disconnect(); reconnecting_ = false; return; } + URCL_LOG_INFO("Successfully reconnected to the RTDE interface, starting communication again"); + start(); if (cur_client_state == ClientState::PAUSED) { diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index a7524465a..f74735175 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -230,4 +230,9 @@ void RTDEServer::actOnInput() } } +void RTDEServer::setStartTime(const std::chrono::steady_clock::time_point& start_time) +{ + start_time_ = start_time; +} + } // namespace urcl diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index 99a072f6c..72be6d59a 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -21,6 +21,8 @@ class RTDEServer void startSendingDataPackages(); void stopSendingDataPackages(); + void setStartTime(const std::chrono::steady_clock::time_point& start_time); + private: std::vector input_recipe_; std::vector output_recipe_; diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 37d1296bf..3b77f2dbd 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -29,8 +29,9 @@ #include #include #include +#include #include -#include +#include #include #include #include @@ -39,16 +40,20 @@ #include #include +#include "fake_rtde_server.h" + using namespace urcl; std::string g_ROBOT_IP = "192.168.56.101"; +uint32_t g_FAKE_RTDE_PORT = 13875; class RTDEClientTest : public ::testing::Test { protected: void SetUp() { - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_)); } void TearDown() @@ -304,6 +309,97 @@ TEST_F(RTDEClientTest, get_data_package) client_->pause(); } +TEST_F(RTDEClientTest, get_data_package_fake_server) +{ + auto fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); + // Skip the bootup check. If uptime is less then 40 seconds, data is read for one second to + // check for safety reset. + fake_rtde_server->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, + resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); + client_->init(); + client_->start(); + + // Test that we can receive a package and extract data from the received package + const std::chrono::milliseconds read_timeout{ 100 }; + std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); + if (data_pkg == nullptr) + { + std::cout << "Failed to get data package from robot" << std::endl; + GTEST_FAIL(); + } + + urcl::vector6d_t actual_q; + EXPECT_TRUE(data_pkg->getData("actual_q", actual_q)); + + URCL_LOG_INFO("Received data package from fake server: %s", data_pkg->toString().c_str()); + client_.reset(); +} + +TEST_F(RTDEClientTest, reconnect_fake_server) +{ + auto fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); + // Skip the bootup check. If uptime is less then 40 seconds, data is read for one second to + // check for safety reset. + fake_rtde_server->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, + resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); + client_->init(0, std::chrono::milliseconds(123), 3, std::chrono::milliseconds(100)); + URCL_LOG_INFO("Client initiliazed"); + client_->start(); + + std::atomic keep_running = true; + std::thread data_consumer_thread([this, &keep_running]() { + std::unique_ptr data_pkg; + const std::chrono::milliseconds read_timeout{ 100 }; + while (keep_running) + { + data_pkg = client_->getDataPackage(read_timeout); + if (data_pkg) + { + URCL_LOG_INFO(data_pkg->toString().c_str()); + } + else + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + fake_rtde_server.reset(); + auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(10) && + client_->getClientState() != rtde_interface::ClientState::UNINITIALIZED) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + URCL_LOG_INFO("Resetting rtde_server"); + fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); + fake_rtde_server->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(52)); + + start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < std::chrono::seconds(10) && + client_->getClientState() != rtde_interface::ClientState::RUNNING) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + ASSERT_EQ(client_->getClientState(), rtde_interface::ClientState::RUNNING); + + if (data_consumer_thread.joinable()) + { + keep_running = false; + data_consumer_thread.join(); + } + std::unique_ptr data_pkg = client_->getDataPackage(std::chrono::milliseconds(100)); + ASSERT_NE(data_pkg, nullptr); + URCL_LOG_INFO(data_pkg->toString().c_str()); + + client_.reset(); + URCL_LOG_INFO("Done"); +} + TEST_F(RTDEClientTest, write_rtde_data) { client_->init(); @@ -534,4 +630,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 2870b2a6d6e646632359bd316b10f49d49aa469a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 11 Feb 2026 15:27:07 +0100 Subject: [PATCH 5/5] Fix initializing state declaration --- src/rtde/rtde_client.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 02d90819d..ab8e0d7e4 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -139,8 +139,6 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chrono::milliseconds reconnection_time) { - // The state initializing is used inside disconnect to stop the pipeline again. - // A running pipeline is needed inside setup. client_state_ = ClientState::UNINITIALIZED; try { @@ -151,9 +149,12 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron URCL_LOG_ERROR("Caught exception '%s', while trying to initialize pipeline", exc.what()); return false; } + // The state initializing is used inside disconnect to stop the pipeline again. + // A running pipeline is needed inside setup. + client_state_ = ClientState::INITIALIZING; + pipeline_->run(); - client_state_ = ClientState::INITIALIZING; uint16_t protocol_version = negotiateProtocolVersion(); // Protocol version must be above zero if (protocol_version == 0)