From fe05bf58bdbc9494d6eb57390ffa9e4da9394395 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 17 Dec 2025 12:27:02 +0000 Subject: [PATCH 01/38] Avoid allocating memory during parsing Creating new objects on the heap isn't really RT-capable and undeterministic. This adds the possibility to parse incoming data packages into a pre-allocated package to avoid having to create new objects on the fly. To achieve this, the pipeline in the RTDE client has been replaced by a double buffer with two pre-allocated objects. --- CMakeLists.txt | 1 + include/ur_client_library/comm/package.h | 6 +- include/ur_client_library/comm/parser.h | 19 +- include/ur_client_library/comm/pipeline.h | 11 ++ include/ur_client_library/comm/producer.h | 111 ++++++----- .../primary/primary_parser.h | 32 +++- include/ur_client_library/rtde/rtde_client.h | 86 +++++++-- include/ur_client_library/rtde/rtde_package.h | 9 + include/ur_client_library/rtde/rtde_parser.h | 101 +++------- include/ur_client_library/ur/ur_driver.h | 43 ++++- src/rtde/data_package.cpp | 16 +- src/rtde/rtde_client.cpp | 163 +++++++++++----- src/rtde/rtde_parser.cpp | 176 ++++++++++++++++++ src/ur/ur_driver.cpp | 9 +- 14 files changed, 584 insertions(+), 199 deletions(-) create mode 100644 src/rtde/rtde_parser.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 146b74a00..144908276 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,7 @@ add_library(urcl src/rtde/get_urcontrol_version.cpp src/rtde/request_protocol_version.cpp src/rtde/rtde_package.cpp + src/rtde/rtde_parser.cpp src/rtde/text_message.cpp src/rtde/rtde_client.cpp src/ur/ur_driver.cpp diff --git a/include/ur_client_library/comm/package.h b/include/ur_client_library/comm/package.h index f014a37e8..8668acf1f 100644 --- a/include/ur_client_library/comm/package.h +++ b/include/ur_client_library/comm/package.h @@ -35,9 +35,9 @@ namespace urcl namespace comm { /*! - * \brief The URPackage a parent class. From that two implementations are inherited, - * one for the primary, one for the rtde interface (primary_interface::primaryPackage; - * rtde_interface::rtdePackage). The URPackage makes use of the template HeaderT. + * \brief The URPackage is an abstract class. From that two implementations are inherited, + * one for the primary, one for the RTDE interface (primary_interface::PrimaryPackage; + * rtde_interface::RTDEPackage). The URPackage makes use of the template HeaderT. */ template class URPackage diff --git a/include/ur_client_library/comm/parser.h b/include/ur_client_library/comm/parser.h index 66d98d19a..08a868948 100644 --- a/include/ur_client_library/comm/parser.h +++ b/include/ur_client_library/comm/parser.h @@ -41,13 +41,28 @@ class Parser virtual ~Parser() = default; /*! - * \brief Declares the parse function. + * \brief Parses data from a binary parser into a vector of packages. + * + * This will create new packages and push them to the results vector, thus new memory will be + * allocated for each package. Do not use this in a real-time context. * * \param bp Instance of class binaryParser - * \param results A unique pointer + * \param results A vector of unique pointers to packages */ virtual bool parse(BinParser& bp, std::vector>& results) = 0; + /*! + * \brief Parses data from a binary parser into a single package. + * + * This implementation may try to store data in the passed package instance to avoid memory + * allocations. Thus, this function may be used in a real-time context. Refer to the specific + * implementation and parser for details. + * + * \param bp Instance of class binaryParser + * \param result A unique pointer to a package where parsed data should be stored. + */ + virtual bool parse(BinParser& bp, std::unique_ptr& result) = 0; + private: typename T::HeaderType header_; // URProducer producer_; diff --git a/include/ur_client_library/comm/pipeline.h b/include/ur_client_library/comm/pipeline.h index aafd0f754..3b55faab0 100644 --- a/include/ur_client_library/comm/pipeline.h +++ b/include/ur_client_library/comm/pipeline.h @@ -245,6 +245,17 @@ class IProducer * \returns Success of the package production. */ virtual bool tryGet(std::vector>& products) = 0; + + /*! + * \brief Reads a single package from some source and produces the corresponding object. + * + * If possible this function should try to reuse existing memory in the passed unique pointer. + * + * \param product Unique pointer to be set to the produced package. + * + * \returns Success of the package production. + */ + virtual bool tryGet(std::unique_ptr& product) = 0; }; /*! diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 19a929a70..221622071 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -47,6 +47,57 @@ class URProducer : public IProducer bool running_; + template + bool tryGetImpl(ProductT& product) + { + // TODO This function has become really ugly! That should be refactored! + + // 4KB should be enough to hold any packet received from UR + uint8_t buf[4096]; + size_t read = 0; + // expoential backoff reconnects + while (true) + { + if (stream_.read(buf, sizeof(buf), read)) + { + // reset sleep amount + timeout_ = std::chrono::seconds(1); + BinParser bp(buf, read); + return parser_.parse(bp, product); + } + + if (!running_) + return true; + + if (stream_.getState() == SocketState::Connected) + { + continue; + } + + if (stream_.closed()) + return false; + + if (on_reconnect_cb_) + { + URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer"); + on_reconnect_cb_(); + return false; + } + + URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); + std::this_thread::sleep_for(timeout_); + + if (stream_.connect()) + continue; + + auto next = timeout_ * 2; + if (next <= std::chrono::seconds(120)) + timeout_ = next; + } + + return false; + } + public: /*! * \brief Creates a URProducer object, registering a stream and a parser. @@ -106,52 +157,22 @@ class URProducer : public IProducer */ bool tryGet(std::vector>& products) override { - // TODO This function has become really ugly! That should be refactored! - - // 4KB should be enough to hold any packet received from UR - uint8_t buf[4096]; - size_t read = 0; - // expoential backoff reconnects - while (true) - { - if (stream_.read(buf, sizeof(buf), read)) - { - // reset sleep amount - timeout_ = std::chrono::seconds(1); - BinParser bp(buf, read); - return parser_.parse(bp, products); - } - - if (!running_) - return true; - - if (stream_.getState() == SocketState::Connected) - { - continue; - } - - if (stream_.closed()) - return false; - - if (on_reconnect_cb_) - { - URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer"); - on_reconnect_cb_(); - return false; - } - - URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count()); - std::this_thread::sleep_for(timeout_); - - if (stream_.connect()) - continue; - - auto next = timeout_ * 2; - if (next <= std::chrono::seconds(120)) - timeout_ = next; - } + return tryGetImpl(products); + } - return false; + /*! + * \brief Attempts to read byte stream from the robot and parse it as a URPackage. + * + * If supported by the parser, this function will try to reuse existing memory in the passed + * unique pointer. + * + * \param product Unique pointer to hold the produced package + * + * \returns Success of reading and parsing the package + */ + bool tryGet(std::unique_ptr& product) override + { + return tryGetImpl(product); } /*! diff --git a/include/ur_client_library/primary/primary_parser.h b/include/ur_client_library/primary/primary_parser.h index 8f3fbba9d..e9ec70c3b 100644 --- a/include/ur_client_library/primary/primary_parser.h +++ b/include/ur_client_library/primary/primary_parser.h @@ -58,7 +58,7 @@ class PrimaryParser : public comm::Parser * \returns True, if the byte stream could successfully be parsed as primary packages, false * otherwise */ - bool parse(comm::BinParser& bp, std::vector>& results) + bool parse(comm::BinParser& bp, std::vector>& results) override { int32_t packet_size; RobotPackageType type; @@ -150,6 +150,36 @@ class PrimaryParser : public comm::Parser return true; } + /** + * \brief Uses the given BinParser to create a single package object from the contained serialization. + * + * Note: This function assumes that the byte stream contains exactly one primary package. For + * packages with sub-packages this will return false. + * + * \param bp A BinParser holding one serialized primary package + * \param results A unique pointer to hold the created primary package object + * + * \returns True, if the byte stream could successfully be parsed as a primary package, false + * otherwise + */ + bool parse(comm::BinParser& bp, std::unique_ptr& results) override + { + std::vector> packages; + if (!parse(bp, packages)) + { + return false; + } + + if (packages.size() != 1) + { + URCL_LOG_ERROR("Expected exactly one primary package, but received %zu", packages.size()); + return false; + } + + results = std::move(packages[0]); + return true; + } + private: RobotState* stateFromType(RobotStateType type) { diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index cad076189..55299fc83 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -31,21 +31,14 @@ #include -#include "ur_client_library/comm/pipeline.h" -#include "ur_client_library/rtde/package_header.h" -#include "ur_client_library/rtde/rtde_package.h" -#include "ur_client_library/comm/stream.h" -#include "ur_client_library/rtde/rtde_parser.h" #include "ur_client_library/comm/producer.h" +#include "ur_client_library/comm/stream.h" #include "ur_client_library/rtde/data_package.h" -#include "ur_client_library/rtde/request_protocol_version.h" -#include "ur_client_library/rtde/control_package_setup_outputs.h" -#include "ur_client_library/rtde/control_package_start.h" -#include "ur_client_library/log.h" +#include "ur_client_library/rtde/rtde_package.h" +#include "ur_client_library/rtde/rtde_parser.h" #include "ur_client_library/rtde/rtde_writer.h" static const int UR_RTDE_PORT = 30004; -static const std::string PIPELINE_NAME = "RTDE Data Pipeline"; namespace urcl { @@ -97,11 +90,11 @@ class RTDEClient public: RTDEClient() = delete; /*! - * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the + * \brief Creates a new RTDEClient object, including a used URStream and Producer to handle the * communication with the robot. * * \param robot_ip The IP of the robot - * \param notifier The notifier to use in the pipeline + * \param notifier The notifier to notify of start and stop events * \param output_recipe_file Path to the file containing the output recipe * \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. @@ -114,11 +107,11 @@ class RTDEClient 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 + * \brief Creates a new RTDEClient object, including a used URStream and Producer to handle the * communication with the robot. * * \param robot_ip The IP of the robot - * \param notifier The notifier to use in the pipeline + * \param notifier The notifier to notify of start and stop events * \param output_recipe Vector containing the output recipe * \param input_recipe Vector containing the input recipe * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency. @@ -130,6 +123,7 @@ class RTDEClient const std::vector& input_recipe, double target_frequency = 0.0, 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 * used protocol version and setting of input and output recipes. @@ -152,17 +146,31 @@ class RTDEClient /*! * \brief Triggers the robot to start sending RTDE data packages in the negotiated format. * + * \param read_packages_in_background Whether to start a background thread to read packages from. + * If packages are read in the background, getDataPackage() will return the latest one received. + * If packages aren't read in the background, the application is required to call + * getDataPackageBlocking() frequently. + * * \returns Success of the requested start */ - bool start(); + bool start(const bool read_packages_in_background = true); + /*! * \brief Pauses RTDE data package communication * * \returns Whether the RTDE data package communication was paused successfully */ bool pause(); + /*! - * \brief Reads the pipeline to fetch the next data package. + * \brief Return the latest data package received + * + * When packages are read from the background thread, this will return the latest data package + * received from the robot. When no new data has been received since the last call to this + * function, it will wait for the time specified in the \p timeout parameter. + * + * When packages are not read from the background thread, this function will return nullptr and + * print an error message. * * \param timeout Time to wait if no data package is currently in the queue * @@ -170,6 +178,21 @@ class RTDEClient */ std::unique_ptr getDataPackage(std::chrono::milliseconds timeout); + /*! + * \brief Blocking call to get the next data package received from the robot. + * + * This function will block until a new data package is received from the robot and return it. + * + * \param data_package Reference to a unique ptr where the received data package will be stored. + * For optimal performance, the data package pointer should contain a pre-allocated data package + * that was initialized with the same output recipe as used in this RTDEClient. If it is not an + * initialized data package, a new one will be allocated internally which will have a negative + * performance impact and print a warning. + * + * \returns Whether a data package was received successfully + */ + bool getDataPackageBlocking(std::unique_ptr& data_package); + /*! * \brief Getter for the maximum frequency the robot can publish RTDE data packages with. * @@ -235,7 +258,8 @@ class RTDEClient return input_recipe_; } - // Reads output or input recipe from a file + /// Reads output or input recipe from a file and parses it into a vector of strings where each + /// string is a line from the file. static std::vector readRecipe(const std::string& recipe_file); ClientState getClientState() const @@ -243,6 +267,23 @@ class RTDEClient return client_state_; } + /*! \brief Starts a background thread to read data packages from the robot. + * + * After calling this function, getDataPackage() can be used to get the latest data package + * received from the robot. + */ + void startBackgroundRead(); + + /*! \brief Stops the background thread reading data packages from the robot. + * + * After calling this function, getDataPackageBlocking() must be used to get data packages + * from the robot. + * + * \note When getDataPackageBlocking() is not called frequently enough, the internal buffer + * of received data packages might fill up, causing the robot to shutdown the RTDE connection. + */ + void stopBackgroundRead(); + private: comm::URStream stream_; std::vector output_recipe_; @@ -251,7 +292,6 @@ class RTDEClient RTDEParser parser_; std::unique_ptr> prod_; comm::INotifier notifier_; - std::unique_ptr> pipeline_; RTDEWriter writer_; std::atomic reconnecting_; std::atomic stop_reconnection_; @@ -263,6 +303,14 @@ class RTDEClient double max_frequency_; double target_frequency_; + std::unique_ptr data_buffer0_; + std::unique_ptr data_buffer1_; + std::mutex read_mutex_; + std::mutex write_mutex_; + std::atomic new_data_ = false; + std::atomic background_read_running_ = false; + std::thread background_read_thread_; + ClientState client_state_; constexpr static const double CB3_MAX_FREQUENCY = 125.0; @@ -313,6 +361,8 @@ class RTDEClient std::chrono::milliseconds reconnection_timeout_; size_t max_initialization_attempts_; std::chrono::milliseconds initialization_timeout_; + + void backgroundReadThreadFunc(); }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/rtde_package.h b/include/ur_client_library/rtde/rtde_package.h index 23156b990..08019aa13 100644 --- a/include/ur_client_library/rtde/rtde_package.h +++ b/include/ur_client_library/rtde/rtde_package.h @@ -61,6 +61,7 @@ class RTDEPackage : public comm::URPackage * \returns True, if the package was parsed successfully, false otherwise */ virtual bool parseWith(comm::BinParser& bp); + /*! * \brief Produces a human readable representation of the package object. * @@ -68,6 +69,14 @@ class RTDEPackage : public comm::URPackage */ virtual std::string toString() const; + /*! + * \brief Returns the type of the RTDE package. + */ + PackageType getType() const + { + return type_; + } + protected: std::unique_ptr buffer_; size_t buffer_length_; diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 7ea4d131b..416dddd4a 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -56,6 +56,21 @@ class RTDEParser : public comm::Parser } virtual ~RTDEParser() = default; + /*! + * \brief Uses the given BinParser to fill single package object from the contained serialization. + * + * + * \param bp A BinParser holding a serialized RTDE package + * \param result A pointer to the created RTDE package object. Ideally, the passed \p result is a pre-allocated + * package of the type expected to be read. For example, when RTDE communication has been setup it enters the data + * communication phase, where the expected package is a DataPackage. If the package content inside the \p bp object + * being doesn't match the result package's type or if the \p result is a nullptr, a new package will be allocated. + * + * \returns True, if the byte stream could successfully be parsed as an RTDE package, false + * otherwise + */ + bool parse(comm::BinParser& bp, std::unique_ptr& result) override; + /*! * \brief Uses the given BinParser to create package objects from the contained serialization. * @@ -65,93 +80,23 @@ class RTDEParser : public comm::Parser * \returns True, if the byte stream could successfully be parsed as RTDE packages, false * otherwise */ - bool parse(comm::BinParser& bp, std::vector>& results) + bool parse(comm::BinParser& bp, std::vector>& results) override; + void setProtocolVersion(uint16_t protocol_version) { - PackageHeader::_package_size_type size; - PackageType type; - bp.parse(size); - bp.parse(type); - - if (!bp.checkSize(size - sizeof(size) - sizeof(type))) - { - URCL_LOG_ERROR("Buffer len shorter than expected packet length"); - return false; - } - - switch (type) - { - case PackageType::RTDE_DATA_PACKAGE: - { - std::unique_ptr package(new DataPackage(recipe_, protocol_version_)); - - if (!package->parseWith(bp)) - { - URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); - return false; - } - results.push_back(std::move(package)); - break; - } - default: - { - std::unique_ptr package(packageFromType(type)); - if (!package->parseWith(bp)) - { - URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); - return false; - } - - results.push_back(std::move(package)); - break; - } - } - if (!bp.empty()) - { - URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type)); - bp.debug(); - return false; - } - - return true; + protocol_version_ = protocol_version; } - void setProtocolVersion(uint16_t protocol_version) + uint16_t getProtocolVersion() const { - protocol_version_ = protocol_version; + return protocol_version_; } private: std::vector recipe_; - RTDEPackage* packageFromType(PackageType type) - { - switch (type) - { - case PackageType::RTDE_TEXT_MESSAGE: - return new TextMessage(protocol_version_); - break; - case PackageType::RTDE_GET_URCONTROL_VERSION: - return new GetUrcontrolVersion; - break; - case PackageType::RTDE_REQUEST_PROTOCOL_VERSION: - return new RequestProtocolVersion; - break; - case PackageType::RTDE_CONTROL_PACKAGE_PAUSE: - return new ControlPackagePause; - break; - case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS: - return new ControlPackageSetupInputs; - break; - case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: - return new ControlPackageSetupOutputs(protocol_version_); - break; - case PackageType::RTDE_CONTROL_PACKAGE_START: - return new ControlPackageStart; - break; - default: - return new RTDEPackage(type); - } - } + PackageType getPackageTypeFromHeader(comm::BinParser& bp) const; + RTDEPackage* createNewPackageFromType(PackageType type) const; + uint16_t protocol_version_; }; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e61a8e077..5e450dfce 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -449,6 +449,21 @@ class UrDriver */ std::unique_ptr getDataPackage(); + /*! + * \brief Blocking call to get the next data package received from the robot. + * + * This function will block until a new data package is received from the robot and return it. + * + * \param data_package Reference to a unique ptr where the received data package will be stored. + * For optimal performance, the data package pointer should contain a pre-allocated data package + * that was initialized with the same output recipe as used in this RTDEClient. If it is not an + * initialized data package, a new one will be allocated internally which will have a negative + * performance impact and print a warning. + * + * \returns Whether a data package was received successfully + */ + bool getDataPackageBlocking(std::unique_ptr& data_package); + uint32_t getControlFrequency() const { return rtde_client_->getTargetFrequency(); @@ -778,8 +793,12 @@ class UrDriver * * After initialization, the cyclic RTDE communication is not started automatically, so that data * consumers can be started also at a later point. + * + * \param read_packages_in_background If true, RTDE packages will be read in a background thread. + * In this case use getDataPackage() to receive the latest package. If set to false, + * getDataPackageBlocking() has to be called frequently. */ - void startRTDECommunication(); + void startRTDECommunication(const bool read_packages_in_background = true); /*! * \brief Sends a stop command to the socket interface which will signal the program running on @@ -1004,6 +1023,26 @@ class UrDriver return primary_client_; } + /*! \brief Enable or disable background reading of RTDE packages. + * + * When enabled, RTDE packages will be read in a background thread. + * In this case use getDataPackage() to receive the latest package. If set to false, + * getDataPackageBlocking() has to be called frequently if RTDE communication is active. + * + * \param enabled Whether background reading should be enabled or disabled. + */ + void setRTDEBackgroundReadEnabled(const bool enabled) + { + if (enabled) + { + rtde_client_->startBackgroundRead(); + } + else + { + rtde_client_->stopBackgroundRead(); + } + } + private: void init(const UrDriverConfiguration& config); @@ -1044,4 +1083,4 @@ class UrDriver VersionInformation robot_version_; }; } // namespace urcl -#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED \ No newline at end of file +#endif // ifndef UR_CLIENT_LIBRARY_UR_UR_DRIVER_H_INCLUDED diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 055c0fd5d..86be3b563 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -482,9 +482,10 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) { if (g_type_list.find(item) != g_type_list.end()) { - _rtde_type_variant entry = g_type_list[item]; - std::visit([&bp](auto&& arg) { bp.parse(arg); }, entry); - data_[item] = entry; + // _rtde_type_variant entry = g_type_list[item]; + // _rtde_type_variant existing_entry = data_[item]; + + std::visit([&bp](auto&& arg) { bp.parse(arg); }, data_[item]); } else { @@ -500,7 +501,14 @@ std::string rtde_interface::DataPackage::toString() const for (auto& item : data_) { ss << item.first << ": "; - std::visit([&ss](auto&& arg) { ss << arg; }, item.second); + if (std::holds_alternative(item.second)) + { + ss << int(std::get(item.second)); + } + else + { + std::visit([&ss](auto&& arg) { ss << arg; }, item.second); + } ss << std::endl; } return ss.str(); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index ab8e0d7e4..f74bfda47 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -46,7 +46,6 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , parser_(output_recipe_) , prod_(std::make_unique>(stream_, parser_)) , notifier_(notifier) - , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , reconnecting_(false) , stop_reconnection_(false) @@ -71,7 +70,6 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , parser_(output_recipe_) , prod_(std::make_unique>(stream_, parser_)) , notifier_(notifier) - , pipeline_(std::make_unique>(*prod_, PIPELINE_NAME, notifier, true)) , writer_(&stream_, input_recipe_) , reconnecting_(false) , stop_reconnection_(false) @@ -89,6 +87,7 @@ RTDEClient::~RTDEClient() { reconnecting_thread_.join(); } + stopBackgroundRead(); disconnect(); } @@ -128,8 +127,6 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); std::this_thread::sleep_for(initialization_timeout); } - // 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 @@ -140,21 +137,9 @@ 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) { 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()); - return false; - } - // The state initializing is used inside disconnect to stop the pipeline again. - // A running pipeline is needed inside setup. + prod_->setupProducer(max_num_tries, reconnection_time); client_state_ = ClientState::INITIALIZING; - pipeline_->run(); - uint16_t protocol_version = negotiateProtocolVersion(); // Protocol version must be above zero if (protocol_version == 0) @@ -203,7 +188,7 @@ uint16_t RTDEClient::negotiateProtocolVersion() while (num_retries < MAX_REQUEST_RETRIES) { std::unique_ptr package; - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("failed to get package from RTDE interface"); return 0; @@ -253,7 +238,7 @@ bool RTDEClient::queryURControlVersion() std::unique_ptr package; while (num_retries < MAX_REQUEST_RETRIES) { - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("No answer to urcontrol version query was received from robot"); return false; @@ -263,13 +248,14 @@ bool RTDEClient::queryURControlVersion() dynamic_cast(package.get())) { urcontrol_version_ = tmp_urcontrol_version->version_information_; + URCL_LOG_INFO("Received URControl version %s", urcontrol_version_.toString().c_str()); return true; } else { std::stringstream ss; ss << "Did not receive URControl version from robot. Message received instead: " << std::endl - << package->toString() << ". Retrying..."; + << package->toString() << "\n Retrying..."; num_retries++; URCL_LOG_WARN("%s", ss.str().c_str()); } @@ -307,13 +293,8 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) 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_); - pipeline_ = std::make_unique>(*prod_, PIPELINE_NAME, notifier_, true); } bool RTDEClient::setupOutputs(const uint16_t protocol_version) @@ -349,7 +330,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version) } std::unique_ptr package; - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe"); return false; @@ -443,7 +424,7 @@ bool RTDEClient::setupInputs() while (num_retries < MAX_REQUEST_RETRIES) { std::unique_ptr package; - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe"); return false; @@ -501,7 +482,6 @@ void RTDEClient::disconnect() } if (client_state_ >= ClientState::INITIALIZING) { - pipeline_->stop(); } if (client_state_ > ClientState::UNINITIALIZED) { @@ -509,6 +489,8 @@ void RTDEClient::disconnect() writer_.stop(); } client_state_ = ClientState::UNINITIALIZED; + prod_->stopProducer(); + notifier_.stopped("RTDE communication stopped"); } bool RTDEClient::isRobotBooted() @@ -518,7 +500,9 @@ bool RTDEClient::isRobotBooted() if (!sendStart()) return false; - std::unique_ptr package; + std::unique_ptr package = std::make_unique(output_recipe_); + dynamic_cast(package.get())->initEmpty(); + double timestamp = 0; int reading_count = 0; // During bootup the RTDE interface gets restarted once. If we connect to the RTDE interface before that happens, we @@ -530,8 +514,7 @@ bool RTDEClient::isRobotBooted() while (timestamp < 40 && reading_count < target_frequency_ * 2) { // Set timeout based on target frequency, to make sure that reading doesn't timeout - int timeout = static_cast((1 / target_frequency_) * 1000) * 10; - if (pipeline_->getLatestProduct(package, std::chrono::milliseconds(timeout))) + if (prod_->tryGet(package)) { rtde_interface::DataPackage* tmp_input = dynamic_cast(package.get()); tmp_input->getData("timestamp", timestamp); @@ -550,7 +533,7 @@ bool RTDEClient::isRobotBooted() return true; } -bool RTDEClient::start() +bool RTDEClient::start(const bool read_packages_in_background) { if (client_state_ == ClientState::RUNNING) return true; @@ -561,11 +544,15 @@ bool RTDEClient::start() return false; } - pipeline_->run(); - if (sendStart()) { + prod_->startProducer(); + if (read_packages_in_background) + { + startBackgroundRead(); + } client_state_ = ClientState::RUNNING; + notifier_.started("RTDE communication started"); return true; } else @@ -584,6 +571,7 @@ bool RTDEClient::pause() return false; } + stopBackgroundRead(); if (sendPause()) { client_state_ = ClientState::PAUSED; @@ -611,7 +599,7 @@ bool RTDEClient::sendStart() unsigned int num_retries = 0; while (num_retries < MAX_REQUEST_RETRIES) { - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("Could not get response to RTDE communication start request from robot"); return false; @@ -660,7 +648,7 @@ bool RTDEClient::sendPause() int seconds = 5; while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) { - if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000))) + if (!prod_->tryGet(package)) { URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot"); return false; @@ -723,16 +711,27 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages if (reconnect_mutex_.try_lock()) { - std::unique_ptr urpackage; - if (pipeline_->getLatestProduct(urpackage, timeout)) + if (background_read_running_) { - rtde_interface::DataPackage* tmp = dynamic_cast(urpackage.get()); - if (tmp != nullptr) + auto start = std::chrono::steady_clock::now(); + + do { - urpackage.release(); - reconnect_mutex_.unlock(); - return std::unique_ptr(tmp); - } + if (new_data_.load()) + { + std::lock_guard guard(read_mutex_); + reconnect_mutex_.unlock(); + DataPackage* temp = dynamic_cast(data_buffer0_.get()); + new_data_.store(false); + return std::make_unique(*temp); + } + + } while ((std::chrono::steady_clock::now() - start) <= timeout); + } + else + { + URCL_LOG_ERROR("Background reading is not running, cannot get data package. Please either start background " + "reading or use getDataPackageBlocking(...)."); } reconnect_mutex_.unlock(); } @@ -746,6 +745,28 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr return std::unique_ptr(nullptr); } +bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package) +{ + // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages + if (reconnect_mutex_.try_lock()) + { + if (prod_->tryGet(data_package)) + { + reconnect_mutex_.unlock(); + return true; + } + 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 false; +} + std::string RTDEClient::getIP() const { return stream_.getIP(); @@ -856,5 +877,59 @@ void RTDEClient::reconnectCallback() reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this); } +void RTDEClient::startBackgroundRead() +{ + if (background_read_running_) + { + URCL_LOG_WARN("Requested to start RTDEClient's background read, while it is already running. Doing nothing."); + return; + } + background_read_running_ = true; + data_buffer0_ = std::make_unique(output_recipe_); + data_buffer1_ = std::make_unique(output_recipe_); + dynamic_cast(data_buffer0_.get())->initEmpty(); + dynamic_cast(data_buffer1_.get())->initEmpty(); + + background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); +} + +void RTDEClient::stopBackgroundRead() +{ + background_read_running_ = false; + if (background_read_thread_.joinable()) + { + background_read_thread_.join(); + } +} + +void RTDEClient::backgroundReadThreadFunc() +{ + while (background_read_running_) + { + if (prod_->tryGet(data_buffer1_)) + { + rtde_interface::DataPackage* data_pkg = dynamic_cast(data_buffer1_.get()); + if (data_pkg != nullptr) + { + { + std::scoped_lock lock(read_mutex_, write_mutex_); + std::swap(data_buffer0_, data_buffer1_); + } + + new_data_.store(true); + } + else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE) + { + URCL_LOG_INFO(data_buffer1_->toString().c_str()); + } + } + else + { + auto period = std::chrono::duration(1.0 / target_frequency_); + std::this_thread::sleep_for(period); + } + } +} + } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp new file mode 100644 index 000000000..93e616ab1 --- /dev/null +++ b/src/rtde/rtde_parser.cpp @@ -0,0 +1,176 @@ +/* + * Copyright 2025, Universal Robots A/S (refactor) + * + * Copyright 2019, FZI Forschungszentrum Informatik (refactor) + * + * Copyright 2017, 2018 Simon Rasmussen (refactor) + * + * Copyright 2015, 2016 Thomas Timm Andersen (original version) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "ur_client_library/rtde/rtde_parser.h" +#include "ur_client_library/rtde/package_header.h" +#include "ur_client_library/rtde/rtde_package.h" + +namespace urcl +{ +namespace rtde_interface +{ + +bool RTDEParser::parse(comm::BinParser& bp, std::vector>& results) +{ + PackageType type; + try + { + type = getPackageTypeFromHeader(bp); + } + catch (const UrException& e) + { + URCL_LOG_ERROR("Exception during RTDE package parsing: %s", e.what()); + return false; + } + + switch (type) + { + case PackageType::RTDE_DATA_PACKAGE: + { + std::unique_ptr package(new DataPackage(recipe_, protocol_version_)); + dynamic_cast(package.get())->initEmpty(); + + if (!package->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); + return false; + } + results.push_back(std::move(package)); + break; + } + default: + { + std::unique_ptr package(createNewPackageFromType(type)); + if (!package->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); + return false; + } + + results.push_back(std::move(package)); + break; + } + } + if (!bp.empty()) + { + URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type)); + bp.debug(); + return false; + } + + return true; +} + +bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result) +{ + PackageType type = getPackageTypeFromHeader(bp); + + switch (type) + { + case PackageType::RTDE_DATA_PACKAGE: + { + if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE) + { + result = std::make_unique(recipe_, protocol_version_); + dynamic_cast(result.get())->initEmpty(); + URCL_LOG_WARN("The passed result pointer is empty or does not contain a DataPackage. A new DataPackage will " + "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " + "would be sent."); + } + + if (!dynamic_cast(result.get())->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); + return false; + } + break; + } + default: + { + if (result == nullptr || result->getType() != type) + { + result = std::unique_ptr(createNewPackageFromType(type)); + } + if (!result->parseWith(bp)) + { + URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); + return false; + } + + break; + } + } + if (!bp.empty()) + { + URCL_LOG_ERROR("Package of type %d was not parsed completely!", static_cast(type)); + bp.debug(); + return false; + } + + return true; +} + +PackageType RTDEParser::getPackageTypeFromHeader(comm::BinParser& bp) const +{ + PackageHeader::_package_size_type size; + PackageType type; + bp.parse(size); + bp.parse(type); + + if (!bp.checkSize(size - sizeof(size) - sizeof(type))) + { + throw UrException("Buffer len shorter than expected packet length"); + } + return type; +} + +RTDEPackage* RTDEParser::createNewPackageFromType(PackageType type) const +{ + switch (type) + { + case PackageType::RTDE_TEXT_MESSAGE: + return new TextMessage(protocol_version_); + break; + case PackageType::RTDE_GET_URCONTROL_VERSION: + return new GetUrcontrolVersion; + break; + case PackageType::RTDE_REQUEST_PROTOCOL_VERSION: + return new RequestProtocolVersion; + break; + case PackageType::RTDE_CONTROL_PACKAGE_PAUSE: + return new ControlPackagePause; + break; + case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS: + return new ControlPackageSetupInputs; + break; + case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: + return new ControlPackageSetupOutputs(protocol_version_); + break; + case PackageType::RTDE_CONTROL_PACKAGE_START: + return new ControlPackageStart; + break; + default: + return new RTDEPackage(type); + } +} + +} // namespace rtde_interface +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 3f5271f52..9a8a150ad 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -198,6 +198,11 @@ std::unique_ptr urcl::UrDriver::getDataPackage() return rtde_client_->getDataPackage(timeout); } +bool UrDriver::getDataPackageBlocking(std::unique_ptr& data_package) +{ + return rtde_client_->getDataPackageBlocking(data_package); +} + bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode, const RobotReceiveTimeout& robot_receive_timeout) { @@ -580,9 +585,9 @@ bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout) return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE, robot_receive_timeout); } -void UrDriver::startRTDECommunication() +void UrDriver::startRTDECommunication(const bool read_packages_in_background) { - rtde_client_->start(); + rtde_client_->start(read_packages_in_background); } bool UrDriver::stopControl() From 257ed586cb594a2f21d137d2a1803e499de6b41e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 11 Dec 2025 13:15:48 +0100 Subject: [PATCH 02/38] Use a vector instead of unordered map for data storage This should be a lot faster in accessing/storing objects in the data package --- include/ur_client_library/rtde/data_package.h | 26 ++++++++++++++----- src/rtde/data_package.cpp | 22 +++++----------- 2 files changed, 26 insertions(+), 22 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index ce2b991d2..a052d010c 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -144,9 +144,13 @@ class DataPackage : public RTDEPackage template bool getData(const std::string& name, T& val) const { - if (data_.find(name) != data_.end()) + const auto it = + std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { + return element.first == name; + }); + if (it != data_.end()) { - val = std::get(data_.at(name)); + val = std::get(it->second); } else { @@ -170,9 +174,13 @@ class DataPackage : public RTDEPackage { static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable"); - if (data_.find(name) != data_.end()) + const auto it = + std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { + return element.first == name; + }); + if (it != data_.end()) { - val = std::bitset(std::get(data_.at(name))); + val = std::bitset(std::get(it->second)); } else { @@ -194,9 +202,13 @@ class DataPackage : public RTDEPackage template bool setData(const std::string& name, T& val) { - if (data_.find(name) != data_.end()) + const auto it = + std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { + return element.first == name; + }); + if (it != data_.end()) { - data_.at(name) = val; + it->second = val; } else { @@ -219,7 +231,7 @@ class DataPackage : public RTDEPackage // Const would be better here static std::unordered_map g_type_list; uint8_t recipe_id_; - std::unordered_map data_; + std::vector> data_; std::vector recipe_; uint16_t protocol_version_; }; diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 86be3b563..8941a5694 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -462,12 +462,14 @@ std::unordered_map DataPackage::g_ void rtde_interface::DataPackage::initEmpty() { + data_.clear(); + data_.reserve(recipe_.size()); for (auto& item : recipe_) { if (g_type_list.find(item) != g_type_list.end()) { _rtde_type_variant entry = g_type_list[item]; - data_[item] = entry; + data_.push_back({ item, entry }); } } } @@ -478,19 +480,9 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) { bp.parse(recipe_id_); } - for (auto& item : recipe_) + for (size_t i = 0; i < recipe_.size(); ++i) { - if (g_type_list.find(item) != g_type_list.end()) - { - // _rtde_type_variant entry = g_type_list[item]; - // _rtde_type_variant existing_entry = data_[item]; - - std::visit([&bp](auto&& arg) { bp.parse(arg); }, data_[item]); - } - else - { - return false; - } + std::visit([&bp](auto&& arg) { bp.parse(arg); }, data_[i].second); } return true; } @@ -525,11 +517,11 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) size_t size = 0; size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size); size += comm::PackageSerializer::serialize(buffer + size, recipe_id_); - for (auto& item : recipe_) + for (size_t i = 0; i < data_.size(); ++i) { size += std::visit( [&buffer, &size](auto&& arg) -> size_t { return comm::PackageSerializer::serialize(buffer + size, arg); }, - data_[item]); + data_[i].second); } return size; From 46dabfd37a08ff0d386c30717829532db4eb3669 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 18 Dec 2025 11:49:59 +0100 Subject: [PATCH 03/38] Remove double package initialization --- include/ur_client_library/rtde/data_package.h | 1 + src/rtde/rtde_client.cpp | 3 --- src/rtde/rtde_parser.cpp | 2 -- src/rtde/rtde_writer.cpp | 4 ++-- tests/test_rtde_data_package.cpp | 6 ------ 5 files changed, 3 insertions(+), 13 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index a052d010c..471c11613 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -97,6 +97,7 @@ class DataPackage : public RTDEPackage DataPackage(const std::vector& recipe, const uint16_t& protocol_version = 2) : RTDEPackage(PackageType::RTDE_DATA_PACKAGE), recipe_(recipe), protocol_version_(protocol_version) { + initEmpty(); } virtual ~DataPackage() = default; diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index f74bfda47..18c102b1d 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -501,7 +501,6 @@ bool RTDEClient::isRobotBooted() return false; std::unique_ptr package = std::make_unique(output_recipe_); - dynamic_cast(package.get())->initEmpty(); double timestamp = 0; int reading_count = 0; @@ -887,8 +886,6 @@ void RTDEClient::startBackgroundRead() background_read_running_ = true; data_buffer0_ = std::make_unique(output_recipe_); data_buffer1_ = std::make_unique(output_recipe_); - dynamic_cast(data_buffer0_.get())->initEmpty(); - dynamic_cast(data_buffer1_.get())->initEmpty(); background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); } diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 93e616ab1..5a6257075 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -46,7 +46,6 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package(new DataPackage(recipe_, protocol_version_)); - dynamic_cast(package.get())->initEmpty(); if (!package->parseWith(bp)) { @@ -90,7 +89,6 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE) { result = std::make_unique(recipe_, protocol_version_); - dynamic_cast(result.get())->initEmpty(); URCL_LOG_WARN("The passed result pointer is empty or does not contain a DataPackage. A new DataPackage will " "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " "would be sent."); diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 2df149505..95cd49d4b 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -33,8 +33,9 @@ namespace urcl namespace rtde_interface { RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) - : stream_(stream), recipe_(recipe), recipe_id_(0), queue_{ 32 }, running_(false), package_(recipe_) + : stream_(stream), recipe_id_(0), queue_{ 32 }, running_(false), package_(recipe_) { + setInputRecipe(recipe); } void RTDEWriter::setInputRecipe(const std::vector& recipe) @@ -52,7 +53,6 @@ void RTDEWriter::init(uint8_t recipe_id) return; } recipe_id_ = recipe_id; - package_.initEmpty(); running_ = true; writer_thread_ = std::thread(&RTDEWriter::run, this); } diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index e1c34298e..0ccec48fc 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -36,7 +36,6 @@ TEST(rtde_data_package, serialize_pkg) { std::vector recipe{ "speed_slider_mask" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t value = 1; package.setData("speed_slider_mask", value); @@ -59,7 +58,6 @@ TEST(rtde_data_package, parse_pkg_protocolv2) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint8_t data_package[] = { 0x01, 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf, 0xf9, 0x9c, 0x77, 0xd1, 0x10, 0xb4, 0x60, 0xbf, 0xfb, 0xa2, 0x33, 0xd1, 0x10, 0xb4, 0x60, 0xc0, 0x01, 0x9f, 0xbe, 0x68, @@ -93,7 +91,6 @@ TEST(rtde_data_package, parse_pkg_protocolv1) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe, 1); - package.initEmpty(); uint8_t data_package[] = { 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf, 0xf9, 0x9c, 0x77, 0xd1, 0x10, 0xb4, 0x60, 0xbf, 0xfb, 0xa2, 0x33, 0xd1, 0x10, 0xb4, 0x60, 0xc0, 0x01, 0x9f, 0xbe, @@ -126,7 +123,6 @@ TEST(rtde_data_package, get_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t speed_slider_mask; EXPECT_FALSE(package.getData("speed_slider_mask", speed_slider_mask)); @@ -136,7 +132,6 @@ TEST(rtde_data_package, set_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint32_t speed_slider_mask = 1; EXPECT_FALSE(package.setData("speed_slider_mask", speed_slider_mask)); @@ -146,7 +141,6 @@ TEST(rtde_data_package, parse_and_get_bitset_data) { std::vector recipe{ "robot_status_bits" }; rtde_interface::DataPackage package(recipe); - package.initEmpty(); uint8_t data_package[] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x40, 0xb2, 0x3d, 0xa9, 0xfb, 0xe7, 0x6c, 0x8b }; comm::BinParser bp(data_package, sizeof(data_package)); From d3a5756fa60f2246f14d44bbcbb83bcc1bf76830 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 16 Jan 2026 20:39:14 +0100 Subject: [PATCH 04/38] Avoid memory allocations in RTDEWriter Before that change we were basically allocating strings and a new DataPackage on each send operation. This change will preallocate a data package and re-use that for future send operations. The necessary RTDE dictionary keys are also preallocated. --- include/ur_client_library/rtde/rtde_writer.h | 21 +- src/rtde/rtde_writer.cpp | 245 ++++++++++--------- 2 files changed, 145 insertions(+), 121 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index d5af8e5df..24cec82da 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -33,8 +33,8 @@ #include "ur_client_library/rtde/rtde_package.h" #include "ur_client_library/rtde/data_package.h" #include "ur_client_library/comm/stream.h" -#include "ur_client_library/queue/readerwriterqueue.h" #include "ur_client_library/ur/datatypes.h" +#include #include #include @@ -181,15 +181,26 @@ class RTDEWriter bool sendExternalForceTorque(const vector6d_t& external_force_torque); private: + void resetMasks(const std::shared_ptr& buffer); + uint8_t pinToMask(uint8_t pin); comm::URStream* stream_; std::vector recipe_; uint8_t recipe_id_; - moodycamel::BlockingReaderWriterQueue> queue_; + std::shared_ptr data_buffer0_; + std::shared_ptr data_buffer1_; + std::shared_ptr current_store_buffer_; + std::shared_ptr current_send_buffer_; + std::vector used_masks_; std::thread writer_thread_; - bool running_; - DataPackage package_; - std::mutex package_mutex_; + std::atomic running_; + std::mutex store_mutex_; + std::atomic new_data_available_; + std::condition_variable data_available_cv_; + + static std::vector g_preallocated_input_bit_register_keys; + static std::vector g_preallocated_input_int_register_keys; + static std::vector g_preallocated_input_double_register_keys; }; } // namespace rtde_interface diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 95cd49d4b..7277f117d 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -27,32 +27,79 @@ //---------------------------------------------------------------------- #include "ur_client_library/rtde/rtde_writer.h" +#include +#include "ur_client_library/log.h" namespace urcl { namespace rtde_interface { +std::vector RTDEWriter::g_preallocated_input_bit_register_keys; +std::vector RTDEWriter::g_preallocated_input_int_register_keys; +std::vector RTDEWriter::g_preallocated_input_double_register_keys; + RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) - : stream_(stream), recipe_id_(0), queue_{ 32 }, running_(false), package_(recipe_) + : stream_(stream), recipe_id_(0), running_(false) { setInputRecipe(recipe); + + g_preallocated_input_bit_register_keys.resize(128); + for (size_t i = 0; i < 128; ++i) + { + g_preallocated_input_bit_register_keys[i] = "input_bit_register_" + std::to_string(i); + } + + g_preallocated_input_int_register_keys.resize(48); + for (size_t i = 0; i < 48; ++i) + { + g_preallocated_input_int_register_keys[i] = "input_int_register_" + std::to_string(i); + } + + g_preallocated_input_double_register_keys.resize(48); + for (size_t i = 0; i < 48; ++i) + { + g_preallocated_input_double_register_keys[i] = "input_double_register_" + std::to_string(i); + } + + for (const auto& field : recipe) + { + if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") + { + used_masks_.push_back(field); + } + } } void RTDEWriter::setInputRecipe(const std::vector& recipe) { - std::lock_guard guard(package_mutex_); + if (running_) + { + throw UrException("Requesting to change the input recipe while the RTDEWriter is running. The writer has to be " + "stopped before setting the recipe."); + } + std::lock_guard lock_guard(store_mutex_); recipe_ = recipe; - package_ = DataPackage(recipe_); - package_.initEmpty(); + data_buffer0_ = std::make_shared(recipe_); + data_buffer1_ = std::make_shared(recipe_); + + current_store_buffer_ = data_buffer0_; + current_send_buffer_ = data_buffer1_; } void RTDEWriter::init(uint8_t recipe_id) { if (running_) { - return; + throw UrException("Requesting to init a RTDEWriter while it is running. The writer has to be " + "stopped before initializing it."); + } + { + std::lock_guard lock_guard(store_mutex_); + data_buffer0_->setRecipeID(recipe_id); + data_buffer1_->setRecipeID(recipe_id); } recipe_id_ = recipe_id; + new_data_available_ = false; running_ = true; writer_thread_ = std::thread(&RTDEWriter::run, this); } @@ -62,14 +109,21 @@ void RTDEWriter::run() uint8_t buffer[4096]; size_t size; size_t written; - std::unique_ptr package; while (running_) { - if (queue_.waitDequeTimed(package, 1000000)) + std::unique_lock lock(store_mutex_); + auto res = data_available_cv_.wait_for(lock, std::chrono::milliseconds(1000)); + if (res != std::cv_status::timeout) { - package->setRecipeID(recipe_id_); - size = package->serializePackage(buffer); - stream_->write(buffer, size, written); + if (new_data_available_.load() == true && running_.load()) + { + std::swap(current_store_buffer_, current_send_buffer_); + new_data_available_ = false; + lock.unlock(); + size = current_send_buffer_->serializePackage(buffer); + stream_->write(buffer, size, written); + resetMasks(current_send_buffer_); + } } } URCL_LOG_DEBUG("Write thread ended."); @@ -95,21 +149,16 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) return false; } - std::lock_guard guard(package_mutex_); + static const std::string key = "speed_slider_fraction"; + static const std::string mask_key = "speed_slider_mask"; + std::lock_guard guard(store_mutex_); uint32_t mask = 1; bool success = true; - success = package_.setData("speed_slider_mask", mask); - success = success && package_.setData("speed_slider_fraction", speed_slider_fraction); + success = current_store_buffer_->setData(mask_key, mask); + success = success && current_store_buffer_->setData(key, speed_slider_fraction); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("speed_slider_mask", mask); return success; } @@ -123,7 +172,9 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) return false; } - std::lock_guard guard(package_mutex_); + static const std::string key_mask = "standard_digital_output_mask"; + static const std::string key_output = "standard_digital_output"; + std::lock_guard guard(store_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; uint8_t digital_output; @@ -135,18 +186,11 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) { digital_output = 0; } - success = package_.setData("standard_digital_output_mask", mask); - success = success && package_.setData("standard_digital_output", digital_output); + success = current_store_buffer_->setData(key_mask, mask); + success = success && current_store_buffer_->setData(key_output, digital_output); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("standard_digital_output_mask", mask); return success; } @@ -161,7 +205,10 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) return false; } - std::lock_guard guard(package_mutex_); + URCL_LOG_INFO("Write thread started."); + static const std::string key_mask = "configurable_digital_output_mask"; + static const std::string key_output = "configurable_digital_output"; + std::lock_guard guard(store_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; uint8_t digital_output; @@ -173,18 +220,11 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) { digital_output = 0; } - success = package_.setData("configurable_digital_output_mask", mask); - success = success && package_.setData("configurable_digital_output", digital_output); + success = current_store_buffer_->setData(key_mask, mask); + success = success && current_store_buffer_->setData(key_output, digital_output); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("configurable_digital_output_mask", mask); return success; } @@ -198,7 +238,9 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) return false; } - std::lock_guard guard(package_mutex_); + static const std::string key_mask = "tool_digital_output_mask"; + static const std::string key_output = "tool_digital_output"; + std::lock_guard guard(store_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; uint8_t digital_output; @@ -210,18 +252,11 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) { digital_output = 0; } - success = package_.setData("tool_digital_output_mask", mask); - success = success && package_.setData("tool_digital_output", digital_output); + success = current_store_buffer_->setData(key_mask, mask); + success = success && current_store_buffer_->setData(key_output, digital_output); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("tool_digital_output_mask", mask); return success; } @@ -242,29 +277,26 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, cons return false; } - std::lock_guard guard(package_mutex_); + std::lock_guard guard(store_mutex_); uint8_t mask = pinToMask(output_pin); bool success = true; - success = package_.setData("standard_analog_output_mask", mask); + static const std::string key_mask = "standard_analog_output_mask"; + success = current_store_buffer_->setData(key_mask, mask); if (type != AnalogOutputType::SET_ON_TEACH_PENDANT) { + static const std::string key_type = "standard_analog_output_type"; auto output_type_bits = [](const uint8_t pin, const uint8_t type) { return type << pin; }; uint8_t output_type = output_type_bits(output_pin, toUnderlying(type)); - success = success && package_.setData("standard_analog_output_type", output_type); + success = success && current_store_buffer_->setData(key_type, output_type); } - success = success && package_.setData("standard_analog_output_0", value); - success = success && package_.setData("standard_analog_output_1", value); + static const std::string key_output_0 = "standard_analog_output_0"; + success = success && current_store_buffer_->setData(key_output_0, value); + static const std::string key_output_1 = "standard_analog_output_1"; + success = success && current_store_buffer_->setData(key_output_1, value); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } - mask = 0; - success = package_.setData("standard_analog_output_mask", mask); return success; } @@ -288,19 +320,11 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value) return false; } - std::lock_guard guard(package_mutex_); - std::stringstream ss; - ss << "input_bit_register_" << register_id; - - bool success = package_.setData(ss.str(), value); + std::lock_guard guard(store_mutex_); + bool success = current_store_buffer_->setData(g_preallocated_input_bit_register_keys[register_id], value); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } return success; } @@ -314,19 +338,11 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value) return false; } - std::lock_guard guard(package_mutex_); - std::stringstream ss; - ss << "input_int_register_" << register_id; + std::lock_guard guard(store_mutex_); + bool success = current_store_buffer_->setData(g_preallocated_input_int_register_keys[register_id], value); + new_data_available_ = true; + data_available_cv_.notify_one(); - bool success = package_.setData(ss.str(), value); - - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } return success; } @@ -340,34 +356,31 @@ bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value) return false; } - std::lock_guard guard(package_mutex_); - std::stringstream ss; - ss << "input_double_register_" << register_id; - - bool success = package_.setData(ss.str(), value); + std::lock_guard guard(store_mutex_); + bool success = current_store_buffer_->setData(g_preallocated_input_double_register_keys[register_id], value); + new_data_available_ = true; + data_available_cv_.notify_one(); - if (success) - { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } - } return success; } bool RTDEWriter::sendExternalForceTorque(const vector6d_t& external_force_torque) { - std::lock_guard guard(package_mutex_); - bool success = package_.setData("external_force_torque", external_force_torque); - if (success) + static const std::string key = "external_force_torque"; + std::lock_guard guard(store_mutex_); + bool success = current_store_buffer_->setData(key, external_force_torque); + new_data_available_ = true; + data_available_cv_.notify_one(); + return success; +} + +void RTDEWriter::resetMasks(const std::shared_ptr& buffer) +{ + for (const auto& mask_name : used_masks_) { - if (!queue_.tryEnqueue(std::unique_ptr(new DataPackage(package_)))) - { - return false; - } + uint8_t mask = 0; + buffer->setData(mask_name, mask); } - return success; } } // namespace rtde_interface From 5a63cbe6461f947841686fb4d426884aaa566783 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 19 Jan 2026 10:36:15 +0100 Subject: [PATCH 05/38] Use synchronous RTDE client in example --- examples/rtde_client.cpp | 16 ++++++++++------ src/rtde/rtde_writer.cpp | 15 +++++++++++---- tests/test_rtde_writer.cpp | 2 -- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 0972aafc7..9f3b6a380 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -39,7 +39,6 @@ using namespace urcl; const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; -const std::chrono::milliseconds READ_TIMEOUT{ 100 }; void printFraction(const double fraction, const std::string& label, const size_t width = 20) { @@ -84,7 +83,10 @@ int main(int argc, char* argv[]) // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. - my_client.start(); + my_client.start(false); + const std::string key = "target_speed_fraction"; + std::unique_ptr data_pkg = + std::make_unique(my_client.getOutputRecipe()); auto start_time = std::chrono::steady_clock::now(); while (second_to_run <= 0 || @@ -96,13 +98,13 @@ int main(int argc, char* argv[]) // communication doesn't work in which case the user will be notified. // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. - std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); - if (data_pkg) + bool success = my_client.getDataPackageBlocking(data_pkg); + if (success) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. - data_pkg->getData("target_speed_fraction", target_speed_fraction); - printFraction(target_speed_fraction, "target_speed_fraction"); + dynamic_cast(data_pkg.get())->getData(key, target_speed_fraction); + printFraction(target_speed_fraction, key); } else { @@ -139,5 +141,7 @@ int main(int argc, char* argv[]) // Resetting the speedslider back to 100% my_client.getWriter().sendSpeedSlider(1); + URCL_LOG_INFO("Exiting RTDE read/write example."); + return 0; } diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 7277f117d..4de13284b 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -112,10 +112,9 @@ void RTDEWriter::run() while (running_) { std::unique_lock lock(store_mutex_); - auto res = data_available_cv_.wait_for(lock, std::chrono::milliseconds(1000)); - if (res != std::cv_status::timeout) + data_available_cv_.wait(lock, [this] { return new_data_available_.load() || !running_.load(); }); { - if (new_data_available_.load() == true && running_.load()) + if (new_data_available_.load() && running_.load()) { std::swap(current_store_buffer_, current_send_buffer_); new_data_available_ = false; @@ -124,6 +123,10 @@ void RTDEWriter::run() stream_->write(buffer, size, written); resetMasks(current_send_buffer_); } + else + { + lock.unlock(); + } } } URCL_LOG_DEBUG("Write thread ended."); @@ -131,7 +134,11 @@ void RTDEWriter::run() void RTDEWriter::stop() { - running_ = false; + { + std::lock_guard lock(store_mutex_); + running_ = false; + data_available_cv_.notify_one(); + } if (writer_thread_.joinable()) { writer_thread_.join(); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index e0b7e76e6..cad7affe1 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -262,8 +262,6 @@ TEST_F(RTDEWriterTest, send_tool_digital_output) TEST_F(RTDEWriterTest, send_standard_analog_output_unknown_domain) { - waitForMessageCallback(1000); - uint8_t expected_standard_analog_output_mask = 1; uint8_t pin = 0; From ebe2e7f9e4e916df506762ade65df73c2ec6fc22 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jan 2026 07:46:04 +0100 Subject: [PATCH 06/38] Add method to send full DataPackage at once --- include/ur_client_library/rtde/rtde_writer.h | 13 ++++++ src/rtde/rtde_writer.cpp | 42 +++++++++++--------- 2 files changed, 36 insertions(+), 19 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 24cec82da..950e116fd 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -89,6 +89,18 @@ class RTDEWriter */ void stop(); + /*! + * \brief Sends a complete RTDEPackage to the robot. + * + * Use this if multiple values need to be sent at once. When using the other provided functions, + * an RTDE data package will be sent each time. + * + * \param package The package to send + * + * \returns Success of the package creation + */ + bool sendPackage(const DataPackage& package); + /*! * \brief Creates a package to request setting a new value for the speed slider. * @@ -182,6 +194,7 @@ class RTDEWriter private: void resetMasks(const std::shared_ptr& buffer); + void markStorageToBeSent(); uint8_t pinToMask(uint8_t pin); comm::URStream* stream_; diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 4de13284b..061957374 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -145,6 +145,14 @@ void RTDEWriter::stop() } } +bool RTDEWriter::sendPackage(const DataPackage& package) +{ + std::lock_guard guard(store_mutex_); + *current_store_buffer_ = package; + markStorageToBeSent(); + return true; +} + bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) { if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0) @@ -163,8 +171,7 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) bool success = true; success = current_store_buffer_->setData(mask_key, mask); success = success && current_store_buffer_->setData(key, speed_slider_fraction); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -195,8 +202,7 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -212,7 +218,6 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) return false; } - URCL_LOG_INFO("Write thread started."); static const std::string key_mask = "configurable_digital_output_mask"; static const std::string key_output = "configurable_digital_output"; std::lock_guard guard(store_mutex_); @@ -229,8 +234,7 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -261,8 +265,7 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -301,8 +304,7 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, cons success = success && current_store_buffer_->setData(key_output_0, value); static const std::string key_output_1 = "standard_analog_output_1"; success = success && current_store_buffer_->setData(key_output_1, value); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -329,8 +331,7 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_bit_register_keys[register_id], value); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -347,8 +348,7 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_int_register_keys[register_id], value); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -365,8 +365,7 @@ bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_double_register_keys[register_id], value); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -376,8 +375,7 @@ bool RTDEWriter::sendExternalForceTorque(const vector6d_t& external_force_torque static const std::string key = "external_force_torque"; std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(key, external_force_torque); - new_data_available_ = true; - data_available_cv_.notify_one(); + markStorageToBeSent(); return success; } @@ -390,5 +388,11 @@ void RTDEWriter::resetMasks(const std::shared_ptr& buffer) } } +void RTDEWriter::markStorageToBeSent() +{ + new_data_available_ = true; + data_available_cv_.notify_one(); +} + } // namespace rtde_interface } // namespace urcl From f979b42ed581b8c14f8fd0695043ad9a74e239fb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jan 2026 08:27:49 +0100 Subject: [PATCH 07/38] Use a pointer to a DataPackage when receiving the data package --- examples/rtde_client.cpp | 10 ++++++---- include/ur_client_library/rtde/rtde_client.h | 20 +++++++++++++++++++- include/ur_client_library/ur/ur_driver.h | 2 +- src/rtde/rtde_client.cpp | 17 +++++++++++++++-- src/ur/ur_driver.cpp | 2 +- 5 files changed, 42 insertions(+), 9 deletions(-) diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 9f3b6a380..b9b5dfe4e 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -40,6 +40,9 @@ const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; +// Preallocation of string to avoid allocation in main loop +const std::string TARGET_SPEED_FRACTION = "target_speed_fraction"; + void printFraction(const double fraction, const std::string& label, const size_t width = 20) { std::cout << "\r" << label << ": ["; @@ -84,8 +87,7 @@ int main(int argc, char* argv[]) // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. my_client.start(false); - const std::string key = "target_speed_fraction"; - std::unique_ptr data_pkg = + std::unique_ptr data_pkg = std::make_unique(my_client.getOutputRecipe()); auto start_time = std::chrono::steady_clock::now(); @@ -103,8 +105,8 @@ int main(int argc, char* argv[]) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. - dynamic_cast(data_pkg.get())->getData(key, target_speed_fraction); - printFraction(target_speed_fraction, key); + data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); + printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); } else { diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 55299fc83..778c5dc4f 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -178,6 +178,24 @@ class RTDEClient */ std::unique_ptr getDataPackage(std::chrono::milliseconds timeout); + /*! + * \brief Return the latest data package received + * + * When packages are read from the background thread, the latest data package + * received from the robot can be fetched with this. When no new data has been received since the last call to this + * function, it will wait for the time specified in the \p timeout parameter. + * + * When packages are not read from the background thread, this function will return false and + * print an error message. + * + * \param data_package Reference to a DataPackage where the received data package will be stored + * if a package was fetched successfully. + * \param timeout Time to wait if no data package is currently in the queue + * + * \returns Whether a data package was received successfully + */ + bool getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout); + /*! * \brief Blocking call to get the next data package received from the robot. * @@ -191,7 +209,7 @@ class RTDEClient * * \returns Whether a data package was received successfully */ - bool getDataPackageBlocking(std::unique_ptr& data_package); + bool getDataPackageBlocking(std::unique_ptr& data_package); /*! * \brief Getter for the maximum frequency the robot can publish RTDE data packages with. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 5e450dfce..4f9b99f08 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -462,7 +462,7 @@ class UrDriver * * \returns Whether a data package was received successfully */ - bool getDataPackageBlocking(std::unique_ptr& data_package); + bool getDataPackageBlocking(std::unique_ptr& data_package); uint32_t getControlFrequency() const { diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 18c102b1d..cd8dd35d9 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -744,14 +744,26 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr return std::unique_ptr(nullptr); } -bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package) +bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) +{ + if (auto package = getDataPackage(timeout)) + { + data_package = *dynamic_cast(package.get()); + return true; + } + return false; +} + +bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package) { // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages + std::unique_ptr base_package(data_package.release()); if (reconnect_mutex_.try_lock()) { - if (prod_->tryGet(data_package)) + if (prod_->tryGet(base_package)) { reconnect_mutex_.unlock(); + data_package.reset(dynamic_cast(base_package.release())); return true; } reconnect_mutex_.unlock(); @@ -763,6 +775,7 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa std::this_thread::sleep_for(period); } + data_package.reset(dynamic_cast(base_package.release())); return false; } diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 9a8a150ad..566d6911a 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -198,7 +198,7 @@ std::unique_ptr urcl::UrDriver::getDataPackage() return rtde_client_->getDataPackage(timeout); } -bool UrDriver::getDataPackageBlocking(std::unique_ptr& data_package) +bool UrDriver::getDataPackageBlocking(std::unique_ptr& data_package) { return rtde_client_->getDataPackageBlocking(data_package); } From 3264ccbb756410090642a76bc43cccdd5bb90a95 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jan 2026 10:12:35 +0100 Subject: [PATCH 08/38] Retry starting RTDE client on input conflicts This is specific to testing since we restart the RTDE client in very quick succession. While we shutdown the connection from our end, the robot might not have freed the resources, yet. --- include/ur_client_library/exceptions.h | 47 +++++++++++++++++ src/rtde/rtde_client.cpp | 23 ++++++--- ...test_deprecated_ur_driver_construction.cpp | 50 +++++++++++-------- 3 files changed, 93 insertions(+), 27 deletions(-) diff --git a/include/ur_client_library/exceptions.h b/include/ur_client_library/exceptions.h index 2f70aa85a..9554a80ce 100644 --- a/include/ur_client_library/exceptions.h +++ b/include/ur_client_library/exceptions.h @@ -272,5 +272,52 @@ class UnexpectedResponse : public UrException private: std::string text_; }; + +class RTDEInvalidKeyException : public UrException +{ +public: + explicit RTDEInvalidKeyException() : std::runtime_error("RTDE Invalid Key Exception") + { + } + + explicit RTDEInvalidKeyException(const std::string& text) : std::runtime_error(text) + { + } + + virtual ~RTDEInvalidKeyException() = default; + + virtual const char* what() const noexcept override + { + return std::runtime_error::what(); + } +}; + +class RTDEInputConflictException : public UrException +{ +public: + explicit RTDEInputConflictException() : std::runtime_error("RTDE Output Conflict Exception") + { + } + + explicit RTDEInputConflictException(const std::string& key) + : std::runtime_error("RTDE Input Conflict for key: " + key), key_(key) + { + message_ = "Variable '" + key_ + + "' is currently controlled by another RTDE client. The input recipe can't be used as " + "configured. Note: when using a fieldbus (e.g. Ethernet/IP or Modbus), all outputs are claimed by " + "those."; + } + + virtual ~RTDEInputConflictException() = default; + + virtual const char* what() const noexcept override + { + return message_.c_str(); + } + +private: + std::string key_; + std::string message_; +}; } // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index cd8dd35d9..669f0c334 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -162,7 +162,21 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron if (input_recipe_.size() > 0) { - is_rtde_comm_setup = is_rtde_comm_setup && setupInputs(); + try + { + is_rtde_comm_setup = is_rtde_comm_setup && setupInputs(); + } + catch (const RTDEInputConflictException& exc) + { + /* + * If we are starting and shutting down the driver in quick succession, the robot might still + * have some old RTDE connections open. In this case conflicts might occur when we try reserve + * the same input fields again. To mitigate this, we try to setup communication again if + * that error occurs. + */ + URCL_LOG_ERROR("Caught exception %s, while trying to setup RTDE inputs.", exc.what()); + return false; + } } return is_rtde_comm_setup; } @@ -442,14 +456,11 @@ bool RTDEClient::setupInputs() { std::string message = "Variable '" + input_recipe_[i] + "' not recognized by the robot. Probably your input recipe contains errors"; - throw UrException(message); + throw RTDEInvalidKeyException(message); } else if (variable_types[i] == "IN_USE") { - std::string message = "Variable '" + input_recipe_[i] + - "' is currently controlled by another RTDE client. The input recipe can't be used as " - "configured"; - throw UrException(message); + throw RTDEInputConflictException(input_recipe_[i]); } } writer_.init(tmp_input->input_recipe_id_); diff --git a/tests/test_deprecated_ur_driver_construction.cpp b/tests/test_deprecated_ur_driver_construction.cpp index 08b55e65f..3f6ff2936 100644 --- a/tests/test_deprecated_ur_driver_construction.cpp +++ b/tests/test_deprecated_ur_driver_construction.cpp @@ -30,6 +30,7 @@ #include #include +#include #include "ur_client_library/ur/ur_driver.h" const std::string SCRIPT_FILE = "../resources/external_control.urscript"; @@ -44,46 +45,53 @@ void handleRobotProgramState(bool program_running) std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; } -TEST(UrDriverTestDeprecatedConstructor, sigA) +void startDriver(std::function()> constructor_fun) { - std::unique_ptr tool_comm_setup; - auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, - std::move(tool_comm_setup)); + auto driver = constructor_fun(); driver->checkCalibration(CALIBRATION_CHECKSUM); auto version = driver->getVersion(); ASSERT_TRUE(version.major > 0); } +TEST(UrDriverTestDeprecatedConstructor, sigA) +{ + std::unique_ptr tool_comm_setup; + startDriver([&tool_comm_setup]() { + return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup)); + }); +} + TEST(UrDriverTestDeprecatedConstructor, sigB) { std::unique_ptr tool_comm_setup; - auto driver = std::make_shared( - g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, std::bind(&handleRobotProgramState, std::placeholders::_1), - g_HEADLESS, std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003, 50004, 0.025, 0.5); - driver->checkCalibration(CALIBRATION_CHECKSUM); - auto version = driver->getVersion(); - ASSERT_TRUE(version.major > 0); + startDriver([&tool_comm_setup]() { + return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup), 50001, 50002, 2000, 0.03, false, "", 50003, + 50004, 0.025, 0.5); + }); } TEST(UrDriverTestDeprecatedConstructor, sigC) { std::unique_ptr tool_comm_setup; - auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, - std::move(tool_comm_setup), CALIBRATION_CHECKSUM); - auto version = driver->getVersion(); - ASSERT_TRUE(version.major > 0); + startDriver([&tool_comm_setup]() { + return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + std::move(tool_comm_setup), CALIBRATION_CHECKSUM); + }); } TEST(UrDriverTestDeprecatedConstructor, sigD) { std::unique_ptr tool_comm_setup; - auto driver = std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, - std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, - CALIBRATION_CHECKSUM); - auto version = driver->getVersion(); - ASSERT_TRUE(version.major > 0); + startDriver([]() { + return std::make_shared(g_ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, + std::bind(&handleRobotProgramState, std::placeholders::_1), g_HEADLESS, + CALIBRATION_CHECKSUM); + }); } int main(int argc, char* argv[]) From 5cbe7d810f80a040065ff2a942e81f7eb855c7e0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jan 2026 10:41:03 +0100 Subject: [PATCH 09/38] Update RTDEClient example documentation --- doc/examples/rtde_client.rst | 51 +++++++++++++++++++++++++++--------- examples/rtde_client.cpp | 12 ++++----- 2 files changed, 43 insertions(+), 20 deletions(-) diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index bf47d97bf..c71bbeadb 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -25,11 +25,6 @@ to initialize the RTDE client. :start-at: const std::string OUTPUT_RECIPE :end-at: const std::string INPUT_RECIPE - -Internally, the RTDE client uses the same producer / consumer architecture as show in the -:ref:`primary_pipeline_example` example. However, it doesn't have a consumer thread, so data has to -be read by the user to avoid the pipeline's queue from overflowing. - Creating an RTDE Client ----------------------- @@ -45,25 +40,48 @@ omitted, RTDE communication will be established at the robot's control frequency :start-at: comm::INotifier notifier; :end-at: my_client.init(); -An RTDE data package containing every key-value pair from the output recipe can be fetched using -the ``getDataPackage()`` method. This method will block until a new package is available. - Reading data from the RTDE client --------------------------------- -Once the RTDE client is initialized, we'll have to start communication separately. As mentioned -above, we'll have to read data from the client once communication is started, hence we start -communication right before a loop reading data. +To read data received by the RTDE client, it has to be polled. For this two modes are available: + +- **Background read**: When background read is enabled (default), the RTDE client will start a + background thread that continuously reads data from the robot. The latest data package can be + fetched using the ``getDataPackage()`` method. This method returns immediately with the latest + data package received from the robot. If no data has been received since last calling this + function, it will block for a specified timeout waiting for new data to arrive. + + .. note:: This methods allocates a new data package on each call. We recommend using the blocking + read method explained below. +- **Blocking synchronous read**: When background read is not enabled, data can (and has to be) + fetched using the ``getDataPackageBlocking()`` method. This call waits for a new data package to + arrive and parses that into the passed ``DataPackage`` object. This has to be called with the + RTDE control frequency, as the robot will shutdown RTDE communication if data is not read by the + client. + +Which of the above strategies is used can be specified when starting RTDE communication using the +``start()`` method. In our example, we do not use background read and instead fetch data +synchronously. Hence, we pass ``false`` to the ``start()`` method. .. literalinclude:: ../../examples/rtde_client.cpp :language: c++ :caption: examples/rtde_client.cpp :linenos: :lineno-match: - :start-at: // Once RTDE communication is started + :start-at: std::unique_ptr data_pkg = :end-before: // Change the speed slider +In our main loop, we wait for a new data package to arrive using the blocking read method. Once +received, data from the received package can be accessed using the ``getData()`` method of the +``DataPackage`` object. This method takes the key of the data to be accessed as a parameter and +returns the corresponding value. + +.. note:: The key used to access data has to be part of the output recipe used to initialize the RTDE + client. Passing a string literal, e.g. ``"actual_q"``, is possible but not recommended as it is + converted to an ``std::string`` automatically, causing heap allocations which should be avoided + in Real-Time contexts. + Writing Data to the RTDE client ------------------------------- @@ -84,6 +102,13 @@ initialize the RTDE client has to contain the keys necessary to send that specif :end-at: } -.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide +.. note:: Many RTDE inputs require setting up the data key and a mask key. That is done + internally, but the mask keys have to be part of the input recipe, as well. See the `RTDE guide `_ for more information. + +.. note:: Every ``send...`` call to the RTDEWriter triggers a package sent to the robot. If you + want to modify more than one input at a time, it is recommended to use the ``sendPackage()`` + method. That allows setting up the complete data package with its input recipe and sending that + to the robot at once. + diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index b9b5dfe4e..9e0dcffca 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -83,28 +83,26 @@ int main(int argc, char* argv[]) double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; + std::unique_ptr data_pkg = + std::make_unique(my_client.getOutputRecipe()); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. my_client.start(false); - std::unique_ptr data_pkg = - std::make_unique(my_client.getOutputRecipe()); auto start_time = std::chrono::steady_clock::now(); while (second_to_run <= 0 || std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < second_to_run) { - // Read latest RTDE package. This will block for READ_TIMEOUT, so the - // robot will effectively be in charge of setting the frequency of this loop unless RTDE - // communication doesn't work in which case the user will be notified. - // In a real-world application this thread should be scheduled with real-time priority in order - // to ensure that this is called in time. + // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in + // order to ensure that this is called in time. bool success = my_client.getDataPackageBlocking(data_pkg); if (success) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. + // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop. data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); } From 539c28db6fdf07186e173dbb62ada69fea40bf31 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 22 Jan 2026 14:29:24 +0100 Subject: [PATCH 10/38] Use blocking read in frequency test --- tests/test_rtde_client.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 3b77f2dbd..16f9d1912 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -196,27 +196,16 @@ TEST_F(RTDEClientTest, set_target_frequency) double expected_target_frequency = 1; EXPECT_EQ(client_->getTargetFrequency(), expected_target_frequency); - EXPECT_TRUE(client_->start()); + EXPECT_TRUE(client_->start(false)); // Test that we receive packages with a frequency of 2 Hz - const std::chrono::milliseconds read_timeout{ 10000 }; - 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(); - } + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); double first_time_stamp = 0.0; data_pkg->getData("timestamp", first_time_stamp); - data_pkg = client_->getDataPackage(read_timeout); - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } - + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); double second_time_stamp = 0.0; data_pkg->getData("timestamp", second_time_stamp); From d568efedf4f80257b518a2e88ac9937187480c99 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 23 Jan 2026 11:29:29 +0100 Subject: [PATCH 11/38] Always upload coverage info --- .github/workflows/ci.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d0f3be1ba..41e5478db 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -49,6 +49,7 @@ jobs: CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage LDFLAGS: -fprofile-arcs -ftest-coverage - name: build + id: build run: cmake --build build --config Debug - name: Create folder for test artifacts run: mkdir -p test_artifacts @@ -57,7 +58,7 @@ jobs: env: URSIM_VERSION: ${{matrix.env.URSIM_VERSION}} - name: Upload test results to Codecov - if: ${{ !cancelled() }} + if: ${{ !cancelled() && steps.build.outcome == 'success' }} uses: codecov/test-results-action@v1 with: token: ${{ secrets.CODECOV_TOKEN }} @@ -65,10 +66,13 @@ jobs: - name: run examples run: ./run_examples.sh "192.168.56.101" 1 - name: install gcovr + if: ${{ !cancelled() && steps.build.outcome == 'success' }} run: sudo apt-get install -y gcovr - name: gcovr + if: ${{ !cancelled() && steps.build.outcome == 'success' }} run: cd build && gcovr -r .. --gcov-ignore-parse-errors negative_hits.warn_once_per_file --exclude "../3rdparty" - name: Upload coverage to Codecov + if: ${{ !cancelled() && steps.build.outcome == 'success' }} uses: codecov/codecov-action@v5 with: fail_ci_if_error: true From fa204bd7bd4fac6ff5799deee6738be6728dea03 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 23 Jan 2026 14:51:54 +0100 Subject: [PATCH 12/38] Add option to use non-blocking read without heap allocation --- doc/architecture/rtde_client.rst | 64 ++++-- doc/examples/direct_torque_control.rst | 2 +- doc/examples/rtde_client.rst | 23 +- doc/examples/ur_driver.rst | 2 +- examples/direct_torque_control.cpp | 10 +- examples/external_fts_through_rtde.cpp | 5 +- examples/full_driver.cpp | 8 +- examples/rtde_client.cpp | 9 +- examples/spline_example.cpp | 16 +- .../ur_client_library/example_robot_wrapper.h | 31 +-- include/ur_client_library/rtde/rtde_client.h | 8 + include/ur_client_library/ur/ur_driver.h | 20 ++ run_examples.sh | 4 +- src/example_robot_wrapper.cpp | 52 +---- src/rtde/rtde_client.cpp | 72 +++--- src/ur/ur_driver.cpp | 12 + tests/test_rtde_client.cpp | 97 ++++---- tests/test_spline_interpolation.cpp | 213 ++++++++---------- tests/test_ur_driver.cpp | 29 +-- 19 files changed, 312 insertions(+), 365 deletions(-) diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index 497906471..0469bc223 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -7,17 +7,34 @@ RTDEClient The Real Time Data Exchange Client, ``RTDEClient``, class serves as a standalone `RTDE `_ -client. To use the RTDE-Client, you'll have to initialize and start it separately: +client. To use the RTDE-Client, you'll have to initialize and start it separately. When starting +it, it can be chosen whether data should be read in a background thread or if the user has to poll +data in each cycle. + +- **Background read**: When background read is enabled (``start(true)``) (default), the RTDE client + will start a background thread that continuously reads data from the robot. The latest data + package can be fetched using the ``getDataPackage()`` method. This method returns immediately + with the latest data package received from the robot. If no data has been received since last + calling this function, it will block for a specified timeout waiting for new data to arrive. + +- **Blocking synchronous read**: When background read is not enabled (``start(false)``), data can + (and has to be) fetched using the ``getDataPackageBlocking()`` method. This call waits for a new + data package to arrive and parses that into the passed ``DataPackage`` object. This has to be + called with the RTDE control frequency, as the robot will shutdown RTDE communication if data is + not read by the client. + +The following example uses the background read method to fetch data from the RTDE interface. See +the :ref:`rtde_client_example` for an example of the blocking read method. .. code-block:: c++ rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, INPUT_RECIPE_FILE); my_client.init(); - my_client.start(); + my_client.start(true); // Start background read + rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe()); while (true) { - std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); - if (data_pkg) + if (my_client.getDataPackage(data_pkg, READ_TIMEOUT)) { std::cout << data_pkg->toString() << std::endl; } @@ -28,27 +45,32 @@ outputs. Please refer to the `RTDE guide `_ on which elements are available. -.. note:: +The recipes can be either passed as a filename or as a list of strings directly. E.g. the +following will work + +.. code-block:: c++ - The recipes can be either passed as a filename or as a list of strings directly. E.g. the - following will work + rtde_interface::RTDEClient my_client( + ROBOT_IP, + notifier, + {"timestamp", "actual_q"}, + {"speed_slider_mask", "speed_slider_fraction"} + ); + +.. note:: + Remember, that ``timestamp`` always has to be part of the output recipe. - .. code-block:: c++ +Reading data +------------ - rtde_interface::RTDEClient my_client( - ROBOT_IP, - notifier, - {"timestamp", "actual_q"}, - {"speed_slider_mask", "speed_slider_fraction"} - ); +After calling ``my_client.start()``, data can be read from the +``RTDEClient`` by calling ``getDataPackage()`` (with background thread running) or ``getDataPackageBlocking()`` (without background thread running) respectively. -Inside the ``RTDEclient`` data is received in a separate thread, parsed by the ``RTDEParser`` and -added to a pipeline queue. +Remember that, when not using a background thread, data has to be polled regularly, as the robot +will shutdown RTDE communication if the receiving side doesn't empty its buffer. -Right after calling ``my_client.start()``, it should be made sure to read the buffer from the -``RTDEClient`` by calling ``getDataPackage()`` frequently. The Client's queue can only contain a -restricted number of items at a time, so a ``Pipeline producer overflowed!`` error will be raised -if the buffer isn't read frequently enough. +Writing data +------------ For writing data to the RTDE interface, use the ``RTDEWriter`` member of the ``RTDEClient``. It can be retrieved by calling ``getWriter()`` method. The ``RTDEWriter`` provides convenience methods to write @@ -83,7 +105,7 @@ an empty input recipe, like this: // Alternatively, pass an empty filename when using recipe files // rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, ""); my_client.init(); - my_client.start(); + my_client.start(false); while (true) { std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); diff --git a/doc/examples/direct_torque_control.rst b/doc/examples/direct_torque_control.rst index f4d385d4d..10e52950a 100644 --- a/doc/examples/direct_torque_control.rst +++ b/doc/examples/direct_torque_control.rst @@ -77,4 +77,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th :linenos: :lineno-match: :start-at: // Setting the RobotReceiveTimeout - :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str()); diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index c71bbeadb..9e5cc4f6e 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -44,25 +44,9 @@ omitted, RTDE communication will be established at the robot's control frequency Reading data from the RTDE client --------------------------------- -To read data received by the RTDE client, it has to be polled. For this two modes are available: - -- **Background read**: When background read is enabled (default), the RTDE client will start a - background thread that continuously reads data from the robot. The latest data package can be - fetched using the ``getDataPackage()`` method. This method returns immediately with the latest - data package received from the robot. If no data has been received since last calling this - function, it will block for a specified timeout waiting for new data to arrive. - - .. note:: This methods allocates a new data package on each call. We recommend using the blocking - read method explained below. -- **Blocking synchronous read**: When background read is not enabled, data can (and has to be) - fetched using the ``getDataPackageBlocking()`` method. This call waits for a new data package to - arrive and parses that into the passed ``DataPackage`` object. This has to be called with the - RTDE control frequency, as the robot will shutdown RTDE communication if data is not read by the - client. - -Which of the above strategies is used can be specified when starting RTDE communication using the -``start()`` method. In our example, we do not use background read and instead fetch data -synchronously. Hence, we pass ``false`` to the ``start()`` method. +To read data received by the RTDE client, it has to be polled. See the :ref:`rtde_client` section +for details on two possible strategies. In this example, we do not use background read and instead +fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method. .. literalinclude:: ../../examples/rtde_client.cpp :language: c++ @@ -111,4 +95,3 @@ initialize the RTDE client has to contain the keys necessary to send that specif want to modify more than one input at a time, it is recommended to use the ``sendPackage()`` method. That allows setting up the complete data package with its input recipe and sending that to the robot at once. - diff --git a/doc/examples/ur_driver.rst b/doc/examples/ur_driver.rst index 874c49220..0942295c6 100644 --- a/doc/examples/ur_driver.rst +++ b/doc/examples/ur_driver.rst @@ -70,4 +70,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th :linenos: :lineno-match: :start-at: // Setting the RobotReceiveTimeout - :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str()); diff --git a/examples/direct_torque_control.cpp b/examples/direct_torque_control.cpp index f50ce4cbf..d213cc97f 100644 --- a/examples/direct_torque_control.cpp +++ b/examples/direct_torque_control.cpp @@ -96,20 +96,22 @@ int main(int argc, char* argv[]) // loop. g_my_robot->getUrDriver()->startRTDECommunication(); auto start_time = std::chrono::system_clock::now(); + + urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the // robot will effectively be in charge of setting the frequency of this loop. // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (!data_pkg) + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { URCL_LOG_WARN("Could not get fresh data package from robot"); return 1; } // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) + if (!data_pkg.getData("actual_q", g_joint_positions)) { // This throwing should never happen unless misconfigured std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; @@ -146,7 +148,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); return 1; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str()); if (second_to_run.count() > 0 && (std::chrono::system_clock::now() - start_time) > second_to_run) { URCL_LOG_WARN("Time limit reached, stopping movement. This is expected on a simualted robot, as it doesn't move " diff --git a/examples/external_fts_through_rtde.cpp b/examples/external_fts_through_rtde.cpp index f8838f092..dfd22cbb2 100644 --- a/examples/external_fts_through_rtde.cpp +++ b/examples/external_fts_through_rtde.cpp @@ -211,11 +211,12 @@ void rtdeWorker(const int second_to_run) vector6d_t actual_tcp_force; auto start_time = std::chrono::steady_clock::now(); + std::unique_ptr data_pkg = + std::make_unique(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); while (g_RUNNING) { urcl::vector6d_t local_ft_vec = g_FT_VEC; - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (data_pkg) + if (g_my_robot->getUrDriver()->getDataPackageBlocking(data_pkg)) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index d123b5237..1af6ffb61 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -87,20 +87,20 @@ int main(int argc, char* argv[]) // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. g_my_robot->getUrDriver()->startRTDECommunication(); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the // robot will effectively be in charge of setting the frequency of this loop. // In a real-world application this thread should be scheduled with real-time priority in order // to ensure that this is called in time. - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (!data_pkg) + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { URCL_LOG_WARN("Could not get fresh data package from robot"); return 1; } // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) + if (!data_pkg.getData("actual_q", g_joint_positions)) { // This throwing should never happen unless misconfigured std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; @@ -140,7 +140,7 @@ int main(int argc, char* argv[]) URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); return 1; } - URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str()); + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str()); } g_my_robot->getUrDriver()->stopControl(); URCL_LOG_INFO("Movement done"); diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 9e0dcffca..54b0c2c78 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -83,12 +83,11 @@ int main(int argc, char* argv[]) double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; - std::unique_ptr data_pkg = - std::make_unique(my_client.getOutputRecipe()); + rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe()); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. - my_client.start(false); + my_client.start(true); // false -> do not start background read thread. auto start_time = std::chrono::steady_clock::now(); while (second_to_run <= 0 || @@ -97,13 +96,13 @@ int main(int argc, char* argv[]) { // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in // order to ensure that this is called in time. - bool success = my_client.getDataPackageBlocking(data_pkg); + bool success = my_client.getDataPackage(data_pkg, std::chrono::milliseconds(100)); if (success) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop. - data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); + data_pkg.getData(TARGET_SPEED_FRACTION, target_speed_fraction); printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); } else diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp index a29b26772..c9303ffde 100644 --- a/examples/spline_example.cpp +++ b/examples/spline_example.cpp @@ -139,12 +139,12 @@ int main(int argc, char* argv[]) g_my_robot->getUrDriver()->startRTDECommunication(); - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + if (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) - if (data_pkg) { // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) + if (!data_pkg.getData("actual_q", g_joint_positions)) { // This throwing should never happen unless misconfigured std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; @@ -199,11 +199,10 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (data_pkg) + if (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) + if (!data_pkg.getData("actual_q", g_joint_positions)) { // This throwing should never happen unless misconfigured std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; @@ -231,11 +230,10 @@ int main(int argc, char* argv[]) g_trajectory_running = true; while (g_trajectory_running) { - std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); - if (data_pkg) + if (g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { // Read current joint positions from robot data - if (!data_pkg->getData("actual_q", g_joint_positions)) + if (!data_pkg.getData("actual_q", g_joint_positions)) { // This throwing should never happen unless misconfigured std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; diff --git a/include/ur_client_library/example_robot_wrapper.h b/include/ur_client_library/example_robot_wrapper.h index 38b422e95..06c31c202 100644 --- a/include/ur_client_library/example_robot_wrapper.h +++ b/include/ur_client_library/example_robot_wrapper.h @@ -112,34 +112,10 @@ class ExampleRobotWrapper * * @param consume_data Once the RTDE client is started, it's data has to be consumed. If you * don't actually care about that data, this class can silently consume RTDE data when `true` is - * passed. This can be stopped and started at any time using the startConsumingRTDEData() and - * stopConsumingRTDEData() methods. + * passed. */ void startRTDECommununication(const bool consume_data = false); - /** - * @brief Start consuming RTDE data in the background. - */ - void startConsumingRTDEData(); - - /** - * @brief Stop consuming RTDE data in the background. Note that data has to be consumed manually - * using readDataPackage(). - */ - void stopConsumingRTDEData(); - - /** - * @brief Get the latest RTDE package. - * - * Do not call this, while RTDE data is being consumed in the background. In doubt, call - * stopConsumingRTDEData() before calling this function. - * - * @param[out] data_pkg The data package will be stored in that object - * @return true on a successful read, false if no package can be read or when RTDE data is - * already being consumed in the background. - */ - bool readDataPackage(std::unique_ptr& data_pkg); - /** * @brief Blocks until there is a robot program connected to the driver's reverse interface or * until the timeout is hit. @@ -217,9 +193,6 @@ class ExampleRobotWrapper comm::INotifier notifier_; std::atomic rtde_communication_started_ = false; - std::atomic consume_rtde_packages_ = false; - std::mutex read_package_mutex_; - std::unique_ptr data_pkg_; bool robot_initialized_ = false; @@ -229,8 +202,6 @@ class ExampleRobotWrapper std::mutex program_running_mutex_; std::mutex program_not_running_mutex_; - std::thread rtde_consumer_thread_; - bool headless_mode_; std::string autostart_program_; }; diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 778c5dc4f..4bb358f24 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -165,6 +165,9 @@ class RTDEClient /*! * \brief Return the latest data package received * + * \deprecated This method allocates memory on each call. Please use the overload which takes a + * reference to an existing DataPackage instead. This function will be removed in May 2027. + * * When packages are read from the background thread, this will return the latest data package * received from the robot. When no new data has been received since the last call to this * function, it will wait for the time specified in the \p timeout parameter. @@ -176,6 +179,8 @@ class RTDEClient * * \returns Unique ptr to the package, if a package was fetched successfully, nullptr otherwise */ + [[deprecated("This method allocates memory on each call. Please use the overload which takes a reference to an " + "existing DataPackage instead. This function will be removed in May 2027.")]] std::unique_ptr getDataPackage(std::chrono::milliseconds timeout); /*! @@ -328,6 +333,9 @@ class RTDEClient std::atomic new_data_ = false; std::atomic background_read_running_ = false; std::thread background_read_thread_; + std::condition_variable background_read_cv_; + + DataPackage preallocated_data_pkg_; ClientState client_state_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 4f9b99f08..5d4c0425d 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -444,11 +444,31 @@ class UrDriver * \brief Access function to receive the latest data package sent from the robot through RTDE * interface. * + * \deprecated This method allocates memory on each call. Please use the overload which takes a + * reference to an existing DataPackage instead. This function will be removed in May 2027. + * * \returns The latest data package on success, a nullptr if no package can be found inside a preconfigured time * window. */ + [[deprecated("This method allocates memory on each call. Please use the overload which takes a reference to an " + "existing DataPackage instead. This function will be removed in May 2027.")]] std::unique_ptr getDataPackage(); + /*! + * \brief Return the latest RTDE data package received + * + * When packages are read from the background thread, the latest data package + * received from the robot can be fetched with this. * + * When packages are not read from the background thread, this function will return false and + * print an error message. + * + * \param data_package Reference to a DataPackage where the received data package will be stored + * if a package was fetched successfully. + * + * \returns Whether a data package was received successfully + */ + bool getDataPackage(rtde_interface::DataPackage& data_package); + /*! * \brief Blocking call to get the next data package received from the robot. * diff --git a/run_examples.sh b/run_examples.sh index 2b91e3dea..91d537860 100755 --- a/run_examples.sh +++ b/run_examples.sh @@ -23,7 +23,7 @@ if [[ -f "$file" && -x "$file" ]]; then exit 1 fi # Delay for 10 seconds to avoid too fast reconnects - echo "Sleep 10" - sleep 10 + echo "Sleep 1" + sleep 1 fi done diff --git a/src/example_robot_wrapper.cpp b/src/example_robot_wrapper.cpp index d5ba33d98..fed647794 100644 --- a/src/example_robot_wrapper.cpp +++ b/src/example_robot_wrapper.cpp @@ -98,10 +98,6 @@ ExampleRobotWrapper::ExampleRobotWrapper(const std::string& robot_ip, const std: ExampleRobotWrapper::~ExampleRobotWrapper() { - if (rtde_communication_started_) - { - stopConsumingRTDEData(); - } } bool ExampleRobotWrapper::clearProtectiveStop() @@ -236,55 +232,9 @@ void ExampleRobotWrapper::startRTDECommununication(const bool consume_data) { if (!rtde_communication_started_) { - ur_driver_->startRTDECommunication(); + ur_driver_->startRTDECommunication(consume_data); rtde_communication_started_ = true; } - if (consume_data) - { - startConsumingRTDEData(); - } -} - -void ExampleRobotWrapper::startConsumingRTDEData() -{ - consume_rtde_packages_ = true; - rtde_consumer_thread_ = std::thread([this]() { - while (consume_rtde_packages_) - { - // Consume package to prevent pipeline overflow - std::lock_guard lk(read_package_mutex_); - data_pkg_ = ur_driver_->getDataPackage(); - } - }); -} - -void ExampleRobotWrapper::stopConsumingRTDEData() -{ - if (consume_rtde_packages_) - { - consume_rtde_packages_ = false; - if (rtde_consumer_thread_.joinable()) - { - rtde_consumer_thread_.join(); - } - } -} - -bool ExampleRobotWrapper::readDataPackage(std::unique_ptr& data_pkg) -{ - if (consume_rtde_packages_ == true) - { - URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); - return false; - } - std::lock_guard lk(read_package_mutex_); - data_pkg = ur_driver_->getDataPackage(); - if (data_pkg == nullptr) - { - URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); - return false; - } - return true; } bool ExampleRobotWrapper::waitForProgramRunning(int milliseconds) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 669f0c334..990c3bf70 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -29,6 +29,7 @@ #include "ur_client_library/rtde/rtde_client.h" #include "ur_client_library/exceptions.h" #include "ur_client_library/log.h" +#include "ur_client_library/rtde/data_package.h" #include #include #include @@ -51,6 +52,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , stop_reconnection_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) + , preallocated_data_pkg_(output_recipe_) , client_state_(ClientState::UNINITIALIZED) { if (!input_recipe_file.empty()) @@ -75,6 +77,7 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st , stop_reconnection_(false) , max_frequency_(URE_MAX_FREQUENCY) , target_frequency_(target_frequency) + , preallocated_data_pkg_(output_recipe_) , client_state_(ClientState::UNINITIALIZED) { } @@ -306,6 +309,7 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); + preallocated_data_pkg_ = DataPackage(output_recipe_); parser_ = RTDEParser(output_recipe_); prod_ = std::make_unique>(stream_, parser_); @@ -718,30 +722,43 @@ std::vector RTDEClient::ensureTimestampIsPresent(const std::vector< std::unique_ptr RTDEClient::getDataPackage(std::chrono::milliseconds timeout) { - // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages - if (reconnect_mutex_.try_lock()) + if (getDataPackage(preallocated_data_pkg_, timeout)) { - if (background_read_running_) - { - auto start = std::chrono::steady_clock::now(); + // Return a copy of the cached one + return std::make_unique(preallocated_data_pkg_); + } - do - { - if (new_data_.load()) - { - std::lock_guard guard(read_mutex_); - reconnect_mutex_.unlock(); - DataPackage* temp = dynamic_cast(data_buffer0_.get()); - new_data_.store(false); - return std::make_unique(*temp); - } + return std::unique_ptr(nullptr); +} - } while ((std::chrono::steady_clock::now() - start) <= timeout); +bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) +{ + if (!background_read_running_) + { + URCL_LOG_ERROR("Background reading is not running, cannot get data package. Please either start background " + "reading or use getDataPackageBlocking(...)."); + return false; + } + + if (reconnect_mutex_.try_lock()) + { + if (new_data_.load()) + { + std::lock_guard guard(read_mutex_); + data_package = *dynamic_cast(data_buffer0_.get()); + new_data_.store(false); } else { - URCL_LOG_ERROR("Background reading is not running, cannot get data package. Please either start background " - "reading or use getDataPackageBlocking(...)."); + std::unique_lock lock(read_mutex_); + auto wait_result = background_read_cv_.wait_for(lock, timeout); + if (wait_result == std::cv_status::timeout) + { + reconnect_mutex_.unlock(); + return false; + } + data_package = *dynamic_cast(data_buffer0_.get()); + new_data_.store(false); } reconnect_mutex_.unlock(); } @@ -750,23 +767,20 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr 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 false; } - - return std::unique_ptr(nullptr); + return true; } -bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) +bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package) { - if (auto package = getDataPackage(timeout)) + if (background_read_running_) { - data_package = *dynamic_cast(package.get()); - return true; + URCL_LOG_ERROR("Background reading is running, cannot get data package in blocking mode. Please either stop " + "background reading or use getDataPackage(...)."); + return false; } - return false; -} -bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_package) -{ // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages std::unique_ptr base_package(data_package.release()); if (reconnect_mutex_.try_lock()) @@ -917,6 +931,7 @@ void RTDEClient::startBackgroundRead() void RTDEClient::stopBackgroundRead() { background_read_running_ = false; + background_read_cv_.notify_one(); if (background_read_thread_.joinable()) { background_read_thread_.join(); @@ -938,6 +953,7 @@ void RTDEClient::backgroundReadThreadFunc() } new_data_.store(true); + background_read_cv_.notify_one(); } else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 566d6911a..6e0a78c78 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -188,6 +188,8 @@ void UrDriver::init(const UrDriverConfiguration& config) URCL_LOG_DEBUG("Initialization done"); } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::unique_ptr urcl::UrDriver::getDataPackage() { // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control @@ -197,6 +199,16 @@ std::unique_ptr urcl::UrDriver::getDataPackage() return rtde_client_->getDataPackage(timeout); } +#pragma GCC diagnostic pop + +bool UrDriver::getDataPackage(rtde_interface::DataPackage& data_package) +{ + // This can take one of two values, 0ms or 100ms. The large timeout is for when the robot is commanding the control + // loop's timing (read is blocking). The zero timeout is for when the robot is sharing a control loop with + // something else (combined_robot_hw) + std::chrono::milliseconds timeout(get_packet_timeout_); + return rtde_client_->getDataPackage(data_package, timeout); +} bool UrDriver::getDataPackageBlocking(std::unique_ptr& data_package) { diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 16f9d1912..a8c67a074 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -278,14 +278,17 @@ TEST_F(RTDEClientTest, recipe_compairson) } } -TEST_F(RTDEClientTest, get_data_package) +TEST_F(RTDEClientTest, get_data_package_w_background_deprecated) { 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 }; +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" std::unique_ptr data_pkg = client_->getDataPackage(read_timeout); +#pragma GCC diagnostic pop if (data_pkg == nullptr) { std::cout << "Failed to get data package from robot" << std::endl; @@ -298,6 +301,30 @@ TEST_F(RTDEClientTest, get_data_package) client_->pause(); } +TEST_F(RTDEClientTest, get_data_package_w_background) +{ + 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 }; + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + + urcl::vector6d_t actual_q; + EXPECT_TRUE(data_pkg.getData("actual_q", actual_q)); + + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + // Trying to get data with a very short timeout should fail + ASSERT_FALSE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(1))); + + auto data_pkg_ptr = std::make_unique(client_->getOutputRecipe()); + ASSERT_FALSE(client_->getDataPackageBlocking(data_pkg_ptr)); + + client_->pause(); +} + TEST_F(RTDEClientTest, get_data_package_fake_server) { auto fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); @@ -311,17 +338,17 @@ TEST_F(RTDEClientTest, get_data_package_fake_server) // 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) + auto data_pkg = rtde_interface::DataPackage(client_->getOutputRecipe()); + if (!client_->getDataPackage(data_pkg, read_timeout)) { 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)); + EXPECT_TRUE(data_pkg.getData("actual_q", actual_q)); - URCL_LOG_INFO("Received data package from fake server: %s", data_pkg->toString().c_str()); + URCL_LOG_INFO("Received data package from fake server: %s", data_pkg.toString().c_str()); client_.reset(); } @@ -339,14 +366,13 @@ TEST_F(RTDEClientTest, reconnect_fake_server) std::atomic keep_running = true; std::thread data_consumer_thread([this, &keep_running]() { - std::unique_ptr data_pkg; + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); const std::chrono::milliseconds read_timeout{ 100 }; while (keep_running) { - data_pkg = client_->getDataPackage(read_timeout); - if (data_pkg) + if (client_->getDataPackage(data_pkg, read_timeout)) { - URCL_LOG_INFO(data_pkg->toString().c_str()); + URCL_LOG_INFO(data_pkg.toString().c_str()); } else { @@ -381,9 +407,9 @@ TEST_F(RTDEClientTest, reconnect_fake_server) 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()); + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + URCL_LOG_INFO(data_pkg.toString().c_str()); client_.reset(); URCL_LOG_INFO("Done"); @@ -399,15 +425,12 @@ TEST_F(RTDEClientTest, write_rtde_data) // Make sure that the data has been written to the robot 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(); - } + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); std::bitset<18> actual_dig_out_bits; - data_pkg->getData("actual_digital_output_bits", actual_dig_out_bits); + data_pkg.getData("actual_digital_output_bits", actual_dig_out_bits); // If we get the data package to soon the digital output might not have been updated, therefore we get the package a // couple of times @@ -415,8 +438,8 @@ TEST_F(RTDEClientTest, write_rtde_data) int counter = 0; while (actual_dig_out_bits[0] != send_digital_output) { - data_pkg = client_->getDataPackage(read_timeout); - data_pkg->getData("actual_digital_output_bits", actual_dig_out_bits); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + data_pkg.getData("actual_digital_output_bits", actual_dig_out_bits); if (counter == max_tries) { break; @@ -491,16 +514,11 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) // Test that we can receive and parse the timestamp from the received package to prove the setup was successful 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(); - } + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp; - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); client_->pause(); } @@ -572,16 +590,11 @@ TEST_F(RTDEClientTest, empty_input_recipe) // Test that we can receive and parse the timestamp from the received package to prove the setup was successful 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(); - } + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp; - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); EXPECT_FALSE(client_->getWriter().sendStandardDigitalOutput(1, false)); @@ -591,14 +604,8 @@ TEST_F(RTDEClientTest, empty_input_recipe) client_->init(); client_->start(); - data_pkg = client_->getDataPackage(read_timeout); - - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } - EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); EXPECT_FALSE(client_->getWriter().sendStandardDigitalOutput(1, false)); diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 6c6630e66..52f859c6e 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -259,12 +259,11 @@ class SplineInterpolationTest : public ::testing::Test bool trajectory_started = false; double timeout = 1; double cur_time = 0.0; - while (trajectory_started == false) + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + while (trajectory_started == false && g_my_robot->getUrDriver()->getDataPackage(data_pkg)) { - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( @@ -353,18 +352,16 @@ class SplineInterpolationTest : public ::testing::Test TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg != nullptr); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel; @@ -394,11 +391,11 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) double plot_time = 0.0; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( @@ -406,7 +403,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) // Read spline travel time from the robot double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); double spline_travel_step_time = spline_travel_time - old_spline_travel_time; old_spline_travel_time = spline_travel_time; @@ -448,8 +445,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); @@ -457,7 +454,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/cubic_spline_with_end_point_velocity.csv", time_vec, expected_positions, @@ -466,21 +463,20 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling) { - g_my_robot->stopConsumingRTDEData(); // Set speed scaling to 25% to test interpolation with speed scaling active const unsigned int reduse_factor(4); g_my_robot->getUrDriver()->getRTDEWriter().sendSpeedSlider(1.0 / reduse_factor); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("target_speed_fraction", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; @@ -517,15 +513,15 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee const double eps_acc_change(1e-15); while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_speed_fraction", speed_scaling)); // Read spline travel time from the robot double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); double spline_travel_step_time = spline_travel_time - old_spline_travel_time; old_spline_travel_time = spline_travel_time; @@ -610,8 +606,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); // Make sure the velocity is zero when the trajectory has finished - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); @@ -619,7 +615,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/quintic_spline_with_end_point_velocity_speedscaling.csv", time_vec, @@ -629,16 +625,15 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee TEST_F(SplineInterpolationTest, spline_interpolation_cubic) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel; @@ -679,14 +674,14 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) double plot_time = 0.0; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // The data received from the robot is one time step behind where the robot is interpolating spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); @@ -725,7 +720,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/spline_interpolation_cubic.csv", time_vec, expected_positions, @@ -734,17 +729,16 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) TEST_F(SplineInterpolationTest, spline_interpolation_quintic) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions, joint_velocities, joint_acc; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); double speed_scaling; - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; @@ -792,14 +786,14 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) g_trajectory_running = true; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); - ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); - ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg.getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg.getData("speed_scaling", speed_scaling)); double spline_travel_time = 0.0; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); // The data received from the robot is one time step behind where the robot is interpolating spline_travel_time = (spline_travel_time == 0 ? spline_travel_time : spline_travel_time - step_time_); @@ -837,7 +831,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) // Verify that the full trajectory have been executed double spline_travel_time; - data_pkg->getData("output_double_register_1", spline_travel_time); + data_pkg.getData("output_double_register_1", spline_travel_time); ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5); writeTrajectoryToFile("../test_artifacts/spline_interpolation_quintic.csv", time_vec, expected_positions, @@ -846,15 +840,11 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create illegal trajectory std::vector s_pos, s_vel; @@ -884,13 +874,10 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -913,15 +900,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_NEAR(joint_positions[i], third_point[i], eps_); @@ -930,15 +917,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create illegal trajectory std::vector s_pos, s_vel, s_acc; @@ -964,13 +947,10 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -994,15 +974,15 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) urcl::vector6d_t joint_positions; while (g_trajectory_running) { - g_my_robot->readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions)); // Keep connection alive ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage( urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); } EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_NEAR(joint_positions[i], third_point[i], eps_); @@ -1011,15 +991,11 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel; @@ -1044,13 +1020,10 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -1059,15 +1032,11 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); - - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; @@ -1093,13 +1062,10 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) ASSERT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result); - // Stop consuming rtde packages - g_my_robot->stopConsumingRTDEData(); - // Ensure that the robot hasn't moved - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_after; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_after)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); @@ -1120,15 +1086,12 @@ TEST_F(SplineInterpolationTest, switching_control_mode_without_trajectory_produc TEST_F(SplineInterpolationTest, switching_control_mode_with_trajectory_produces_result) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before{ 0, 0, 0, 0, 0, 0 }; - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before)); + ASSERT_TRUE(data_pkg.getData("target_q", joint_positions_before)); - // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly - g_my_robot->startConsumingRTDEData(); // Create a trajectory that cannot be executed within the robots limits std::vector s_pos, s_vel, s_acc; urcl::vector6d_t first_point = { diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 4ea0d1d00..5892b89b4 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -217,66 +217,62 @@ TEST_F(UrDriverTest, stop_robot_control) TEST_F(UrDriverTest, target_outside_limits_servoj) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions_before; - ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions_before)); + ASSERT_TRUE(data_pkg.getData("actual_q", joint_positions_before)); // Create physically unfeasible target urcl::vector6d_t joint_target = joint_positions_before; joint_target[5] -= 2.5; // Send unfeasible targets to the robot - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); g_my_robot->getUrDriver()->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t joint_positions; - ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions)); + ASSERT_TRUE(data_pkg.getData("actual_q", joint_positions)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions[i]); } // Make sure the program is stopped - g_my_robot->startConsumingRTDEData(); g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } TEST_F(UrDriverTest, target_outside_limits_pose) { - g_my_robot->stopConsumingRTDEData(); - std::unique_ptr data_pkg; - g_my_robot->readDataPackage(data_pkg); + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t tcp_pose_before; - ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose_before)); + ASSERT_TRUE(data_pkg.getData("actual_TCP_pose", tcp_pose_before)); // Create physically unfeasible target urcl::vector6d_t tcp_target = tcp_pose_before; tcp_target[2] += 0.3; // Send unfeasible targets to the robot - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); g_my_robot->getUrDriver()->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); // Ensure that the robot didn't move - g_my_robot->readDataPackage(data_pkg); + ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg)); urcl::vector6d_t tcp_pose; - ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose)); + ASSERT_TRUE(data_pkg.getData("actual_TCP_pose", tcp_pose)); for (unsigned int i = 0; i < 6; ++i) { EXPECT_FLOAT_EQ(tcp_pose_before[i], tcp_pose[i]); } // Make sure the program is stopped - g_my_robot->startConsumingRTDEData(); g_my_robot->getUrDriver()->stopControl(); g_my_robot->waitForProgramNotRunning(1000); } @@ -299,7 +295,6 @@ TEST_F(UrDriverTest, send_robot_program_retry_on_failure) TEST_F(UrDriverTest, reset_rtde_client) { - g_my_robot->stopConsumingRTDEData(); double target_frequency = 50; g_my_robot->getUrDriver()->resetRTDEClient(OUTPUT_RECIPE, INPUT_RECIPE, target_frequency); ASSERT_EQ(g_my_robot->getUrDriver()->getControlFrequency(), target_frequency); From 98a86d81cec0bad14b10d0e9501819b998cac0f5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 23 Jan 2026 21:14:31 +0100 Subject: [PATCH 13/38] Add test for data package string format During refactoring I discovered an error in the string output, so I added this test. --- include/ur_client_library/rtde/data_package.h | 2 +- src/rtde/data_package.cpp | 7 ++-- tests/test_rtde_data_package.cpp | 35 +++++++++++++++++++ 3 files changed, 38 insertions(+), 6 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 471c11613..b72057f2b 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -201,7 +201,7 @@ class DataPackage : public RTDEPackage * \returns True on success, false if the field cannot be found inside the package. */ template - bool setData(const std::string& name, T& val) + bool setData(const std::string& name, const T& val) { const auto it = std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 8941a5694..9f26ec2c6 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -466,11 +466,8 @@ void rtde_interface::DataPackage::initEmpty() data_.reserve(recipe_.size()); for (auto& item : recipe_) { - if (g_type_list.find(item) != g_type_list.end()) - { - _rtde_type_variant entry = g_type_list[item]; - data_.push_back({ item, entry }); - } + _rtde_type_variant entry = g_type_list[item]; + data_.push_back({ item, entry }); } } diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 0ccec48fc..3b385b237 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -154,6 +154,41 @@ TEST(rtde_data_package, parse_and_get_bitset_data) EXPECT_EQ(expected_robot_status_bits, actual_robot_status_bits); } +TEST(rtde_data_package, parse_incorrect_data_size) +{ + std::vector recipe{ "timestamp", "actual_q" }; + rtde_interface::DataPackage package(recipe); + + // Data package with incorrect size (should be 56 bytes for the given recipe) + uint8_t data_package[] = { 0x01, 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35, 0xbf }; + + comm::BinParser bp(data_package, sizeof(data_package)); + + EXPECT_THROW(package.parseWith(bp), UrException); +} + +TEST(rtde_data_package, data_package_to_string) +{ + std::vector recipe{ "speed_slider_mask", "speed_slider_fraction", "external_force_torque", + "standard_digital_output_mask", "actual_digital_output_bits" }; + rtde_interface::DataPackage package(recipe); + package.setData("speed_slider_mask", 1); + package.setData("speed_slider_fraction", 0.5); + package.setData("external_force_torque", vector6d_t{ -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }); + package.setData("standard_digital_output_mask", 1 << 7); + package.setData("actual_digital_output_bits", 1 << 7); + + std::string pkg_str = package.toString(); + + std::string expected_str = "speed_slider_mask: 1\n" + "speed_slider_fraction: 0.5\n" + "external_force_torque: [-1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031]\n" + "standard_digital_output_mask: 128\n" + "actual_digital_output_bits: 128\n"; + std::cout << "Package string:\n" << pkg_str << std::endl; + EXPECT_EQ(expected_str, pkg_str); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 82cf213e706b11b1155d0dd687105562786ccf4d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 26 Jan 2026 12:25:33 +0100 Subject: [PATCH 14/38] Added more tests for RTDEClient --- .github/workflows/ci.yml | 2 +- include/ur_client_library/comm/producer.h | 2 +- include/ur_client_library/rtde/rtde_client.h | 3 +- scripts/start_ursim.sh | 7 +- src/rtde/rtde_client.cpp | 15 +- tests/test_rtde_client.cpp | 225 +++++++++++++++---- tests/test_start_ursim.bats | 8 + 7 files changed, 210 insertions(+), 52 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 41e5478db..a327b0ea0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -34,7 +34,7 @@ jobs: - uses: actions/checkout@v6 - name: start ursim run: | - scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d + scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d -f DISABLED env: DOCKER_RUN_OPTS: --network ursim_net ROBOT_MODEL: ${{matrix.env.ROBOT_MODEL}} diff --git a/include/ur_client_library/comm/producer.h b/include/ur_client_library/comm/producer.h index 221622071..e509d9017 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -67,7 +67,7 @@ class URProducer : public IProducer } if (!running_) - return true; + return false; if (stream_.getState() == SocketState::Connected) { diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 4bb358f24..ba094ed1f 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -200,6 +200,7 @@ class RTDEClient * \returns Whether a data package was received successfully */ bool getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout); + bool getDataPackage(std::unique_ptr& data_package, std::chrono::milliseconds timeout); /*! * \brief Blocking call to get the next data package received from the robot. @@ -307,7 +308,7 @@ class RTDEClient */ void stopBackgroundRead(); -private: +protected: comm::URStream stream_; std::vector output_recipe_; bool ignore_unavailable_outputs_; diff --git a/scripts/start_ursim.sh b/scripts/start_ursim.sh index c0d38583e..d7f49f944 100755 --- a/scripts/start_ursim.sh +++ b/scripts/start_ursim.sh @@ -54,7 +54,7 @@ help() echo " -n Name of the docker container. Defaults to '$CONTAINER_NAME'" echo " -i IP address the container should get. Defaults to $IP_ADDRESS" echo " -d Detached mode - start in background" - echo " -f Specify port forwarding to use. Defaults to '$PORT_FORWARDING'. Set to empty string to disable port forwarding." + echo " -f Specify port forwarding to use. Defaults to '$PORT_FORWARDING'. Set to \"DISABLED\" to disable port forwarding." echo " -h Print this Help." echo } @@ -431,6 +431,11 @@ main() { fi fi + if [ "$PORT_FORWARDING" == "DISABLED" ]; then + echo "Port forwarding disabled" + PORT_FORWARDING="" + fi + DOCKER_ARGS="" if [ "$ROBOT_SERIES" == "polyscopex" ]; then diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 990c3bf70..2bf3f293a 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -159,6 +159,8 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron setTargetFrequency(); } + prod_->startProducer(); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted(); @@ -505,6 +507,7 @@ void RTDEClient::disconnect() } client_state_ = ClientState::UNINITIALIZED; prod_->stopProducer(); + stopBackgroundRead(); notifier_.stopped("RTDE communication stopped"); } @@ -560,7 +563,6 @@ bool RTDEClient::start(const bool read_packages_in_background) if (sendStart()) { - prod_->startProducer(); if (read_packages_in_background) { startBackgroundRead(); @@ -731,6 +733,12 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr return std::unique_ptr(nullptr); } +bool RTDEClient::getDataPackage(std::unique_ptr& data_package, + std::chrono::milliseconds timeout) +{ + return getDataPackage(*data_package, timeout); +} + bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) { if (!background_read_running_) @@ -822,6 +830,7 @@ void RTDEClient::reconnect() std::lock_guard lock(reconnect_mutex_); ClientState cur_client_state = client_state_; client_state_ = ClientState::CONNECTION_LOST; + bool background_read_was_running = background_read_running_; disconnect(); size_t cur_initialization_attempt = 0; @@ -891,7 +900,7 @@ void RTDEClient::reconnect() URCL_LOG_INFO("Successfully reconnected to the RTDE interface, starting communication again"); - start(); + start(background_read_was_running); if (cur_client_state == ClientState::PAUSED) { pause(); @@ -966,6 +975,8 @@ void RTDEClient::backgroundReadThreadFunc() std::this_thread::sleep_for(period); } } + new_data_.store(false); + URCL_LOG_INFO("RTDE background read thread stopped"); } } // namespace rtde_interface diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index a8c67a074..3c68796a2 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -35,6 +35,7 @@ #include #include #include +#include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/exceptions.h" #include @@ -47,20 +48,30 @@ using namespace urcl; std::string g_ROBOT_IP = "192.168.56.101"; uint32_t g_FAKE_RTDE_PORT = 13875; +class TestableRTDEClient : public rtde_interface::RTDEClient +{ +public: + using RTDEClient::RTDEClient; + + void triggerReconnect() + { + this->reconnect(); + } +}; + class RTDEClientTest : public ::testing::Test { protected: void SetUp() { - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_)); } void TearDown() { client_.reset(); // If we don't sleep we can get a conflict between two tests controlling the same rtde inputs - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } std::string output_recipe_file_ = "resources/rtde_output_recipe.txt"; @@ -68,7 +79,7 @@ class RTDEClientTest : public ::testing::Test std::string docs_output_recipe_file_ = "resources/docs_rtde_output_recipe.txt"; std::string input_recipe_file_ = "resources/rtde_input_recipe.txt"; comm::INotifier notifier_; - std::unique_ptr client_; + std::unique_ptr client_; std::vector resources_output_recipe_ = { "timestamp", "actual_q", @@ -122,16 +133,14 @@ TEST_F(RTDEClientTest, no_recipe) { std::string output_recipe_file = ""; std::string input_recipe_file = ""; - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is unconfigured - EXPECT_NO_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); + EXPECT_NO_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); - EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, - "/i/do/not/exist/urclrtdetest.txt")), + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, + "/i/do/not/exist/urclrtdetest.txt")), UrException); } @@ -139,26 +148,22 @@ TEST_F(RTDEClientTest, empty_recipe_file) { std::string output_recipe_file = "resources/empty.txt"; std::string input_recipe_file = "resources/empty.txt"; - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is empty - EXPECT_THROW( - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), - UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), + UrException); } TEST_F(RTDEClientTest, invalid_target_frequency) { // Setting target frequency below 0 or above 500, should throw an exception - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); EXPECT_THROW(client_->init(), UrException); - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); EXPECT_THROW(client_->init(), UrException); } @@ -177,8 +182,12 @@ TEST_F(RTDEClientTest, unconfigured_target_frequency) TEST_F(RTDEClientTest, set_target_frequency) { + // Set a target frequency that is different from the maximum frequency but a factor of it. + // Since we check timestamp differences, we need to make sure that the target frequency is + // achievable. 25 Hz is a factor of both 125 Hz and 500 Hz. + const double target_frequency = 25.0; client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1, false)); + new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, target_frequency, false)); client_->init(); // Maximum frequency should still be equal to the robot's maximum frequency @@ -193,12 +202,11 @@ TEST_F(RTDEClientTest, set_target_frequency) EXPECT_EQ(client_->getMaxFrequency(), expected_max_frequency); } - double expected_target_frequency = 1; - EXPECT_EQ(client_->getTargetFrequency(), expected_target_frequency); + EXPECT_EQ(client_->getTargetFrequency(), target_frequency); EXPECT_TRUE(client_->start(false)); - // Test that we receive packages with a frequency of 2 Hz + // Test that we receive packages with a frequency of `target_frequency` Hz auto data_pkg = std::make_unique(client_->getOutputRecipe()); ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); @@ -209,8 +217,8 @@ TEST_F(RTDEClientTest, set_target_frequency) double second_time_stamp = 0.0; data_pkg->getData("timestamp", second_time_stamp); - // There should be 1 second between each timestamp - EXPECT_NEAR(second_time_stamp - first_time_stamp, 1, 1e-6); + // There should be 0.1 second between each timestamp + EXPECT_NEAR(second_time_stamp - first_time_stamp, 1.0 / target_frequency, 1e-6); client_->pause(); } @@ -265,7 +273,16 @@ TEST_F(RTDEClientTest, output_recipe_file) } } -TEST_F(RTDEClientTest, recipe_compairson) +TEST_F(RTDEClientTest, input_recipe_with_invalid_key) +{ + std::vector actual_input_recipe = resources_input_recipe_; + actual_input_recipe.push_back("i_do_not_exist"); + + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); + EXPECT_THROW(client_->init(), RTDEInvalidKeyException); +} + +TEST_F(RTDEClientTest, recipe_comparison) { // Check that vectorized constructor provides same recipes as from file auto client = rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_); @@ -309,19 +326,106 @@ TEST_F(RTDEClientTest, get_data_package_w_background) // Test that we can receive a package and extract data from the received package const std::chrono::milliseconds read_timeout{ 100 }; + // Create an empty data package. Its timestamp should be 0.0 rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + double timestamp; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + data_pkg.setData("timestamp", 0.0); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); - urcl::vector6d_t actual_q; - EXPECT_TRUE(data_pkg.getData("actual_q", actual_q)); + // Verify that we actually got data from the robot + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); // Trying to get data with a very short timeout should fail ASSERT_FALSE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(1))); + // Check the second signature auto data_pkg_ptr = std::make_unique(client_->getOutputRecipe()); + data_pkg_ptr->setData("timestamp", 0.0); + ASSERT_TRUE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + EXPECT_TRUE(data_pkg_ptr->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Blocking call while packages are fetched in background should fail ASSERT_FALSE(client_->getDataPackageBlocking(data_pkg_ptr)); + // starting the background read twice should be fine (no effect) + ASSERT_NO_THROW(client_->startBackgroundRead()); + + ASSERT_NO_THROW(client_->stopBackgroundRead()); + EXPECT_FALSE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + ASSERT_NO_THROW(client_->startBackgroundRead()); + EXPECT_TRUE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); + + client_->pause(); +} + +TEST_F(RTDEClientTest, get_data_package_wo_background) +{ + client_->init(); + client_->start(false); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + data_pkg->setData("timestamp", 0.0); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + + urcl::vector6d_t actual_q; + EXPECT_TRUE(data_pkg->getData("actual_q", actual_q)); + double timestamp; + EXPECT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Non-blocking call should fail since we are not reading in background + ASSERT_FALSE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + ASSERT_FALSE(client_->getDataPackage(*data_pkg, std::chrono::milliseconds(100))); + + // We should be able to start background reading while the client is started without background + // reading and then query packages using the non-blocking call + client_->startBackgroundRead(); + ASSERT_TRUE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + double timestamp_2; + EXPECT_TRUE(data_pkg->getData("timestamp", timestamp_2)); + EXPECT_GT(timestamp_2, timestamp); + EXPECT_FALSE(client_->getDataPackageBlocking(data_pkg)); + + client_->pause(); +} + +TEST_F(RTDEClientTest, reconnect_rtde_client) +{ + 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 }; + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + data_pkg.setData("timestamp", 0.0); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + + double timestamp; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + std::thread reconnection_thread([this]() { client_->triggerReconnect(); }); + + // Give some time to ensure that the reconnection has started. TODO: A proper thread sync + // mechanism would be better here. + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // Trying to get data packages while disconnected should fail + ASSERT_FALSE(client_->getDataPackage(data_pkg, read_timeout)); + + reconnection_thread.join(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + double timestamp_2; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp_2)); + EXPECT_GT(timestamp_2, timestamp); + + std::this_thread::sleep_for(std::chrono::seconds(2)); + client_->pause(); } @@ -331,8 +435,8 @@ TEST_F(RTDEClientTest, get_data_package_fake_server) // 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_.reset(new TestableRTDEClient("localhost", notifier_, resources_output_recipe_, resources_input_recipe_, 100, + false, g_FAKE_RTDE_PORT)); client_->init(); client_->start(); @@ -358,8 +462,8 @@ TEST_F(RTDEClientTest, reconnect_fake_server) // 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_.reset(new TestableRTDEClient("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(); @@ -455,7 +559,7 @@ TEST_F(RTDEClientTest, write_rtde_data) TEST_F(RTDEClientTest, output_recipe_without_timestamp) { std::string output_recipe_file = "resources/rtde_output_recipe_without_timestamp.txt"; - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); std::vector actual_output_recipe_from_file = client_->getOutputRecipe(); const std::string timestamp = "timestamp"; @@ -472,11 +576,10 @@ TEST_F(RTDEClientTest, output_recipe_without_timestamp) TEST_F(RTDEClientTest, connect_non_running_robot) { - // We use an IP address on the integration_test's subnet - client_.reset( - new rtde_interface::RTDEClient("192.168.56.123", notifier_, resources_output_recipe_, resources_input_recipe_)); + // Make sure that there's no simulator running exposing RTDE on localhost. + client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, resources_input_recipe_)); auto start = std::chrono::system_clock::now(); - EXPECT_THROW(client_->init(2, std::chrono::milliseconds(500), 1), UrException); + EXPECT_THROW(client_->init(2, std::chrono::milliseconds(50), 1), UrException); auto end = std::chrono::system_clock::now(); auto elapsed = end - start; // This is only a rough estimate, obviously. @@ -506,8 +609,8 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_->init(); // Ignore unknown output variables to account for variables not available in old urcontrol versions. - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, - input_recipe_file_, 0.0, false)); + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, 0.0, false)); EXPECT_TRUE(client_->init()); client_->start(); @@ -519,6 +622,7 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) double timestamp; EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); client_->pause(); } @@ -575,16 +679,21 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) std::vector incorrect_output_recipe = client_->getOutputRecipe(); incorrect_output_recipe.push_back("unknown_rtde_variable"); - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, - 0.0, false)); - + // If unknown variables are not ignored, initialization should fail + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, false)); EXPECT_THROW(client_->init(), UrException); + + // Unknown variables can be ignored, so initialization should succeed + client_.reset( + new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, true)); + EXPECT_TRUE(client_->init()); } TEST_F(RTDEClientTest, empty_input_recipe) { std::vector empty_input_recipe = {}; - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); client_->init(); client_->start(); @@ -600,7 +709,7 @@ TEST_F(RTDEClientTest, empty_input_recipe) client_->pause(); - client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); client_->init(); client_->start(); @@ -612,6 +721,30 @@ TEST_F(RTDEClientTest, empty_input_recipe) client_->pause(); } +TEST_F(RTDEClientTest, test_initialization) +{ + // Test that initialization fails with 0 initialization attempts + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(100), 0), UrException); + + comm::TCPServer dummy_server(UR_RTDE_PORT); + dummy_server.start(); + + // Test that initialization fails when no RTDE interface is available on the robot + // within the given initialization attempts + // We use a dummy server here that doesn't implement the RTDE interface + // to simulate this scenario. + // The total time should be at least (initialization attempts - 1) * initialization timeout + // since the last attempt doesn't wait after failing. + + URCL_LOG_INFO("Starting initialization timing test"); + client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, {})); + auto start = std::chrono::system_clock::now(); + EXPECT_THROW(client_->init(2, std::chrono::milliseconds(10), 2, std::chrono::milliseconds(10)), UrException); + auto end = std::chrono::system_clock::now(); + auto elapsed = end - start; + EXPECT_GE(std::chrono::duration_cast(elapsed).count(), 20); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_start_ursim.bats b/tests/test_start_ursim.bats index ab368f616..7feead249 100644 --- a/tests/test_start_ursim.bats +++ b/tests/test_start_ursim.bats @@ -516,6 +516,14 @@ setup() { [ "$port_forwarding" = "-p 1234:1234 -p 50001-50004:60001-60004" ] } +@test "disable_port_forwarding" { + run main -t -f "DISABLED" + echo "$output" + [ $status -eq 0 ] + docker_line=$(echo "$output" | tail -n -1) + grep -v -E "\-p\s*[0-9]+(\-[0-9]+)?:[0-9]+(\-[0-9]+)?" <<< "$docker_line" +} + @test "default_container_name" { run main -t echo "$output" From 897276233304a8cf1be84267f9f4729efe5de933 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 27 Jan 2026 10:58:32 +0100 Subject: [PATCH 15/38] Add test for RTDEWriter::sendPackage --- include/ur_client_library/rtde/data_package.h | 7 ++++ src/rtde/rtde_writer.cpp | 15 ++++++-- tests/test_rtde_client.cpp | 10 +++--- tests/test_rtde_data_package.cpp | 10 +++--- tests/test_rtde_writer.cpp | 34 +++++++++++++++++++ 5 files changed, 63 insertions(+), 13 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index b72057f2b..ffd9cc7a6 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -209,6 +209,13 @@ class DataPackage : public RTDEPackage }); if (it != data_.end()) { + if (!std::holds_alternative(it->second)) + { + // TODO: It might be better to replace the return type by void and use exceptions for the + // error case. + URCL_LOG_ERROR("Type of passed data doesn't match type of existing field for index '%s'", name.c_str()); + return false; + } it->second = val; } else diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 061957374..bae1fccb5 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -383,8 +383,19 @@ void RTDEWriter::resetMasks(const std::shared_ptr& buffer) { for (const auto& mask_name : used_masks_) { - uint8_t mask = 0; - buffer->setData(mask_name, mask); + // "speed_slider_mask" is uint32_t, all others are uint8_t + // If we reset it to the wrong type, serialization will be wrong + if (mask_name == "speed_slider_mask") + + { + uint32_t mask = 0; + buffer->setData(mask_name, mask); + } + else + { + uint8_t mask = 0; + buffer->setData(mask_name, mask); + } } } diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 3c68796a2..0d2afc10a 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -330,7 +330,7 @@ TEST_F(RTDEClientTest, get_data_package_w_background) rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); double timestamp; EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); - data_pkg.setData("timestamp", 0.0); + ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); @@ -344,7 +344,7 @@ TEST_F(RTDEClientTest, get_data_package_w_background) // Check the second signature auto data_pkg_ptr = std::make_unique(client_->getOutputRecipe()); - data_pkg_ptr->setData("timestamp", 0.0); + ASSERT_TRUE(data_pkg_ptr->setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackage(data_pkg_ptr, std::chrono::milliseconds(100))); EXPECT_TRUE(data_pkg_ptr->getData("timestamp", timestamp)); EXPECT_GT(timestamp, 0.0); @@ -369,7 +369,7 @@ TEST_F(RTDEClientTest, get_data_package_wo_background) client_->start(false); auto data_pkg = std::make_unique(client_->getOutputRecipe()); - data_pkg->setData("timestamp", 0.0); + ASSERT_TRUE(data_pkg->setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); urcl::vector6d_t actual_q; @@ -402,7 +402,7 @@ TEST_F(RTDEClientTest, reconnect_rtde_client) // Test that we can receive a package and extract data from the received package const std::chrono::milliseconds read_timeout{ 100 }; rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); - data_pkg.setData("timestamp", 0.0); + ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp; @@ -424,8 +424,6 @@ TEST_F(RTDEClientTest, reconnect_rtde_client) EXPECT_TRUE(data_pkg.getData("timestamp", timestamp_2)); EXPECT_GT(timestamp_2, timestamp); - std::this_thread::sleep_for(std::chrono::seconds(2)); - client_->pause(); } diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 3b385b237..f641fcf21 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -172,11 +172,11 @@ TEST(rtde_data_package, data_package_to_string) std::vector recipe{ "speed_slider_mask", "speed_slider_fraction", "external_force_torque", "standard_digital_output_mask", "actual_digital_output_bits" }; rtde_interface::DataPackage package(recipe); - package.setData("speed_slider_mask", 1); - package.setData("speed_slider_fraction", 0.5); - package.setData("external_force_torque", vector6d_t{ -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }); - package.setData("standard_digital_output_mask", 1 << 7); - package.setData("actual_digital_output_bits", 1 << 7); + ASSERT_TRUE(package.setData("speed_slider_mask", 1)); + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + ASSERT_TRUE(package.setData("external_force_torque", vector6d_t{ -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 })); + ASSERT_TRUE(package.setData("standard_digital_output_mask", 1 << 7)); + ASSERT_TRUE(package.setData("actual_digital_output_bits", 1 << 7)); std::string pkg_str = package.toString(); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index cad7affe1..e03cc360c 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -458,6 +458,40 @@ TEST_F(RTDEWriterTest, send_external_force_torque) EXPECT_EQ(send_external_force_torque[5], received_external_force_torque[5]); } +TEST_F(RTDEWriterTest, send_data_package) +{ + const uint32_t send_speed_slider_mask = 1; + const double send_speed_slider_fraction = 0.7; + const bool standard_digital_output_value = true; + const uint8_t standard_digital_output_mask = 0b00000001; // pin 1 + + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", send_speed_slider_fraction)); + ASSERT_TRUE(data_package.setData("speed_slider_mask", send_speed_slider_mask)); + ASSERT_TRUE( + data_package.setData("standard_digital_output", static_cast(standard_digital_output_value ? 255 : 0))); + ASSERT_TRUE(data_package.setData("standard_digital_output_mask", standard_digital_output_mask)); + + EXPECT_TRUE(writer_->sendPackage(data_package)); + + waitForMessageCallback(1000); + + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + ASSERT_TRUE(dataFieldExist("speed_slider_mask")); + ASSERT_TRUE(dataFieldExist("standard_digital_output")); + ASSERT_TRUE(dataFieldExist("standard_digital_output_mask")); + + double received_speed_slider_fraction = std::get(parsed_data_["speed_slider_fraction"]); + uint32_t received_speed_slider_mask = std::get(parsed_data_["speed_slider_mask"]); + bool received_standard_digital_output_value = std::get(parsed_data_["standard_digital_output"]) != 0; + uint8_t received_standard_digital_output_mask = std::get(parsed_data_["standard_digital_output_mask"]); + + EXPECT_EQ(send_speed_slider_fraction, received_speed_slider_fraction); + EXPECT_EQ(send_speed_slider_mask, received_speed_slider_mask); + EXPECT_EQ(standard_digital_output_value, received_standard_digital_output_value); + EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 4b5002432f21e7fef3f9355ca28642f2921d08c7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 27 Jan 2026 11:15:38 +0100 Subject: [PATCH 16/38] Increase coverage of RTDEWriterTest --- tests/test_rtde_writer.cpp | 44 +++++++++++++++++++++++++++++++++++--- 1 file changed, 41 insertions(+), 3 deletions(-) diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index e03cc360c..37d450828 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -212,6 +212,15 @@ TEST_F(RTDEWriterTest, send_standard_digital_output) // Changing pins above 7, should return false. pin = 8; EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); + + // Set pin to value false + pin = 2; + EXPECT_TRUE(writer_->sendStandardDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["standard_digital_output"]) != 0; + received_standard_digital_output_mask = std::get(parsed_data_["standard_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_standard_digital_output_mask, received_standard_digital_output_mask); } TEST_F(RTDEWriterTest, send_configurable_digital_output) @@ -227,14 +236,25 @@ TEST_F(RTDEWriterTest, send_configurable_digital_output) ASSERT_TRUE(dataFieldExist("configurable_digital_output_mask")); bool received_pin_value = std::get(parsed_data_["configurable_digital_output"]) != 0; - uint8_t received_standard_digital_output_mask = std::get(parsed_data_["configurable_digital_output_mask"]); + uint8_t received_configurable_digital_output_mask = std::get(parsed_data_["configurable_digital_output_" + "mask"]); EXPECT_EQ(send_pin_value, received_pin_value); - EXPECT_EQ(expected_configurable_digital_output_mask, received_standard_digital_output_mask); + EXPECT_EQ(expected_configurable_digital_output_mask, received_configurable_digital_output_mask); // Changing pins above 7, should return false. pin = 8; - EXPECT_FALSE(writer_->sendStandardDigitalOutput(pin, send_pin_value)); + EXPECT_FALSE(writer_->sendConfigurableDigitalOutput(pin, send_pin_value)); + + // Set pin to value false + pin = 2; + expected_configurable_digital_output_mask = 0b0000100; + EXPECT_TRUE(writer_->sendConfigurableDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["configurable_digital_output"]) != 0; + received_configurable_digital_output_mask = std::get(parsed_data_["configurable_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_configurable_digital_output_mask, received_configurable_digital_output_mask); } TEST_F(RTDEWriterTest, send_tool_digital_output) @@ -258,6 +278,14 @@ TEST_F(RTDEWriterTest, send_tool_digital_output) // Changing pins above 1, should return false. pin = 2; EXPECT_FALSE(writer_->sendToolDigitalOutput(pin, send_pin_value)); + // Set pin to value false + pin = 0; + EXPECT_TRUE(writer_->sendToolDigitalOutput(pin, false)); + waitForMessageCallback(1000); + received_pin_value = std::get(parsed_data_["tool_digital_output"]) != 0; + received_tool_digital_output_mask = std::get(parsed_data_["tool_digital_output_mask"]); + EXPECT_EQ(received_pin_value, false); + EXPECT_EQ(expected_tool_digital_output_mask, received_tool_digital_output_mask); } TEST_F(RTDEWriterTest, send_standard_analog_output_unknown_domain) @@ -492,6 +520,16 @@ TEST_F(RTDEWriterTest, send_data_package) EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); } +TEST_F(RTDEWriterTest, init_while_running_throws) +{ + EXPECT_THROW(writer_->init(1), UrException); +} + +TEST_F(RTDEWriterTest, set_recipe_while_running_throws) +{ + EXPECT_THROW(writer_->setInputRecipe(input_recipe_), UrException); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); From 835cd7e0e38e95cf046377284e4d7cbc68d37c07 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 27 Jan 2026 12:16:30 +0100 Subject: [PATCH 17/38] Add primary_parser test for single package --- src/primary/robot_message.cpp | 1 + tests/test_primary_parser.cpp | 20 +++++++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/primary/robot_message.cpp b/src/primary/robot_message.cpp index 33a3ab966..0e3fb2ce9 100644 --- a/src/primary/robot_message.cpp +++ b/src/primary/robot_message.cpp @@ -46,6 +46,7 @@ bool RobotMessage::consumeWith(AbstractPrimaryConsumer& consumer) std::string RobotMessage::toString() const { std::stringstream ss; + ss << "RobotMessage: " << std::endl; ss << "timestamp: " << timestamp_ << std::endl; ss << "source: " << static_cast(source_) << std::endl; ss << "message_type: " << static_cast(message_type_) << std::endl; diff --git a/tests/test_primary_parser.cpp b/tests/test_primary_parser.cpp index 19429e780..c076ebd89 100644 --- a/tests/test_primary_parser.cpp +++ b/tests/test_primary_parser.cpp @@ -205,19 +205,29 @@ TEST(primary_parser, parse_calibration_data) } } +TEST(primary_parser, parse_robot_state_with_single_parser) +{ + unsigned char raw_data[sizeof(ROBOT_STATE)]; + memcpy(raw_data, ROBOT_STATE, sizeof(ROBOT_STATE)); + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::unique_ptr product; + primary_interface::PrimaryParser parser; + ASSERT_FALSE(parser.parse(bp, product)); +}; + TEST(primary_parser, parse_version_message) { unsigned char raw_data[sizeof(VERSION_MESSAGE)]; memcpy(raw_data, VERSION_MESSAGE, sizeof(VERSION_MESSAGE)); comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; primary_interface::PrimaryParser parser; - ASSERT_TRUE(parser.parse(bp, products)); - - EXPECT_EQ(products.size(), 1); + ASSERT_TRUE(parser.parse(bp, product)); - if (primary_interface::VersionMessage* data = dynamic_cast(products[0].get())) + EXPECT_NE(product, nullptr); + if (primary_interface::VersionMessage* data = dynamic_cast(product.get())) { EXPECT_EQ(data->major_version_, 5); EXPECT_EQ(data->minor_version_, 24); From 0dbc5931b05c189dbf66e456e58ed2a97e8dd401 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 2 Feb 2026 09:34:24 +0100 Subject: [PATCH 18/38] Add a debug message if getting a RTDE package failed because of the timeout is being violated --- src/rtde/rtde_client.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 2bf3f293a..26ddb1f5c 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -762,6 +762,7 @@ bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::millisec auto wait_result = background_read_cv_.wait_for(lock, timeout); if (wait_result == std::cv_status::timeout) { + URCL_LOG_ERROR("RTDE package receive timeout passed without a new package. Timeout was %d ms", timeout.count()); reconnect_mutex_.unlock(); return false; } From 9e4ef8b511b2fd262f64c106b72172a29c6dd9e5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 5 Feb 2026 12:08:13 +0100 Subject: [PATCH 19/38] Update doc/architecture/rtde_client.rst MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com> --- doc/architecture/rtde_client.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index 0469bc223..1162b7d6b 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -58,7 +58,7 @@ following will work ); .. note:: - Remember, that ``timestamp`` always has to be part of the output recipe. + ``timestamp`` will always be a part of the output recipe and will be added afterwards, if not defined. As the ``timestamp`` used for verifying the connectivity. Reading data ------------ From 8e850d566af199574f562e5cc2a40a839ceae60b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 5 Feb 2026 11:55:40 +0100 Subject: [PATCH 20/38] Add a deprecation warning for using the allocating parse function --- include/ur_client_library/rtde/rtde_parser.h | 2 + src/rtde/rtde_parser.cpp | 22 +++- tests/test_rtde_parser.cpp | 130 ++++++++++++------- 3 files changed, 108 insertions(+), 46 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 416dddd4a..9c97827b0 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -80,6 +80,8 @@ class RTDEParser : public comm::Parser * \returns True, if the byte stream could successfully be parsed as RTDE packages, false * otherwise */ + [[deprecated("This method allocates memory on each call. Please use the overload which takes a single unique ptr to " + "a pre-allocated package. This function will be removed in May 2027.")]] bool parse(comm::BinParser& bp, std::vector>& results) override; void setProtocolVersion(uint16_t protocol_version) diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 5a6257075..6fb8de926 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -30,6 +30,15 @@ namespace rtde_interface bool RTDEParser::parse(comm::BinParser& bp, std::vector>& results) { + static bool warning_printed = false; + if (!warning_printed) + { + URCL_LOG_WARN("Calling RTDEParser::parse(...) with a vector of products is deprecated. It will allocate memory in " + "each cycle in order to create parsed packages. Please use the overloaded function that accepts a " + "pre-allocated unique pointer to a package. This message is for application developers using this " + "function."); + warning_printed = true; + } PackageType type; try { @@ -80,7 +89,16 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector& result) { - PackageType type = getPackageTypeFromHeader(bp); + PackageType type; + try + { + type = getPackageTypeFromHeader(bp); + } + catch (const UrException& e) + { + URCL_LOG_ERROR("Exception during RTDE package parsing: %s", e.what()); + return false; + } switch (type) { @@ -105,6 +123,8 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result { if (result == nullptr || result->getType() != type) { + // For all non data-packages real-time communication isn't critical. Hence, we silently + // allocate a new package if preallocation isn't given. result = std::unique_ptr(createNewPackageFromType(type)); } if (!result->parseWith(bp)) diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index cc8691890..da6ce4518 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -37,16 +37,34 @@ TEST(rtde_parser, request_protocol_version) { // Accepted request protocol version unsigned char raw_data[] = { 0x00, 0x04, 0x56, 0x01 }; - comm::BinParser bp(raw_data, sizeof(raw_data)); - - std::vector> products; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); - EXPECT_EQ(products.size(), 1); + // test a non-preallocated product + std::unique_ptr product; + { + comm::BinParser bp(raw_data, sizeof(raw_data)); + parser.parse(bp, product); + } + + if (rtde_interface::RequestProtocolVersion* data = + dynamic_cast(product.get())) + { + EXPECT_EQ(data->accepted_, true); + } + else + { + std::cout << "Failed to get request protocol version data" << std::endl; + GTEST_FAIL(); + } + // test a preallocated product + std::unique_ptr product2 = std::make_unique(); + { + comm::BinParser bp(raw_data, sizeof(raw_data)); + parser.parse(bp, product2); + } if (rtde_interface::RequestProtocolVersion* data = - dynamic_cast(products[0].get())) + dynamic_cast(product.get())) { EXPECT_EQ(data->accepted_, true); } @@ -64,13 +82,11 @@ TEST(rtde_parser, get_urcontrol_version) 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); - if (rtde_interface::GetUrcontrolVersion* data = dynamic_cast(products[0].get())) + if (rtde_interface::GetUrcontrolVersion* data = dynamic_cast(product.get())) { EXPECT_EQ(data->version_information_.major, 5); EXPECT_EQ(data->version_information_.minor, 8); @@ -90,13 +106,11 @@ TEST(rtde_parser, control_package_pause) unsigned char raw_data[] = { 0x00, 0x04, 0x50, 0x01 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); + parser.parse(bp, product); - EXPECT_EQ(products.size(), 1); - - if (rtde_interface::ControlPackagePause* data = dynamic_cast(products[0].get())) + if (rtde_interface::ControlPackagePause* data = dynamic_cast(product.get())) { EXPECT_EQ(data->accepted_, true); } @@ -113,13 +127,11 @@ TEST(rtde_parser, control_package_start) unsigned char raw_data[] = { 0x00, 0x04, 0x53, 0x01 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); - if (rtde_interface::ControlPackageStart* data = dynamic_cast(products[0].get())) + if (rtde_interface::ControlPackageStart* data = dynamic_cast(product.get())) { EXPECT_EQ(data->accepted_, true); } @@ -137,14 +149,12 @@ TEST(rtde_parser, control_package_setup_inputs) 0x32, 0x2c, 0x44, 0x4f, 0x55, 0x42, 0x4c, 0x45 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); if (rtde_interface::ControlPackageSetupInputs* data = - dynamic_cast(products[0].get())) + dynamic_cast(product.get())) { EXPECT_EQ(data->input_recipe_id_, 1); EXPECT_EQ(data->variable_types_, "UINT32,DOUBLE"); @@ -163,15 +173,13 @@ TEST(rtde_parser, control_package_setup_outputs) 0x2c, 0x56, 0x45, 0x43, 0x54, 0x4f, 0x52, 0x36, 0x44 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); parser.setProtocolVersion(2); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); if (rtde_interface::ControlPackageSetupOutputs* data = - dynamic_cast(products[0].get())) + dynamic_cast(product.get())) { EXPECT_EQ(data->output_recipe_id_, 1); EXPECT_EQ(data->variable_types_, "DOUBLE,VECTOR6D"); @@ -190,15 +198,13 @@ TEST(rtde_parser, data_package) 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; std::vector recipe = { "timestamp", "target_speed_fraction" }; rtde_interface::RTDEParser parser(recipe); parser.setProtocolVersion(2); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); - if (rtde_interface::DataPackage* data = dynamic_cast(products[0].get())) + if (rtde_interface::DataPackage* data = dynamic_cast(product.get())) { double timestamp, target_speed_fraction; data->getData("timestamp", timestamp); @@ -220,17 +226,15 @@ TEST(rtde_parser, test_to_string) unsigned char raw_data[] = { 0x00, 0x05, 0x02, 0x00, 0x00 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - parser.parse(bp, products); - - EXPECT_EQ(products.size(), 1); + parser.parse(bp, product); std::stringstream expected; expected << "Type: 2" << std::endl; expected << "Raw byte stream: 0 0 " << std::endl; - EXPECT_EQ(products[0]->toString(), expected.str()); + EXPECT_EQ(product->toString(), expected.str()); } TEST(rtde_parser, test_buffer_too_short) @@ -239,9 +243,9 @@ TEST(rtde_parser, test_buffer_too_short) unsigned char raw_data[] = { 0x00, 0x06, 0x02, 0x00, 0x00 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - EXPECT_FALSE(parser.parse(bp, products)); + EXPECT_FALSE(parser.parse(bp, product)); } TEST(rtde_parser, test_buffer_too_long) @@ -250,9 +254,45 @@ TEST(rtde_parser, test_buffer_too_long) unsigned char raw_data[] = { 0x00, 0x04, 0x56, 0x01, 0x02, 0x01, 0x02 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - std::vector> products; + std::unique_ptr product; rtde_interface::RTDEParser parser({ "" }); - EXPECT_FALSE(parser.parse(bp, products)); + EXPECT_FALSE(parser.parse(bp, product)); +} + +TEST(rtde_parser, test_deprecated_parse_method) +{ + // received data package, + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(recipe); + parser.setProtocolVersion(2); + + std::vector> products; + { + comm::BinParser bp(raw_data, sizeof(raw_data)); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + parser.parse(bp, products); +#pragma GCC diagnostic pop + } + + EXPECT_EQ(products.size(), 1); + + if (rtde_interface::DataPackage* data = dynamic_cast(products[0].get())) + { + double timestamp, target_speed_fraction; + data->getData("timestamp", timestamp); + data->getData("target_speed_fraction", target_speed_fraction); + + EXPECT_FLOAT_EQ(timestamp, 16412.2); + EXPECT_EQ(target_speed_fraction, 1); + } + else + { + std::cout << "Failed to get data package data" << std::endl; + GTEST_FAIL(); + } } int main(int argc, char* argv[]) From 5879bfb091b59dc3549c02e90178ee692754d736 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 11:22:03 +0100 Subject: [PATCH 21/38] Make RTDE example match docs --- doc/examples/rtde_client.rst | 2 +- examples/rtde_client.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index 9e5cc4f6e..5339e7198 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -53,7 +53,7 @@ fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method. :caption: examples/rtde_client.cpp :linenos: :lineno-match: - :start-at: std::unique_ptr data_pkg = + :start-at: auto data_pkg = std::make_unique(my_client.getOutputRecipe()); :end-before: // Change the speed slider In our main loop, we wait for a new data package to arrive using the blocking read method. Once diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index 54b0c2c78..d92605c5a 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -83,11 +83,11 @@ int main(int argc, char* argv[]) double target_speed_fraction = 1.0; double speed_slider_increment = 0.01; - rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe()); + auto data_pkg = std::make_unique(my_client.getOutputRecipe()); // Once RTDE communication is started, we have to make sure to read from the interface buffer, as // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main // loop. - my_client.start(true); // false -> do not start background read thread. + my_client.start(false); // false -> do not start background read thread. auto start_time = std::chrono::steady_clock::now(); while (second_to_run <= 0 || @@ -96,13 +96,13 @@ int main(int argc, char* argv[]) { // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in // order to ensure that this is called in time. - bool success = my_client.getDataPackage(data_pkg, std::chrono::milliseconds(100)); + bool success = my_client.getDataPackageBlocking(data_pkg); if (success) { // Data fields in the data package are accessed by their name. Only names present in the // output recipe can be accessed. Otherwise this function will return false. // We preallocated the string TARGET_SPEED_FRACTION to avoid allocations in the main loop. - data_pkg.getData(TARGET_SPEED_FRACTION, target_speed_fraction); + data_pkg->getData(TARGET_SPEED_FRACTION, target_speed_fraction); printFraction(target_speed_fraction, TARGET_SPEED_FRACTION); } else From d2351cb82454e3eab6a71c5504dca0035e329c87 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 11:46:24 +0100 Subject: [PATCH 22/38] Suppress warning about unconfirmed safety setup during protocol negotiation --- src/rtde/rtde_client.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 26ddb1f5c..80ab4659e 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -270,6 +270,22 @@ bool RTDEClient::queryURControlVersion() URCL_LOG_INFO("Received URControl version %s", urcontrol_version_.toString().c_str()); return true; } + else if (rtde_interface::TextMessage* tmp_text_msg = dynamic_cast(package.get())) + { + // PolyScope X simulators seem to send a text message on every connect until they have been + // switched on. + if (tmp_text_msg->message_.find("SafetySetup has not been confirmed yet") != std::string::npos) + { + // silently retry + } + else + { + URCL_LOG_WARN("Received unexpected message from robot while querying URControl version. " + "Message:\n%s\nRetrying...", + tmp_text_msg->message_.c_str()); + } + num_retries++; + } else { std::stringstream ss; From 3cafd4e190ef9f8afd8eb879aa119d1fdaa52f13 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 11:47:47 +0100 Subject: [PATCH 23/38] Make warning on falsely allocated package more specific --- src/rtde/rtde_parser.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 6fb8de926..48bd60c25 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -106,10 +106,20 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result { if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE) { + if (result == nullptr) + { + URCL_LOG_WARN("The passed result pointer is empty. A new DataPackage will " + "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " + "would be sent."); + } + else + { + URCL_LOG_WARN("Passed a pre-allocated RTDE package of type %u while a DataPackage was received. A new " + "DataPackage will have to be allocated. Please pass a pre-allocated DataPackage if you expect " + "a DataPackage would be sent.", + result->getType()); + } result = std::make_unique(recipe_, protocol_version_); - URCL_LOG_WARN("The passed result pointer is empty or does not contain a DataPackage. A new DataPackage will " - "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " - "would be sent."); } if (!dynamic_cast(result.get())->parseWith(bp)) From 9a80738744f2a095b663557c0f96107d136dbbf9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 12:16:56 +0100 Subject: [PATCH 24/38] Mark preallocated register keys const and initialize them statically --- include/ur_client_library/rtde/rtde_writer.h | 7 ++-- src/rtde/rtde_writer.cpp | 37 +++++++++----------- 2 files changed, 20 insertions(+), 24 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 950e116fd..8b482454f 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -211,9 +211,10 @@ class RTDEWriter std::atomic new_data_available_; std::condition_variable data_available_cv_; - static std::vector g_preallocated_input_bit_register_keys; - static std::vector g_preallocated_input_int_register_keys; - static std::vector g_preallocated_input_double_register_keys; + static const std::vector g_preallocated_input_bit_register_keys; + static const std::vector g_preallocated_input_int_register_keys; + static const std::vector g_preallocated_input_double_register_keys; + static const std::vector initializePreallocatedKeys(const std::string& prefix, const size_t size); }; } // namespace rtde_interface diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index bae1fccb5..d9f30efee 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -34,33 +34,28 @@ namespace urcl { namespace rtde_interface { -std::vector RTDEWriter::g_preallocated_input_bit_register_keys; -std::vector RTDEWriter::g_preallocated_input_int_register_keys; -std::vector RTDEWriter::g_preallocated_input_double_register_keys; +const std::vector RTDEWriter::g_preallocated_input_bit_register_keys = + RTDEWriter::initializePreallocatedKeys("input_bit_register_", 128); +const std::vector RTDEWriter::g_preallocated_input_int_register_keys = + RTDEWriter::initializePreallocatedKeys("input_int_register_", 48); +const std::vector RTDEWriter::g_preallocated_input_double_register_keys = + RTDEWriter::initializePreallocatedKeys("input_double_register_", 48); + +const std::vector RTDEWriter::initializePreallocatedKeys(const std::string& prefix, const size_t size) +{ + std::vector keys(size); + for (size_t i = 0; i < size; ++i) + { + keys[i] = prefix + std::to_string(i); + } + return keys; +} RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) : stream_(stream), recipe_id_(0), running_(false) { setInputRecipe(recipe); - g_preallocated_input_bit_register_keys.resize(128); - for (size_t i = 0; i < 128; ++i) - { - g_preallocated_input_bit_register_keys[i] = "input_bit_register_" + std::to_string(i); - } - - g_preallocated_input_int_register_keys.resize(48); - for (size_t i = 0; i < 48; ++i) - { - g_preallocated_input_int_register_keys[i] = "input_int_register_" + std::to_string(i); - } - - g_preallocated_input_double_register_keys.resize(48); - for (size_t i = 0; i < 48; ++i) - { - g_preallocated_input_double_register_keys[i] = "input_double_register_" + std::to_string(i); - } - for (const auto& field : recipe) { if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") From 23257b97c32ffbc693579e61b37f7233c92a1277 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 12:22:47 +0100 Subject: [PATCH 25/38] Fix errors in RTDE writer and receive thread --- src/rtde/rtde_client.cpp | 7 +++++-- src/rtde/rtde_writer.cpp | 45 ++++++++++++++++++++++++++++++++-------- 2 files changed, 41 insertions(+), 11 deletions(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 80ab4659e..4d1d1ca31 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -782,8 +782,11 @@ bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::millisec reconnect_mutex_.unlock(); return false; } - data_package = *dynamic_cast(data_buffer0_.get()); - new_data_.store(false); + if (new_data_.load()) + { + data_package = *dynamic_cast(data_buffer0_.get()); + new_data_.store(false); + } } reconnect_mutex_.unlock(); } diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index d9f30efee..06c48ca83 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -166,7 +166,10 @@ bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) bool success = true; success = current_store_buffer_->setData(mask_key, mask); success = success && current_store_buffer_->setData(key, speed_slider_fraction); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -197,7 +200,10 @@ bool RTDEWriter::sendStandardDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -229,7 +235,10 @@ bool RTDEWriter::sendConfigurableDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -260,7 +269,10 @@ bool RTDEWriter::sendToolDigitalOutput(uint8_t output_pin, bool value) } success = current_store_buffer_->setData(key_mask, mask); success = success && current_store_buffer_->setData(key_output, digital_output); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -299,7 +311,10 @@ bool RTDEWriter::sendStandardAnalogOutput(uint8_t output_pin, double value, cons success = success && current_store_buffer_->setData(key_output_0, value); static const std::string key_output_1 = "standard_analog_output_1"; success = success && current_store_buffer_->setData(key_output_1, value); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -326,7 +341,10 @@ bool RTDEWriter::sendInputBitRegister(uint32_t register_id, bool value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_bit_register_keys[register_id], value); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -343,7 +361,10 @@ bool RTDEWriter::sendInputIntRegister(uint32_t register_id, int32_t value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_int_register_keys[register_id], value); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -360,7 +381,10 @@ bool RTDEWriter::sendInputDoubleRegister(uint32_t register_id, double value) std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(g_preallocated_input_double_register_keys[register_id], value); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } @@ -370,7 +394,10 @@ bool RTDEWriter::sendExternalForceTorque(const vector6d_t& external_force_torque static const std::string key = "external_force_torque"; std::lock_guard guard(store_mutex_); bool success = current_store_buffer_->setData(key, external_force_torque); - markStorageToBeSent(); + if (success) + { + markStorageToBeSent(); + } return success; } From e4ae2617e8da673bc584c311f016fc04833a8a4e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 12:44:37 +0100 Subject: [PATCH 26/38] Throw an error when attempting to create an RTDE Data package with an unknown key Initializing a robot with keys that the specific version doesn't know is unaffected by this. This affects only configurations where users specify a key that is unknown to the client library. --- src/rtde/data_package.cpp | 4 ++++ tests/test_rtde_client.cpp | 30 +++++++++++++++++++++--------- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 9f26ec2c6..4d6b8112f 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -466,6 +466,10 @@ void rtde_interface::DataPackage::initEmpty() data_.reserve(recipe_.size()); for (auto& item : recipe_) { + if (g_type_list.find(item) == g_type_list.end()) + { + throw RTDEInvalidKeyException("Unknown item in recipe: " + item); + } _rtde_type_variant entry = g_type_list[item]; data_.push_back({ item, entry }); } diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 0d2afc10a..fffbc62e4 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -278,8 +278,9 @@ TEST_F(RTDEClientTest, input_recipe_with_invalid_key) std::vector actual_input_recipe = resources_input_recipe_; actual_input_recipe.push_back("i_do_not_exist"); - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); - EXPECT_THROW(client_->init(), RTDEInvalidKeyException); + EXPECT_THROW( + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)), + RTDEInvalidKeyException); } TEST_F(RTDEClientTest, recipe_comparison) @@ -678,14 +679,25 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) incorrect_output_recipe.push_back("unknown_rtde_variable"); // If unknown variables are not ignored, initialization should fail - client_.reset( - new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, false)); - EXPECT_THROW(client_->init(), UrException); + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, false)), + RTDEInvalidKeyException); - // Unknown variables can be ignored, so initialization should succeed - client_.reset( - new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, true)); - EXPECT_TRUE(client_->init()); + // Unknown variables (by the control box) can be ignored, so initialization should succeed + if ((client_->getVersion().major == 5 && client_->getVersion().minor < 23) || + (client_->getVersion().major == 10 && client_->getVersion().minor < 11)) + { + std::vector output_recipe = client_->getOutputRecipe(); + output_recipe.push_back("actual_robot_energy_consumed"); // That has been added in 5.23.0 / 10.11.0 + client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe, resources_input_recipe_, 0.0, true)); + EXPECT_TRUE(client_->init()); + } + + // Passing a completely unknown variable should still lead to an exception, even if unknown + // variables are ignored. + EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, true)), + RTDEInvalidKeyException); } TEST_F(RTDEClientTest, empty_input_recipe) From ddc87118859938dea0b13ee7cc5d73de4eb40de0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 12:49:21 +0100 Subject: [PATCH 27/38] Always setup used_masks when changing the recipe --- src/rtde/rtde_writer.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 06c48ca83..329ec5985 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -55,14 +55,6 @@ RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector= 5 && field.substr(field.size() - 5) == "_mask") - { - used_masks_.push_back(field); - } - } } void RTDEWriter::setInputRecipe(const std::vector& recipe) @@ -74,6 +66,13 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) } std::lock_guard lock_guard(store_mutex_); recipe_ = recipe; + for (const auto& field : recipe) + { + if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") + { + used_masks_.push_back(field); + } + } data_buffer0_ = std::make_shared(recipe_); data_buffer1_ = std::make_shared(recipe_); From d002ef8816f517d3954a2cb751a8b71d53fddb17 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 13:16:12 +0100 Subject: [PATCH 28/38] Bugfixes --- src/rtde/rtde_client.cpp | 6 ++++++ src/rtde/rtde_writer.cpp | 1 + 2 files changed, 7 insertions(+) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 4d1d1ca31..c2ef8b7b4 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -816,6 +816,12 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa if (prod_->tryGet(base_package)) { reconnect_mutex_.unlock(); + auto package_type = base_package->getType(); + if (package_type != PackageType::RTDE_DATA_PACKAGE) + { + URCL_LOG_ERROR("Received package from RTDE interface is not a data package, but of type %d", package_type); + return false; + } data_package.reset(dynamic_cast(base_package.release())); return true; } diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 329ec5985..425ec0912 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -66,6 +66,7 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) } std::lock_guard lock_guard(store_mutex_); recipe_ = recipe; + used_masks_.clear(); for (const auto& field : recipe) { if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") From a291e008d2844757ca6121053c34f599800df02b Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 6 Feb 2026 14:53:16 +0100 Subject: [PATCH 29/38] Remove error log that wasn't there before When no package can be retreived in the given timeout, don't log an error. That hasn't been there before, as well. --- src/rtde/rtde_client.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index c2ef8b7b4..88f7724a2 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -778,7 +778,6 @@ bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::millisec auto wait_result = background_read_cv_.wait_for(lock, timeout); if (wait_result == std::cv_status::timeout) { - URCL_LOG_ERROR("RTDE package receive timeout passed without a new package. Timeout was %d ms", timeout.count()); reconnect_mutex_.unlock(); return false; } From 617061216dd2bff9d7a1f56ab91a4694363232c8 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 06:44:43 +0100 Subject: [PATCH 30/38] Add a mutex on stopping the send thread Otherwise there is a race condition when checking whether the thread is joinable. --- tests/fake_rtde_server.cpp | 3 ++- tests/fake_rtde_server.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index f74735175..72e23a632 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -192,10 +192,11 @@ void RTDEServer::startSendingDataPackages() void RTDEServer::stopSendingDataPackages() { - URCL_LOG_INFO("Stop sending data."); + std::lock_guard thread_lock(thread_control_mutex_); send_loop_running_ = false; if (send_thread_.joinable()) { + URCL_LOG_INFO("Stop sending data."); send_thread_.join(); } } diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index 72be6d59a..b34eec510 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -51,6 +51,7 @@ class RTDEServer void actOnInput(); std::mutex output_data_mutex_; + std::mutex thread_control_mutex_; }; } // namespace urcl From be3e4c989186e15c197ce69281b607a9c834c8be Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 07:51:53 +0100 Subject: [PATCH 31/38] Move reconnect logic to background thread insteaf og get function --- src/rtde/rtde_client.cpp | 86 ++++++++++++++++++++++------------------ 1 file changed, 47 insertions(+), 39 deletions(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 88f7724a2..9e8670a67 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -627,7 +627,9 @@ bool RTDEClient::sendStart() return false; } - std::unique_ptr package; + // Worst case we get a data package as part of a race condition in the communication. If we + // didn't preallocate that, it might print a warning. + std::unique_ptr package = std::make_unique(output_recipe_); unsigned int num_retries = 0; while (num_retries < MAX_REQUEST_RETRIES) { @@ -675,7 +677,9 @@ bool RTDEClient::sendPause() URCL_LOG_ERROR("Sending RTDE pause command failed!"); return false; } - std::unique_ptr package; + // Worst case we get a data package as part of a race condition in the communication. If we + // didn't preallocate that, it might print a warning. + std::unique_ptr package = std::make_unique(output_recipe_); std::chrono::time_point start = std::chrono::steady_clock::now(); int seconds = 5; while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) @@ -757,6 +761,11 @@ bool RTDEClient::getDataPackage(std::unique_ptr& da bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) { + if (reconnecting_) + { + URCL_LOG_WARN("Currently reconnecting to the RTDE interface, unable to get data package"); + return false; + } if (!background_read_running_) { URCL_LOG_ERROR("Background reading is not running, cannot get data package. Please either start background " @@ -764,37 +773,25 @@ bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::millisec return false; } - if (reconnect_mutex_.try_lock()) + if (new_data_.load()) + { + std::lock_guard guard(read_mutex_); + data_package = *dynamic_cast(data_buffer0_.get()); + new_data_.store(false); + } + else { + std::unique_lock lock(read_mutex_); + auto wait_result = background_read_cv_.wait_for(lock, timeout); + if (wait_result == std::cv_status::timeout) + { + return false; + } if (new_data_.load()) { - std::lock_guard guard(read_mutex_); data_package = *dynamic_cast(data_buffer0_.get()); new_data_.store(false); } - else - { - std::unique_lock lock(read_mutex_); - auto wait_result = background_read_cv_.wait_for(lock, timeout); - if (wait_result == std::cv_status::timeout) - { - reconnect_mutex_.unlock(); - return false; - } - if (new_data_.load()) - { - data_package = *dynamic_cast(data_buffer0_.get()); - new_data_.store(false); - } - } - 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 false; } return true; } @@ -850,6 +847,7 @@ RTDEWriter& RTDEClient::getWriter() void RTDEClient::reconnect() { URCL_LOG_INFO("Reconnecting to the RTDE interface"); + reconnecting_ = true; // Locking mutex to ensure that calling getDataPackage doesn't influence the communication needed for reconfiguring // the RTDE connection std::lock_guard lock(reconnect_mutex_); @@ -944,7 +942,6 @@ void RTDEClient::reconnectCallback() { reconnecting_thread_.join(); } - reconnecting_ = true; reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this); } @@ -976,27 +973,38 @@ void RTDEClient::backgroundReadThreadFunc() { while (background_read_running_) { - if (prod_->tryGet(data_buffer1_)) + if (reconnect_mutex_.try_lock()) { - rtde_interface::DataPackage* data_pkg = dynamic_cast(data_buffer1_.get()); - if (data_pkg != nullptr) + if (prod_->tryGet(data_buffer1_)) { + reconnect_mutex_.unlock(); + rtde_interface::DataPackage* data_pkg = dynamic_cast(data_buffer1_.get()); + if (data_pkg != nullptr) { - std::scoped_lock lock(read_mutex_, write_mutex_); - std::swap(data_buffer0_, data_buffer1_); - } + { + std::scoped_lock lock(read_mutex_, write_mutex_); + std::swap(data_buffer0_, data_buffer1_); + } - new_data_.store(true); - background_read_cv_.notify_one(); + new_data_.store(true); + background_read_cv_.notify_one(); + } + else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE) + { + URCL_LOG_INFO(data_buffer1_->toString().c_str()); + } } - else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE) + else { - URCL_LOG_INFO(data_buffer1_->toString().c_str()); + reconnect_mutex_.unlock(); + auto period = std::chrono::duration(1.0 / target_frequency_); + std::this_thread::sleep_for(period); } } else { - auto period = std::chrono::duration(1.0 / target_frequency_); + URCL_LOG_WARN("Unable to get data package while reconnecting to the RTDE interface"); + auto period = std::chrono::duration(2.0 / target_frequency_); std::this_thread::sleep_for(period); } } From bd1d00e3ec57bff85e93b3333b8e990695bb8593 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 07:54:38 +0100 Subject: [PATCH 32/38] Remove trigger_reconnect test That test was not adding a real scenario and now that we have the real reconnection tests using the fake server, we can remove that test. --- tests/test_rtde_client.cpp | 188 +++++++++++++++++++++---------------- 1 file changed, 108 insertions(+), 80 deletions(-) diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index fffbc62e4..a6437cca3 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -42,29 +42,20 @@ #include #include "fake_rtde_server.h" +#include "ur_client_library/helpers.h" using namespace urcl; std::string g_ROBOT_IP = "192.168.56.101"; uint32_t g_FAKE_RTDE_PORT = 13875; -class TestableRTDEClient : public rtde_interface::RTDEClient -{ -public: - using RTDEClient::RTDEClient; - - void triggerReconnect() - { - this->reconnect(); - } -}; - class RTDEClientTest : public ::testing::Test { protected: void SetUp() { - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_)); } void TearDown() @@ -79,7 +70,7 @@ class RTDEClientTest : public ::testing::Test std::string docs_output_recipe_file_ = "resources/docs_rtde_output_recipe.txt"; std::string input_recipe_file_ = "resources/rtde_input_recipe.txt"; comm::INotifier notifier_; - std::unique_ptr client_; + std::unique_ptr client_; std::vector resources_output_recipe_ = { "timestamp", "actual_q", @@ -133,14 +124,16 @@ TEST_F(RTDEClientTest, no_recipe) { std::string output_recipe_file = ""; std::string input_recipe_file = ""; - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW( + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is unconfigured - EXPECT_NO_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); + EXPECT_NO_THROW( + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file))); - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, - "/i/do/not/exist/urclrtdetest.txt")), + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, + "/i/do/not/exist/urclrtdetest.txt")), UrException); } @@ -148,22 +141,26 @@ TEST_F(RTDEClientTest, empty_recipe_file) { std::string output_recipe_file = "resources/empty.txt"; std::string input_recipe_file = "resources/empty.txt"; - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), - UrException); + EXPECT_THROW( + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file)), + UrException); // Only input recipe is empty - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), - UrException); + EXPECT_THROW( + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file)), + UrException); } TEST_F(RTDEClientTest, invalid_target_frequency) { // Setting target frequency below 0 or above 500, should throw an exception - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, -1.0, false)); EXPECT_THROW(client_->init(), UrException); - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, 1000, false)); EXPECT_THROW(client_->init(), UrException); } @@ -186,8 +183,8 @@ TEST_F(RTDEClientTest, set_target_frequency) // Since we check timestamp differences, we need to make sure that the target frequency is // achievable. 25 Hz is a factor of both 125 Hz and 500 Hz. const double target_frequency = 25.0; - client_.reset( - new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, target_frequency, false)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, input_recipe_file_, + target_frequency, false)); client_->init(); // Maximum frequency should still be equal to the robot's maximum frequency @@ -278,9 +275,9 @@ TEST_F(RTDEClientTest, input_recipe_with_invalid_key) std::vector actual_input_recipe = resources_input_recipe_; actual_input_recipe.push_back("i_do_not_exist"); - EXPECT_THROW( - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)), - RTDEInvalidKeyException); + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, + actual_input_recipe)), + RTDEInvalidKeyException); } TEST_F(RTDEClientTest, recipe_comparison) @@ -395,47 +392,14 @@ TEST_F(RTDEClientTest, get_data_package_wo_background) client_->pause(); } -TEST_F(RTDEClientTest, reconnect_rtde_client) -{ - 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 }; - rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); - ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); - ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); - - double timestamp; - EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); - EXPECT_GT(timestamp, 0.0); - - std::thread reconnection_thread([this]() { client_->triggerReconnect(); }); - - // Give some time to ensure that the reconnection has started. TODO: A proper thread sync - // mechanism would be better here. - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // Trying to get data packages while disconnected should fail - ASSERT_FALSE(client_->getDataPackage(data_pkg, read_timeout)); - - reconnection_thread.join(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); - double timestamp_2; - EXPECT_TRUE(data_pkg.getData("timestamp", timestamp_2)); - EXPECT_GT(timestamp_2, timestamp); - - 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 TestableRTDEClient("localhost", notifier_, resources_output_recipe_, resources_input_recipe_, 100, - false, g_FAKE_RTDE_PORT)); + client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, + resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); client_->init(); client_->start(); @@ -455,14 +419,14 @@ TEST_F(RTDEClientTest, get_data_package_fake_server) client_.reset(); } -TEST_F(RTDEClientTest, reconnect_fake_server) +TEST_F(RTDEClientTest, reconnect_fake_server_background_read) { 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 TestableRTDEClient("localhost", notifier_, resources_output_recipe_, resources_input_recipe_, 100, - false, g_FAKE_RTDE_PORT)); + 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(); @@ -475,7 +439,7 @@ TEST_F(RTDEClientTest, reconnect_fake_server) { if (client_->getDataPackage(data_pkg, read_timeout)) { - URCL_LOG_INFO(data_pkg.toString().c_str()); + // URCL_LOG_INFO(data_pkg.toString().c_str()); } else { @@ -518,6 +482,68 @@ TEST_F(RTDEClientTest, reconnect_fake_server) URCL_LOG_INFO("Done"); } +TEST_F(RTDEClientTest, reconnect_fake_server_blocking_read) +{ + 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(false); + + std::atomic keep_running = true; + std::thread data_consumer_thread([this, &keep_running]() { + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + while (keep_running) + { + if (client_->getDataPackageBlocking(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(); + } + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + URCL_LOG_INFO(data_pkg->toString().c_str()); + + client_.reset(); + URCL_LOG_INFO("Done"); +} + TEST_F(RTDEClientTest, write_rtde_data) { client_->init(); @@ -558,7 +584,7 @@ TEST_F(RTDEClientTest, write_rtde_data) TEST_F(RTDEClientTest, output_recipe_without_timestamp) { std::string output_recipe_file = "resources/rtde_output_recipe_without_timestamp.txt"; - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file, input_recipe_file_)); std::vector actual_output_recipe_from_file = client_->getOutputRecipe(); const std::string timestamp = "timestamp"; @@ -576,7 +602,8 @@ TEST_F(RTDEClientTest, output_recipe_without_timestamp) TEST_F(RTDEClientTest, connect_non_running_robot) { // Make sure that there's no simulator running exposing RTDE on localhost. - client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, resources_input_recipe_)); + client_.reset( + new rtde_interface::RTDEClient("127.0.0.1", notifier_, resources_output_recipe_, resources_input_recipe_)); auto start = std::chrono::system_clock::now(); EXPECT_THROW(client_->init(2, std::chrono::milliseconds(50), 1), UrException); auto end = std::chrono::system_clock::now(); @@ -608,8 +635,8 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) client_->init(); // Ignore unknown output variables to account for variables not available in old urcontrol versions. - client_.reset( - new TestableRTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, 0.0, false)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, exhaustive_output_recipe_file_, + input_recipe_file_, 0.0, false)); EXPECT_TRUE(client_->init()); client_->start(); @@ -679,8 +706,8 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) incorrect_output_recipe.push_back("unknown_rtde_variable"); // If unknown variables are not ignored, initialization should fail - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, - resources_input_recipe_, 0.0, false)), + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, false)), RTDEInvalidKeyException); // Unknown variables (by the control box) can be ignored, so initialization should succeed @@ -689,21 +716,22 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) { std::vector output_recipe = client_->getOutputRecipe(); output_recipe.push_back("actual_robot_energy_consumed"); // That has been added in 5.23.0 / 10.11.0 - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe, resources_input_recipe_, 0.0, true)); + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe, resources_input_recipe_, 0.0, true)); EXPECT_TRUE(client_->init()); } // Passing a completely unknown variable should still lead to an exception, even if unknown // variables are ignored. - EXPECT_THROW(client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, - resources_input_recipe_, 0.0, true)), + EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, true)), RTDEInvalidKeyException); } TEST_F(RTDEClientTest, empty_input_recipe) { std::vector empty_input_recipe = {}; - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, empty_input_recipe)); client_->init(); client_->start(); @@ -719,7 +747,7 @@ TEST_F(RTDEClientTest, empty_input_recipe) client_->pause(); - client_.reset(new TestableRTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe_file_, "")); client_->init(); client_->start(); @@ -747,7 +775,7 @@ TEST_F(RTDEClientTest, test_initialization) // since the last attempt doesn't wait after failing. URCL_LOG_INFO("Starting initialization timing test"); - client_.reset(new TestableRTDEClient("127.0.0.1", notifier_, resources_output_recipe_, {})); + client_.reset(new rtde_interface::RTDEClient("127.0.0.1", notifier_, resources_output_recipe_, {})); auto start = std::chrono::system_clock::now(); EXPECT_THROW(client_->init(2, std::chrono::milliseconds(10), 2, std::chrono::milliseconds(10)), UrException); auto end = std::chrono::system_clock::now(); From 1a2c9178253bee8916a1d016f69f9766cbb0c51c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 08:49:58 +0100 Subject: [PATCH 33/38] Expect that we get data packages while waiting for pause confirmation --- src/rtde/rtde_client.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 9e8670a67..fd39bb40f 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -650,6 +650,10 @@ bool RTDEClient::sendStart() double timestamp; return tmp->getData("timestamp", timestamp); } + else if (dynamic_cast(package.get())) + { + // silently ignore, as this can happen that packages are still queued. + } else { std::stringstream ss; From cffd6b887126872d798173d5acb45dc55befe3e1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 09:35:36 +0100 Subject: [PATCH 34/38] Revert "Expect that we get data packages while waiting for pause confirmation" This reverts commit 1a2c9178253bee8916a1d016f69f9766cbb0c51c. --- src/rtde/rtde_client.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index fd39bb40f..9e8670a67 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -650,10 +650,6 @@ bool RTDEClient::sendStart() double timestamp; return tmp->getData("timestamp", timestamp); } - else if (dynamic_cast(package.get())) - { - // silently ignore, as this can happen that packages are still queued. - } else { std::stringstream ss; From 89f13e037bc28af2dbc6022dc50dc3ea0fbae0a1 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 09:44:25 +0100 Subject: [PATCH 35/38] Update RTDEClient doc example --- doc/architecture/rtde_client.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index 1162b7d6b..b45823974 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -105,11 +105,11 @@ an empty input recipe, like this: // Alternatively, pass an empty filename when using recipe files // rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, ""); my_client.init(); - my_client.start(false); + auto data_pkg = std::make_unique(my_client->getOutputRecipe()); + my_client.start(); while (true) { - std::unique_ptr data_pkg = my_client.getDataPackage(READ_TIMEOUT); - if (data_pkg) + if (my_client.getDataPackage(data_package, READ_TIMEOUT)) { std::cout << data_pkg->toString() << std::endl; } From 554c66817ce526f78637ec18e3eda5c2a2bf16c6 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Feb 2026 09:56:21 +0100 Subject: [PATCH 36/38] Replace bare mutex::try_lock with unique_locks --- src/rtde/rtde_client.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 9e8670a67..32407e8f6 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -807,11 +807,12 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages std::unique_ptr base_package(data_package.release()); - if (reconnect_mutex_.try_lock()) + std::unique_lock lock(reconnect_mutex_, std::defer_lock); + if (lock.try_lock()) { if (prod_->tryGet(base_package)) { - reconnect_mutex_.unlock(); + lock.unlock(); auto package_type = base_package->getType(); if (package_type != PackageType::RTDE_DATA_PACKAGE) { @@ -821,7 +822,7 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa data_package.reset(dynamic_cast(base_package.release())); return true; } - reconnect_mutex_.unlock(); + lock.unlock(); } else { @@ -973,7 +974,8 @@ void RTDEClient::backgroundReadThreadFunc() { while (background_read_running_) { - if (reconnect_mutex_.try_lock()) + std::unique_lock lock(reconnect_mutex_, std::defer_lock); + if (lock.try_lock()) { if (prod_->tryGet(data_buffer1_)) { @@ -996,7 +998,7 @@ void RTDEClient::backgroundReadThreadFunc() } else { - reconnect_mutex_.unlock(); + lock.unlock(); auto period = std::chrono::duration(1.0 / target_frequency_); std::this_thread::sleep_for(period); } From c265855a36ed9dc5c65567cb948ce6a419bc624f Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 13 Feb 2026 14:41:40 +0100 Subject: [PATCH 37/38] Apply suggestions from code review --- include/ur_client_library/rtde/rtde_client.h | 4 +++- src/rtde/rtde_client.cpp | 24 ++++++++++---------- tests/test_rtde_parser.cpp | 2 +- 3 files changed, 16 insertions(+), 14 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index ba094ed1f..92d3693a2 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -340,6 +340,8 @@ class RTDEClient ClientState client_state_; + uint16_t protocol_version_; + constexpr static const double CB3_MAX_FREQUENCY = 125.0; constexpr static const double URE_MAX_FREQUENCY = 500.0; @@ -352,7 +354,7 @@ class RTDEClient uint16_t negotiateProtocolVersion(); bool queryURControlVersion(); void setTargetFrequency(); - bool setupOutputs(const uint16_t protocol_version); + bool setupOutputs(); bool setupInputs(); void disconnect(); diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 32407e8f6..a89cdf307 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -143,9 +143,9 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron prod_->setupProducer(max_num_tries, reconnection_time); client_state_ = ClientState::INITIALIZING; - uint16_t protocol_version = negotiateProtocolVersion(); + protocol_version_ = negotiateProtocolVersion(); // Protocol version must be above zero - if (protocol_version == 0) + if (protocol_version_ == 0) { client_state_ = ClientState::UNINITIALIZED; return false; @@ -161,7 +161,7 @@ bool RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron prod_->startProducer(); - is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(protocol_version); + is_rtde_comm_setup = is_rtde_comm_setup && setupOutputs(); is_rtde_comm_setup = is_rtde_comm_setup && isRobotBooted(); @@ -327,13 +327,13 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); - preallocated_data_pkg_ = DataPackage(output_recipe_); + preallocated_data_pkg_ = DataPackage(output_recipe_, protocol_version_); parser_ = RTDEParser(output_recipe_); prod_ = std::make_unique>(stream_, parser_); } -bool RTDEClient::setupOutputs(const uint16_t protocol_version) +bool RTDEClient::setupOutputs() { unsigned int num_retries = 0; size_t size; @@ -344,7 +344,7 @@ bool RTDEClient::setupOutputs(const uint16_t protocol_version) while (num_retries < MAX_REQUEST_RETRIES) { URCL_LOG_DEBUG("Sending output recipe"); - if (protocol_version == 2) + if (protocol_version_ == 2) { size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_); } @@ -534,7 +534,7 @@ bool RTDEClient::isRobotBooted() if (!sendStart()) return false; - std::unique_ptr package = std::make_unique(output_recipe_); + std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_); double timestamp = 0; int reading_count = 0; @@ -629,7 +629,7 @@ bool RTDEClient::sendStart() // Worst case we get a data package as part of a race condition in the communication. If we // didn't preallocate that, it might print a warning. - std::unique_ptr package = std::make_unique(output_recipe_); + std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_); unsigned int num_retries = 0; while (num_retries < MAX_REQUEST_RETRIES) { @@ -679,7 +679,7 @@ bool RTDEClient::sendPause() } // Worst case we get a data package as part of a race condition in the communication. If we // didn't preallocate that, it might print a warning. - std::unique_ptr package = std::make_unique(output_recipe_); + std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_); std::chrono::time_point start = std::chrono::steady_clock::now(); int seconds = 5; while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) @@ -954,8 +954,8 @@ void RTDEClient::startBackgroundRead() return; } background_read_running_ = true; - data_buffer0_ = std::make_unique(output_recipe_); - data_buffer1_ = std::make_unique(output_recipe_); + data_buffer0_ = std::make_unique(output_recipe_, protocol_version_); + data_buffer1_ = std::make_unique(output_recipe_, protocol_version_); background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); } @@ -979,7 +979,7 @@ void RTDEClient::backgroundReadThreadFunc() { if (prod_->tryGet(data_buffer1_)) { - reconnect_mutex_.unlock(); + lock.unlock(); rtde_interface::DataPackage* data_pkg = dynamic_cast(data_buffer1_.get()); if (data_pkg != nullptr) { diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index da6ce4518..44e1f18a4 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -64,7 +64,7 @@ TEST(rtde_parser, request_protocol_version) parser.parse(bp, product2); } if (rtde_interface::RequestProtocolVersion* data = - dynamic_cast(product.get())) + dynamic_cast(product2.get())) { EXPECT_EQ(data->accepted_, true); } From 4e233954621dc1d3bfa7b8fad236209eb054bee3 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 13 Feb 2026 15:15:02 +0100 Subject: [PATCH 38/38] Move back setting reconnection flag --- src/rtde/rtde_client.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index a89cdf307..9adeccd38 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -848,7 +848,6 @@ RTDEWriter& RTDEClient::getWriter() void RTDEClient::reconnect() { URCL_LOG_INFO("Reconnecting to the RTDE interface"); - reconnecting_ = true; // Locking mutex to ensure that calling getDataPackage doesn't influence the communication needed for reconfiguring // the RTDE connection std::lock_guard lock(reconnect_mutex_); @@ -943,6 +942,7 @@ void RTDEClient::reconnectCallback() { reconnecting_thread_.join(); } + reconnecting_ = true; reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this); }