From 6836fabcbcc91187df2f98260416bb80d69e4d23 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 11:45:08 +0200 Subject: [PATCH 01/18] Take RTDE data types from the robot's setup acknowledgement DataPackage carried a hand-maintained table of every subscribable field name and its data type. Every field the controller gained had to be added to it, and a field missing from it could not be used even though the robot would happily serve it. The robot already reports the data type of every field it acknowledges, so the table was a second source of truth for information we were being told anyway. The library now keeps only the list of data types the protocol defines and takes the per-field types from the setup acknowledgement. Applications do not have to change. A DataPackage is still constructed from a recipe, that is still the only place it allocates, and the robot's answer is applied to it afterwards in place. Because every RTDE type is trivially copyable and lives inline in the variant, changing which type a field holds touches no memory, so a package can be created before the connection exists and still be used in a real-time loop. tests/test_rtde_allocations.cpp pins this down by counting allocations across the receive and send paths. - Add DataPackage::getDataType(), reporting the type the robot gave a field so it does not have to be hardcoded, as an rtde_interface::DataType with toString() for the protocol's name. This is the only entry point the change adds; applying types, relaying them to the parser and writer, and asking whether a package is ready are all internal. - Remove DataPackage::g_type_list. - A field name the robot does not know is now reported by RTDEClient::init() instead of the constructor, still as an RTDEInvalidKeyException, since it is the robot that decides which names exist. - RTDEWriter::sendPackage() validates the package it is given against the robot's types, naming the field and both types on a mismatch, and sends unset fields as zeros. setData() can no longer catch a wrong type on its own because it is what establishes an input field's type. - getData() returns false rather than throwing std::bad_variant_access on a type mismatch, and takes the field name as a string_view so passing a literal does not allocate. Also fixes comm::TCPServer::writeUnchecked() reading its written out-parameter before assigning it, which made the fake RTDE server silently send nothing and is why the new robot-free tests could not run before. See doc/migration_notes.rst for the three behavioural differences. --- doc/architecture/rtde_client.rst | 75 +- doc/examples/rtde_client.rst | 9 +- doc/migration_notes.rst | 28 + examples/rtde_client.cpp | 2 - include/ur_client_library/rtde/data_package.h | 254 +++++- include/ur_client_library/rtde/rtde_parser.h | 27 + include/ur_client_library/rtde/rtde_writer.h | 23 +- src/comm/tcp_server.cpp | 1 + src/rtde/data_package.cpp | 763 ++++++++---------- src/rtde/rtde_client.cpp | 51 +- src/rtde/rtde_parser.cpp | 56 +- src/rtde/rtde_writer.cpp | 42 +- tests/CMakeLists.txt | 15 + tests/fake_rtde_server.cpp | 537 +++++++++++- tests/resources/generate_rtde_outputs.py | 5 +- tests/rtde_test_helpers.h | 88 ++ tests/test_pipeline.cpp | 7 +- tests/test_producer.cpp | 8 +- tests/test_rtde_allocations.cpp | 294 +++++++ tests/test_rtde_client.cpp | 59 +- tests/test_rtde_client_fake_server.cpp | 378 +++++++++ tests/test_rtde_data_package.cpp | 260 +++++- tests/test_rtde_parser.cpp | 66 +- tests/test_rtde_writer.cpp | 79 +- 24 files changed, 2537 insertions(+), 590 deletions(-) create mode 100644 tests/rtde_test_helpers.h create mode 100644 tests/test_rtde_allocations.cpp create mode 100644 tests/test_rtde_client_fake_server.cpp diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index b45823974..31022ca61 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -36,10 +36,20 @@ the :ref:`rtde_client_example` for an example of the blocking read method. { if (my_client.getDataPackage(data_pkg, READ_TIMEOUT)) { - std::cout << data_pkg->toString() << std::endl; + std::cout << data_pkg.toString() << std::endl; } } +.. note:: + Constructing the ``DataPackage`` is where its memory is allocated, so create it before entering + your control loop and reuse it: ``getDataPackage()`` and ``getDataPackageBlocking()`` don't + allocate. + + A recipe only lists field names. The data types belonging to them are reported by the robot when + it acknowledges the recipe, and the first package received applies them to your ``DataPackage``, + which costs no memory. Until that has happened ``getData()`` on the package fails. See `Field + data types`_ for how to ask a package what type it gave a field. + Upon construction, two recipe files have to be given, one for the RTDE inputs, one for the RTDE outputs. Please refer to the `RTDE guide `_ @@ -69,6 +79,53 @@ After calling ``my_client.start()``, data can be read from the 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. +Both methods parse into a ``DataPackage`` that the caller owns, which is what keeps the read path +free of memory allocations. The deprecated ``getDataPackage(timeout)`` overload, which returns a new +package instead, allocates on every call by design and is therefore not suited for real-time use. + +Field data types +~~~~~~~~~~~~~~~~ + +``getData()`` has to be given a variable of the field's own type, and returns ``false`` if it isn't. +Rather than hardcoding which type a field has, ask the package: ``getDataType()`` reports the +``DataType`` the robot gave a field, or nothing at all if the recipe hasn't been acknowledged yet. +This is useful for code that has to handle whatever recipe it is configured with, such as a bridge +to another middleware: + +.. code-block:: c++ + + const std::optional type = data_pkg.getDataType(field_name); + if (!type) + { + // Not part of the recipe, or the recipe hasn't been acknowledged yet + return; + } + + // For "actual_q" this prints "VECTOR6D", the same spelling the RTDE guide uses + std::cout << field_name << " is a " << rtde_interface::toString(*type) << std::endl; + + switch (*type) + { + case rtde_interface::DataType::DOUBLE: + { + double value; + data_pkg.getData(field_name, value); + break; + } + case rtde_interface::DataType::VECTOR6D: + { + vector6d_t value; + data_pkg.getData(field_name, value); + break; + } + // ... remaining types + } + +``DataType`` covers the complete set the protocol defines: ``BOOL``, ``UINT8``, ``UINT32``, +``UINT64``, ``INT32``, ``DOUBLE``, ``VECTOR3D``, ``VECTOR6D``, ``VECTOR6INT32`` and +``VECTOR6UINT32``. Switching over it exhaustively means the compiler will point out any case a +future protocol addition leaves unhandled. + Writing data ------------ @@ -105,11 +162,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(); - auto data_pkg = std::make_unique(my_client->getOutputRecipe()); + auto data_pkg = std::make_unique(my_client.getOutputRecipe()); my_client.start(); while (true) { - if (my_client.getDataPackage(data_package, READ_TIMEOUT)) + if (my_client.getDataPackage(data_pkg, READ_TIMEOUT)) { std::cout << data_pkg->toString() << std::endl; } @@ -125,6 +182,18 @@ The class offers specific methods for every RTDE input possible to write. Data is sent asynchronously to the RTDE interface. +To write several fields at once, construct a ``DataPackage`` from ``RTDEClient::getInputRecipe()``, +fill the fields you care about and pass it to ``sendPackage()``. Fields you leave alone are sent as +zeros. Since the robot decides what type each field has, ``sendPackage()`` is where a value written +with the wrong type is reported: + +.. code-block:: c++ + + rtde_interface::DataPackage input_pkg(my_client.getInputRecipe()); + input_pkg.setData("speed_slider_mask", 1); + input_pkg.setData("speed_slider_fraction", 0.5); + my_client.getWriter().sendPackage(input_pkg); + .. note:: The ``RTDEWriter`` will return ``false`` on any writing attempts for fields that have not been diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index 5339e7198..4e9779b28 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -56,15 +56,18 @@ fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method. :start-at: auto data_pkg = std::make_unique(my_client.getOutputRecipe()); :end-before: // Change the speed slider +Creating the package we read into is the last allocation the read path makes; the loop below reuses +the same package. The recipe only names the fields, so the first package received is also what tells +this one what type each of its fields has, which needs no further memory. + 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. + client. ``getData()`` returns ``false`` for an unknown key, and also if the type of the passed + variable doesn't match the type the robot reported for that field. Writing Data to the RTDE client ------------------------------- diff --git a/doc/migration_notes.rst b/doc/migration_notes.rst index bb271b5f0..a98a9c599 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -5,6 +5,34 @@ This document contains notes on the migration of the ur_client_library between m It contains only breaking changes. +RTDE field types come from the robot +------------------------------------ + +The data types of an RTDE recipe's fields are now taken from the robot's answer to the recipe setup, +instead of from a table of field names maintained inside the library. No application code has to +change for this: ``DataPackage`` is still constructed from a recipe, still allocates all of its +storage there, and is typed by the robot's answer afterwards, which costs no memory. + +Three consequences are worth knowing about: + +- **A field name the robot doesn't know is reported later.** Since the library no longer has its own + list of field names, a typo is caught when the robot rejects the recipe during + ``RTDEClient::init()`` rather than while constructing the ``RTDEClient``. It is still an + ``RTDEInvalidKeyException``, and ``ignore_unavailable_outputs`` still strips such fields instead. +- **A wrongly typed input field is reported when the package is sent.** ``DataPackage::setData()`` + decides a field's type from the value passed to it, so it can no longer tell on its own that the + robot expects something else. ``RTDEWriter::sendPackage()`` checks the package against the robot's + answer and names the field and both types if they disagree. +- **Reading a field as the wrong type no longer throws.** ``DataPackage::getData()`` used to let a + ``std::bad_variant_access`` escape when the passed variable didn't match the field's type. It now + returns ``false`` and logs which type the robot reported for that field, matching what its + documentation always promised. Code that caught that exception should check the return value + instead. + +On a ``DataPackage`` that hasn't been typed yet, meaning it has neither received data nor been +written to, ``getData()`` fails with an explanatory message instead of returning stale values, and +``getDataType()`` reports that the field has no type yet. + Migrating from 1.x.x to 2.x.x ----------------------------- diff --git a/examples/rtde_client.cpp b/examples/rtde_client.cpp index d92605c5a..995f359ab 100644 --- a/examples/rtde_client.cpp +++ b/examples/rtde_client.cpp @@ -40,7 +40,6 @@ 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) @@ -101,7 +100,6 @@ 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. - // 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); } diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 3a5d69b1b..fdb62577e 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -30,10 +30,14 @@ #define UR_CLIENT_LIBRARY_DATA_PACKAGE_H_INCLUDED #include -#include +#include +#include +#include +#include #include #include +#include "ur_client_library/log.h" #include "ur_client_library/types.h" #include "ur_client_library/rtde/rtde_package.h" @@ -41,6 +45,8 @@ namespace urcl { namespace rtde_interface { +class RTDEWriter; + /*! * \brief Possible values for the runtime state */ @@ -54,33 +60,87 @@ enum class RUNTIME_STATE : uint32_t RESUMING = 5 }; +/*! + * \brief The data types an RTDE field can have. + * + * This is the complete set the protocol defines. Which one a given field has is decided by the + * robot when it acknowledges a recipe, so this list is all the type knowledge the library needs to + * carry; see DataPackage::getDataType(). + */ +enum class DataType : uint8_t +{ + BOOL, + UINT8, + UINT32, + UINT64, + INT32, + DOUBLE, + VECTOR3D, + VECTOR6D, + VECTOR6INT32, + VECTOR6UINT32 +}; + +/*! + * \brief The name the RTDE protocol uses for a data type, e.g. "VECTOR6D". + * + * This is the spelling the robot uses on the wire and the RTDE guide uses in its field tables. + */ +std::string toString(const DataType type); + /*! * \brief The DataPackage class handles communication in the form of RTDE data packages both to and * from the robot. It contains functionality to parse and serialize packages for arbitrary recipes. + * + * A recipe only names the fields to exchange; their data types are reported by the robot in the + * RTDE setup acknowledgement. Constructing a package from a recipe therefore allocates all of its + * storage but leaves it *untyped*, and the acknowledgement later decides which type each field + * holds. Since every RTDE data type is trivially copyable with inline storage, that second step + * costs no memory, which is why a package can be created before a connection exists and still be + * used in a real-time loop: + * + * \code + * rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe()); // allocates here + * while (true) + * { + * my_client.getDataPackage(data_pkg, timeout); // types it once, then never allocates + * } + * \endcode + * + * Until a package has been typed, either by receiving into it or by writing to it with setData(), + * it cannot be parsed into or serialized and getData() will fail. */ class DataPackage : public RTDEPackage { public: - using _rtde_type_variant = std::variant; + /*! + * \brief The type a data field can hold. + * + * std::monostate is the state of a field whose type isn't decided yet, which is how a package + * constructed from a recipe alone starts out. It is also what distinguishes the fields an + * application has written from the ones it left alone. + */ + using _rtde_type_variant = std::variant; DataPackage() = delete; - DataPackage(const DataPackage& other) : DataPackage(other.recipe_) + DataPackage(const DataPackage& other) + : RTDEPackage(PackageType::RTDE_DATA_PACKAGE) + , recipe_id_(other.recipe_id_) + , data_(other.data_) + , recipe_(other.recipe_) + , protocol_version_(other.protocol_version_) { - this->data_ = other.data_; - this->protocol_version_ = other.protocol_version_; } - DataPackage& operator=(DataPackage& other) - { - this->data_ = other.data_; - this->recipe_ = other.recipe_; - this->protocol_version_ = other.protocol_version_; - return *this; - } - - DataPackage operator=(const DataPackage& other) + /*! + * \brief Copies recipe, type information and values from another package. + * + * The recipe id is deliberately left untouched: an RTDEWriter's send buffers own the id that was + * negotiated during the input setup, while packages passed in by an application have none. + */ + DataPackage& operator=(const DataPackage& other) { this->data_ = other.data_; this->recipe_ = other.recipe_; @@ -89,25 +149,48 @@ class DataPackage : public RTDEPackage } /*! - * \brief Creates a new DataPackage object, based on a given recipe. + * \brief Creates a new DataPackage object based on a given recipe, allocating all of its storage. * - * \param recipe The used recipe + * The data types of the recipe's fields are only known once the robot has acknowledged the + * recipe, so the package starts out *untyped*: it cannot be parsed into or serialized, and + * getData() fails, until it has been typed. That happens either by receiving into it (see + * RTDEClient::getDataPackage()) or, for input recipes, by writing to it with setData(). + * + * Typing a package does not allocate, so this constructor is the only point at which the package + * touches the heap. Call it wherever suits your application; it needs no connection. * + * \param recipe The used recipe * \param protocol_version Protocol version used for the RTDE communication */ DataPackage(const std::vector& recipe, const uint16_t& protocol_version = 2) : RTDEPackage(PackageType::RTDE_DATA_PACKAGE), recipe_(recipe), protocol_version_(protocol_version) { - initEmpty(); + initStorage(); } virtual ~DataPackage() = default; /*! - * \brief Initializes to contained list with empty values based on the recipe. + * \brief Resets every data field to a default-constructed value of its own type. + * + * The types are left alone, so a typed package stays typed. */ void initEmpty(); + /*! + * \brief Get the data type the robot reported for a field. + * + * Which type a field holds is decided by the robot when it acknowledges the recipe, so this is + * the way to find out what to pass to getData() without hardcoding it. A package that hasn't + * been acknowledged yet has no answer to give. + * + * \param name The string identifier for the data field as used in the documentation. + * + * \returns The field's data type, or an empty optional if the field cannot be found inside the + * package or if its type isn't known yet. + */ + std::optional getDataType(const std::string_view name) const; + /*! * \brief Sets the attributes of the package by parsing a serialized representation of the * package. @@ -141,23 +224,27 @@ class DataPackage : public RTDEPackage * \param name The string identifier for the data field as used in the documentation. * \param val Target variable. Make sure, it's the correct type. * - * \returns True on success, false if the field cannot be found inside the package. + * \returns True on success, false if the field cannot be found inside the package or if its type + * doesn't match the requested one. */ template - bool getData(const std::string& name, T& val) const + bool getData(const std::string_view name, T& val) const { const auto it = std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { return element.first == name; }); - if (it != data_.end()) + if (it == data_.end()) { - val = std::get(it->second); + return false; } - else + const T* value = std::get_if(&it->second); + if (value == nullptr) { + reportReadFailure(name, it->second); return false; } + val = *value; return true; } @@ -169,10 +256,11 @@ class DataPackage : public RTDEPackage * \param name The string identifier for the data field as used in the documentation. * \param val Target variable. Make sure, it's the correct type. * - * \returns True on success, false if the field cannot be found inside the package. + * \returns True on success, false if the field cannot be found inside the package or if its type + * doesn't match the requested one. */ template - bool getData(const std::string& name, std::bitset& val) const + bool getData(const std::string_view name, std::bitset& val) const { static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable"); @@ -180,14 +268,17 @@ class DataPackage : public RTDEPackage std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { return element.first == name; }); - if (it != data_.end()) + if (it == data_.end()) { - val = std::bitset(std::get(it->second)); + return false; } - else + const T* value = std::get_if(&it->second); + if (value == nullptr) { + reportReadFailure(name, it->second); return false; } + val = std::bitset(*value); return true; } @@ -196,33 +287,36 @@ class DataPackage : public RTDEPackage * * The data package contains a lot of different data fields, depending on the recipe. * + * On a field whose type isn't decided yet this establishes the type from \p val. Whether that + * matches what the robot expects is checked when the package is sent, since only then is the + * robot's acknowledgement available. On a field that already has a type, \p val has to match it. + * * \param name The string identifier for the data field as used in the documentation. * \param val Value to set. Make sure, it's the correct type. * - * \returns True on success, false if the field cannot be found inside the package. + * \returns True on success, false if the field cannot be found inside the package or if its type + * doesn't match the passed one. */ template - bool setData(const std::string& name, const T& val) + bool setData(const std::string_view name, const T& val) { const auto it = std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { return element.first == name; }); - if (it != data_.end()) + 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; + return false; } - else + if (!std::holds_alternative(it->second) && !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'", + static_cast(name.size()), name.data()); return false; } + it->second = val; return true; } @@ -236,10 +330,82 @@ class DataPackage : public RTDEPackage recipe_id_ = recipe_id; } +protected: + // Applying the robot's setup acknowledgement to a package is the library's job: the parser does + // it on the way in, the writer when the input recipe is acknowledged, and the client for the + // package it reads into. An application never has the types to pass here. + friend class RTDEWriter; + friend class RTDEClient; + friend class RTDEParser; + + /*! + * \brief Applies the data types reported by the robot, resetting all values to zero. + * + * The storage was already allocated by the constructor, so this only decides which type each + * field holds and therefore performs no memory allocation. That is what allows a package to be + * created before the recipe has been acknowledged and still be used in a real-time loop. + * + * \param types The data types of the recipe's fields, in the same order as the recipe + * + * \throws UrException if the number of types doesn't match the recipe or if a type is unknown + */ + void initEmpty(const std::vector& types); + private: - // Const would be better here - static std::unordered_map g_type_list; - uint8_t recipe_id_; + /*! + * \brief Whether every field of this package has a data type. + * + * A package constructed from a recipe alone is untyped until either the robot's setup + * acknowledgement has been applied to it or setData() has been used to write to every field. An + * untyped package cannot be parsed into or serialized, and getData() fails on it. + * + * There is no separate flag for this: a field whose type is undecided holds a std::monostate, so + * the fields themselves are the answer. The scan is over recipe-many variant tags and costs far + * less than the parse it guards. + * + * \returns True if the package carries type information for all of its fields + */ + bool isTyped() const + { + return std::none_of(data_.begin(), data_.end(), [](const std::pair& field) { + return std::holds_alternative(field.second); + }); + } + + /*! + * \brief Resets a data field to a default-constructed value of its own type. + * + * \param name The string identifier for the data field as used in the documentation. + * + * \returns True on success, false if the field cannot be found inside the package. + */ + bool resetData(const std::string_view name); + + /*! + * \brief Copies the fields that \p other has values for into this package. + * + * Fields \p other hasn't written are left untouched, which is what lets an application send an + * input package covering only part of the recipe. This package keeps its own types, so it is + * where a type disagreement between the application and the robot surfaces. + * + * \param other The package to copy values from + * + * \returns True if every value could be copied, false if \p other names a field this package + * doesn't have or holds a value of a different type than the robot reported for it + */ + bool copySetFieldsFrom(const DataPackage& other); + + /*! + * \brief Allocates one slot per recipe field, with the type left undecided. + */ + void initStorage(); + + /*! + * \brief Logs why reading \p field didn't produce the requested type. + */ + static void reportReadFailure(const std::string_view name, const _rtde_type_variant& field); + + uint8_t recipe_id_ = 0; std::vector> data_; std::vector recipe_; uint16_t protocol_version_; diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 9c97827b0..d4326f731 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -49,6 +49,9 @@ class RTDEParser : public comm::Parser /*! * \brief Creates a new RTDEParser object, registering the used recipe. * + * The data types belonging to the recipe are only known once the robot has acknowledged it, so + * setRecipeTypes() has to be called before data packages can be parsed. + * * \param recipe The recipe used in RTDE data communication */ RTDEParser(const std::vector& recipe) : recipe_(recipe), protocol_version_(1) @@ -94,8 +97,32 @@ class RTDEParser : public comm::Parser return protocol_version_; } +protected: + // Relays the data types from the robot's setup acknowledgement, which only the client receives. + friend class RTDEClient; + + /*! + * \brief Registers the data types belonging to the recipe, as reported by the robot in the RTDE + * setup acknowledgement. + * + * This has to be called before the robot starts sending data packages, i.e. before the + * RTDE_CONTROL_PACKAGE_START request is sent. + * + * \param types The data types, in the same order as the recipe + */ + void setRecipeTypes(const std::vector& types) + { + recipe_types_ = types; + } + private: + static std::unique_ptr makeTypedDataPackage(const std::vector& recipe, + const std::vector& types, + const uint16_t protocol_version); + std::vector recipe_; + std::vector recipe_types_; + bool recipeTypesKnown() const; PackageType getPackageTypeFromHeader(comm::BinParser& bp) const; RTDEPackage* createNewPackageFromType(PackageType type) const; diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 8b482454f..807b37183 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -95,7 +95,12 @@ class RTDEWriter * 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 + * Only the fields \p package has values for are taken over; the rest of the input recipe is sent + * as zeros. The values are checked against the data types the robot reported for the input + * recipe, so a field written with the wrong type is reported here rather than silently corrupting + * the package. + * + * \param package The package to send, constructed from the client's input recipe * * \returns Success of the package creation */ @@ -192,6 +197,22 @@ class RTDEWriter */ bool sendExternalForceTorque(const vector6d_t& external_force_torque); +protected: + // Relays the data types from the robot's setup acknowledgement, which only the client receives. + friend class RTDEClient; + + /*! + * \brief Applies the data types the robot reported for the input recipe. + * + * This is what makes the send buffers usable, and it is also the reference against which values + * passed to sendPackage() are checked. + * + * \param types The data types of the input recipe's fields, in the same order as the recipe + * + * \throws UrException if the number of types doesn't match the recipe or if a type is unknown + */ + void setRecipeTypes(const std::vector& types); + private: void resetMasks(const std::shared_ptr& buffer); void markStorageToBeSent(); diff --git a/src/comm/tcp_server.cpp b/src/comm/tcp_server.cpp index b0d3646c2..9760d9964 100644 --- a/src/comm/tcp_server.cpp +++ b/src/comm/tcp_server.cpp @@ -456,6 +456,7 @@ bool TCPServer::write(const socket_t fd, const uint8_t* buf, const size_t buf_le bool TCPServer::writeUnchecked(const socket_t fd, const uint8_t* buf, const size_t buf_len, size_t& written) { + written = 0; size_t remaining = buf_len; // handle partial sends diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index b2abff24d..bb0b2e75d 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -29,463 +29,268 @@ #include "ur_client_library/rtde/data_package.h" #include + +#include "ur_client_library/exceptions.h" + namespace urcl { namespace rtde_interface { -std::unordered_map DataPackage::g_type_list{ - // INPUTS - { "speed_slider_mask", uint32_t() }, - { "speed_slider_fraction", double() }, - { "standard_digital_output_mask", uint8_t() }, - { "standard_digital_output", uint8_t() }, - { "configurable_digital_output_mask", uint8_t() }, - { "configurable_digital_output", uint8_t() }, - { "standard_analog_output_mask", uint8_t() }, - { "standard_analog_output_type", uint8_t() }, - { "standard_analog_output_0", double() }, - { "standard_analog_output_1", double() }, - { "external_force_torque", vector6d_t() }, +namespace +{ +/*! + * \brief Whether the alternative a visitor was handed is the "type not decided yet" one. + * + * The visitors below are only reached on typed packages, but they still have to compile for every + * alternative of the variant. + */ +template +constexpr bool is_untyped_v = std::is_same_v, std::monostate>; + +/*! + * \brief The RTDE protocol's name for each data type. + * + * The single place the spellings live. Both directions of the name conversion read from it, so a + * name can never disagree with itself. + */ +constexpr struct +{ + DataType type; + std::string_view name; +} g_type_names[] = { + { DataType::BOOL, "BOOL" }, + { DataType::UINT8, "UINT8" }, + { DataType::UINT32, "UINT32" }, + { DataType::UINT64, "UINT64" }, + { DataType::INT32, "INT32" }, + { DataType::DOUBLE, "DOUBLE" }, + { DataType::VECTOR3D, "VECTOR3D" }, + { DataType::VECTOR6D, "VECTOR6D" }, + { DataType::VECTOR6INT32, "VECTOR6INT32" }, + { DataType::VECTOR6UINT32, "VECTOR6UINT32" }, +}; + +/*! + * \brief The data type a field holds, or an empty optional if it has none yet. + */ +std::optional typeOf(const DataPackage::_rtde_type_variant& field) +{ + if (std::holds_alternative(field)) + { + return DataType::BOOL; + } + if (std::holds_alternative(field)) + { + return DataType::UINT8; + } + if (std::holds_alternative(field)) + { + return DataType::UINT32; + } + if (std::holds_alternative(field)) + { + return DataType::UINT64; + } + if (std::holds_alternative(field)) + { + return DataType::INT32; + } + if (std::holds_alternative(field)) + { + return DataType::DOUBLE; + } + if (std::holds_alternative(field)) + { + return DataType::VECTOR3D; + } + if (std::holds_alternative(field)) + { + return DataType::VECTOR6D; + } + if (std::holds_alternative(field)) + { + return DataType::VECTOR6INT32; + } + if (std::holds_alternative(field)) + { + return DataType::VECTOR6UINT32; + } + return std::nullopt; +} + +/*! + * \brief Names the type a field holds for an error message, even if it has none. + */ +std::string typeNameOf(const DataPackage::_rtde_type_variant& field) +{ + const std::optional type = typeOf(field); + return type.has_value() ? toString(*type) : "unknown"; +} - // INPUT / OUTPUT - { "input_bit_registers0_to_31", uint32_t() }, - { "input_bit_registers32_to_63", uint32_t() }, - { "input_bit_register_64", bool() }, - { "input_bit_register_65", bool() }, - { "input_bit_register_66", bool() }, - { "input_bit_register_67", bool() }, - { "input_bit_register_68", bool() }, - { "input_bit_register_69", bool() }, - { "input_bit_register_70", bool() }, - { "input_bit_register_71", bool() }, - { "input_bit_register_72", bool() }, - { "input_bit_register_73", bool() }, - { "input_bit_register_74", bool() }, - { "input_bit_register_75", bool() }, - { "input_bit_register_76", bool() }, - { "input_bit_register_77", bool() }, - { "input_bit_register_78", bool() }, - { "input_bit_register_79", bool() }, - { "input_bit_register_80", bool() }, - { "input_bit_register_81", bool() }, - { "input_bit_register_82", bool() }, - { "input_bit_register_83", bool() }, - { "input_bit_register_84", bool() }, - { "input_bit_register_85", bool() }, - { "input_bit_register_86", bool() }, - { "input_bit_register_87", bool() }, - { "input_bit_register_88", bool() }, - { "input_bit_register_89", bool() }, - { "input_bit_register_90", bool() }, - { "input_bit_register_91", bool() }, - { "input_bit_register_92", bool() }, - { "input_bit_register_93", bool() }, - { "input_bit_register_94", bool() }, - { "input_bit_register_95", bool() }, - { "input_bit_register_96", bool() }, - { "input_bit_register_97", bool() }, - { "input_bit_register_98", bool() }, - { "input_bit_register_99", bool() }, - { "input_bit_register_100", bool() }, - { "input_bit_register_101", bool() }, - { "input_bit_register_102", bool() }, - { "input_bit_register_103", bool() }, - { "input_bit_register_104", bool() }, - { "input_bit_register_105", bool() }, - { "input_bit_register_106", bool() }, - { "input_bit_register_107", bool() }, - { "input_bit_register_108", bool() }, - { "input_bit_register_109", bool() }, - { "input_bit_register_110", bool() }, - { "input_bit_register_111", bool() }, - { "input_bit_register_112", bool() }, - { "input_bit_register_113", bool() }, - { "input_bit_register_114", bool() }, - { "input_bit_register_115", bool() }, - { "input_bit_register_116", bool() }, - { "input_bit_register_117", bool() }, - { "input_bit_register_118", bool() }, - { "input_bit_register_119", bool() }, - { "input_bit_register_120", bool() }, - { "input_bit_register_121", bool() }, - { "input_bit_register_122", bool() }, - { "input_bit_register_123", bool() }, - { "input_bit_register_124", bool() }, - { "input_bit_register_125", bool() }, - { "input_bit_register_126", bool() }, - { "input_bit_register_127", bool() }, - { "input_int_register_0", int32_t() }, - { "input_int_register_1", int32_t() }, - { "input_int_register_2", int32_t() }, - { "input_int_register_3", int32_t() }, - { "input_int_register_4", int32_t() }, - { "input_int_register_5", int32_t() }, - { "input_int_register_6", int32_t() }, - { "input_int_register_7", int32_t() }, - { "input_int_register_8", int32_t() }, - { "input_int_register_9", int32_t() }, - { "input_int_register_10", int32_t() }, - { "input_int_register_11", int32_t() }, - { "input_int_register_12", int32_t() }, - { "input_int_register_13", int32_t() }, - { "input_int_register_14", int32_t() }, - { "input_int_register_15", int32_t() }, - { "input_int_register_16", int32_t() }, - { "input_int_register_17", int32_t() }, - { "input_int_register_18", int32_t() }, - { "input_int_register_19", int32_t() }, - { "input_int_register_20", int32_t() }, - { "input_int_register_21", int32_t() }, - { "input_int_register_22", int32_t() }, - { "input_int_register_23", int32_t() }, - { "input_int_register_24", int32_t() }, - { "input_int_register_25", int32_t() }, - { "input_int_register_26", int32_t() }, - { "input_int_register_27", int32_t() }, - { "input_int_register_28", int32_t() }, - { "input_int_register_29", int32_t() }, - { "input_int_register_30", int32_t() }, - { "input_int_register_31", int32_t() }, - { "input_int_register_32", int32_t() }, - { "input_int_register_33", int32_t() }, - { "input_int_register_34", int32_t() }, - { "input_int_register_35", int32_t() }, - { "input_int_register_36", int32_t() }, - { "input_int_register_37", int32_t() }, - { "input_int_register_38", int32_t() }, - { "input_int_register_39", int32_t() }, - { "input_int_register_40", int32_t() }, - { "input_int_register_41", int32_t() }, - { "input_int_register_42", int32_t() }, - { "input_int_register_43", int32_t() }, - { "input_int_register_44", int32_t() }, - { "input_int_register_45", int32_t() }, - { "input_int_register_46", int32_t() }, - { "input_int_register_47", int32_t() }, - { "input_double_register_0", double() }, - { "input_double_register_1", double() }, - { "input_double_register_2", double() }, - { "input_double_register_3", double() }, - { "input_double_register_4", double() }, - { "input_double_register_5", double() }, - { "input_double_register_6", double() }, - { "input_double_register_7", double() }, - { "input_double_register_8", double() }, - { "input_double_register_9", double() }, - { "input_double_register_10", double() }, - { "input_double_register_11", double() }, - { "input_double_register_12", double() }, - { "input_double_register_13", double() }, - { "input_double_register_14", double() }, - { "input_double_register_15", double() }, - { "input_double_register_16", double() }, - { "input_double_register_17", double() }, - { "input_double_register_18", double() }, - { "input_double_register_19", double() }, - { "input_double_register_20", double() }, - { "input_double_register_21", double() }, - { "input_double_register_22", double() }, - { "input_double_register_23", double() }, - { "input_double_register_24", double() }, - { "input_double_register_25", double() }, - { "input_double_register_26", double() }, - { "input_double_register_27", double() }, - { "input_double_register_28", double() }, - { "input_double_register_29", double() }, - { "input_double_register_30", double() }, - { "input_double_register_31", double() }, - { "input_double_register_32", double() }, - { "input_double_register_33", double() }, - { "input_double_register_34", double() }, - { "input_double_register_35", double() }, - { "input_double_register_36", double() }, - { "input_double_register_37", double() }, - { "input_double_register_38", double() }, - { "input_double_register_39", double() }, - { "input_double_register_40", double() }, - { "input_double_register_41", double() }, - { "input_double_register_42", double() }, - { "input_double_register_43", double() }, - { "input_double_register_44", double() }, - { "input_double_register_45", double() }, - { "input_double_register_46", double() }, - { "input_double_register_47", double() }, +/*! + * \brief Creates an empty value of the given data type. + * + * Switching over the enum rather than testing names in sequence means the compiler points at this + * function if a data type is ever added to the protocol. + */ +DataPackage::_rtde_type_variant variantFor(const DataType type) +{ + switch (type) + { + case DataType::BOOL: + return bool(); + case DataType::UINT8: + return uint8_t(); + case DataType::UINT32: + return uint32_t(); + case DataType::UINT64: + return uint64_t(); + case DataType::INT32: + return int32_t(); + case DataType::DOUBLE: + return double(); + case DataType::VECTOR3D: + return vector3d_t(); + case DataType::VECTOR6D: + return vector6d_t(); + case DataType::VECTOR6INT32: + return vector6int32_t(); + case DataType::VECTOR6UINT32: + return vector6uint32_t(); + } + throw UrException("Unhandled RTDE data type."); +} - // OUTPUTS - { "timestamp", double() }, - { "target_q", vector6d_t() }, - { "target_qd", vector6d_t() }, - { "target_qdd", vector6d_t() }, - { "target_current", vector6d_t() }, - { "target_moment", vector6d_t() }, - { "actual_q", vector6d_t() }, - { "actual_qd", vector6d_t() }, - { "actual_current", vector6d_t() }, - { "actual_current_window", vector6d_t() }, - { "actual_current_as_torque", vector6d_t() }, - { "joint_control_output", vector6d_t() }, - { "actual_TCP_pose", vector6d_t() }, - { "actual_TCP_speed", vector6d_t() }, - { "actual_TCP_force", vector6d_t() }, - { "target_TCP_pose", vector6d_t() }, - { "target_TCP_speed", vector6d_t() }, - { "tcp_offset", vector6d_t() }, - { "actual_TCP_acceleration", vector6d_t() }, - { "target_TCP_acceleration", vector6d_t() }, - { "actual_digital_input_bits", uint64_t() }, - { "actual_configurable_digital_input_bits", uint64_t() }, - { "joint_temperatures", vector6d_t() }, - { "actual_execution_time", double() }, - { "target_execution_time", double() }, - { "robot_mode", int32_t() }, - { "joint_mode", vector6int32_t() }, - { "safety_mode", int32_t() }, - { "safety_status", int32_t() }, - { "actual_tool_accelerometer", vector3d_t() }, - { "speed_scaling", double() }, - { "target_speed_fraction", double() }, - { "actual_momentum", double() }, - { "actual_main_voltage", double() }, - { "actual_robot_voltage", double() }, - { "actual_robot_current", double() }, - { "actual_joint_voltage", vector6d_t() }, - { "actual_digital_output_bits", uint64_t() }, - { "actual_configurable_digital_output_bits", uint64_t() }, - { "runtime_state", uint32_t() }, - { "elbow_position", vector3d_t() }, - { "elbow_velocity", vector3d_t() }, - { "robot_status_bits", uint32_t() }, - { "safety_status_bits", uint32_t() }, - { "analog_io_types", uint32_t() }, - { "standard_analog_input0", double() }, - { "standard_analog_input1", double() }, - { "standard_analog_output0", double() }, - { "standard_analog_output1", double() }, - { "io_current", double() }, - { "output_bit_registers0_to_31", uint32_t() }, - { "output_bit_registers32_to_63", uint32_t() }, - { "output_bit_register_64", bool() }, - { "output_bit_register_65", bool() }, - { "output_bit_register_66", bool() }, - { "output_bit_register_67", bool() }, - { "output_bit_register_68", bool() }, - { "output_bit_register_69", bool() }, - { "output_bit_register_70", bool() }, - { "output_bit_register_71", bool() }, - { "output_bit_register_72", bool() }, - { "output_bit_register_73", bool() }, - { "output_bit_register_74", bool() }, - { "output_bit_register_75", bool() }, - { "output_bit_register_76", bool() }, - { "output_bit_register_77", bool() }, - { "output_bit_register_78", bool() }, - { "output_bit_register_79", bool() }, - { "output_bit_register_80", bool() }, - { "output_bit_register_81", bool() }, - { "output_bit_register_82", bool() }, - { "output_bit_register_83", bool() }, - { "output_bit_register_84", bool() }, - { "output_bit_register_85", bool() }, - { "output_bit_register_86", bool() }, - { "output_bit_register_87", bool() }, - { "output_bit_register_88", bool() }, - { "output_bit_register_89", bool() }, - { "output_bit_register_90", bool() }, - { "output_bit_register_91", bool() }, - { "output_bit_register_92", bool() }, - { "output_bit_register_93", bool() }, - { "output_bit_register_94", bool() }, - { "output_bit_register_95", bool() }, - { "output_bit_register_96", bool() }, - { "output_bit_register_97", bool() }, - { "output_bit_register_98", bool() }, - { "output_bit_register_99", bool() }, - { "output_bit_register_100", bool() }, - { "output_bit_register_101", bool() }, - { "output_bit_register_102", bool() }, - { "output_bit_register_103", bool() }, - { "output_bit_register_104", bool() }, - { "output_bit_register_105", bool() }, - { "output_bit_register_106", bool() }, - { "output_bit_register_107", bool() }, - { "output_bit_register_108", bool() }, - { "output_bit_register_109", bool() }, - { "output_bit_register_110", bool() }, - { "output_bit_register_111", bool() }, - { "output_bit_register_112", bool() }, - { "output_bit_register_113", bool() }, - { "output_bit_register_114", bool() }, - { "output_bit_register_115", bool() }, - { "output_bit_register_116", bool() }, - { "output_bit_register_117", bool() }, - { "output_bit_register_118", bool() }, - { "output_bit_register_119", bool() }, - { "output_bit_register_120", bool() }, - { "output_bit_register_121", bool() }, - { "output_bit_register_122", bool() }, - { "output_bit_register_123", bool() }, - { "output_bit_register_124", bool() }, - { "output_bit_register_125", bool() }, - { "output_bit_register_126", bool() }, - { "output_bit_register_127", bool() }, - { "output_int_register_0", int32_t() }, - { "output_int_register_1", int32_t() }, - { "output_int_register_2", int32_t() }, - { "output_int_register_3", int32_t() }, - { "output_int_register_4", int32_t() }, - { "output_int_register_5", int32_t() }, - { "output_int_register_6", int32_t() }, - { "output_int_register_7", int32_t() }, - { "output_int_register_8", int32_t() }, - { "output_int_register_9", int32_t() }, - { "output_int_register_10", int32_t() }, - { "output_int_register_11", int32_t() }, - { "output_int_register_12", int32_t() }, - { "output_int_register_13", int32_t() }, - { "output_int_register_14", int32_t() }, - { "output_int_register_15", int32_t() }, - { "output_int_register_16", int32_t() }, - { "output_int_register_17", int32_t() }, - { "output_int_register_18", int32_t() }, - { "output_int_register_19", int32_t() }, - { "output_int_register_20", int32_t() }, - { "output_int_register_21", int32_t() }, - { "output_int_register_22", int32_t() }, - { "output_int_register_23", int32_t() }, - { "output_int_register_24", int32_t() }, - { "output_int_register_25", int32_t() }, - { "output_int_register_26", int32_t() }, - { "output_int_register_27", int32_t() }, - { "output_int_register_28", int32_t() }, - { "output_int_register_29", int32_t() }, - { "output_int_register_30", int32_t() }, - { "output_int_register_31", int32_t() }, - { "output_int_register_32", int32_t() }, - { "output_int_register_33", int32_t() }, - { "output_int_register_34", int32_t() }, - { "output_int_register_35", int32_t() }, - { "output_int_register_36", int32_t() }, - { "output_int_register_37", int32_t() }, - { "output_int_register_38", int32_t() }, - { "output_int_register_39", int32_t() }, - { "output_int_register_40", int32_t() }, - { "output_int_register_41", int32_t() }, - { "output_int_register_42", int32_t() }, - { "output_int_register_43", int32_t() }, - { "output_int_register_44", int32_t() }, - { "output_int_register_45", int32_t() }, - { "output_int_register_46", int32_t() }, - { "output_int_register_47", int32_t() }, - { "output_double_register_0", double() }, - { "output_double_register_1", double() }, - { "output_double_register_2", double() }, - { "output_double_register_3", double() }, - { "output_double_register_4", double() }, - { "output_double_register_5", double() }, - { "output_double_register_6", double() }, - { "output_double_register_7", double() }, - { "output_double_register_8", double() }, - { "output_double_register_9", double() }, - { "output_double_register_10", double() }, - { "output_double_register_11", double() }, - { "output_double_register_12", double() }, - { "output_double_register_13", double() }, - { "output_double_register_14", double() }, - { "output_double_register_15", double() }, - { "output_double_register_16", double() }, - { "output_double_register_17", double() }, - { "output_double_register_18", double() }, - { "output_double_register_19", double() }, - { "output_double_register_20", double() }, - { "output_double_register_21", double() }, - { "output_double_register_22", double() }, - { "output_double_register_23", double() }, - { "output_double_register_24", double() }, - { "output_double_register_25", double() }, - { "output_double_register_26", double() }, - { "output_double_register_27", double() }, - { "output_double_register_28", double() }, - { "output_double_register_29", double() }, - { "output_double_register_30", double() }, - { "output_double_register_31", double() }, - { "output_double_register_32", double() }, - { "output_double_register_33", double() }, - { "output_double_register_34", double() }, - { "output_double_register_35", double() }, - { "output_double_register_36", double() }, - { "output_double_register_37", double() }, - { "output_double_register_38", double() }, - { "output_double_register_39", double() }, - { "output_double_register_40", double() }, - { "output_double_register_41", double() }, - { "output_double_register_42", double() }, - { "output_double_register_43", double() }, - { "output_double_register_44", double() }, - { "output_double_register_45", double() }, - { "output_double_register_46", double() }, - { "output_double_register_47", double() }, - { "actual_robot_energy_consumed", double() }, - { "actual_robot_braking_energy_dissipated", double() }, - { "encoder0_raw", int32_t() }, - { "encoder1_raw", int32_t() }, - { "euromap67_input_bits", uint32_t() }, - { "euromap67_output_bits", uint32_t() }, - { "euromap67_24V_voltage", double() }, - { "euromap67_24V_current", double() }, - { "tool_mode", uint32_t() }, - { "tool_analog_input_types", uint32_t() }, - { "tool_analog_input0", double() }, - { "tool_analog_input1", double() }, - { "tool_output_voltage", int32_t() }, - { "tool_output_current", double() }, - { "tool_temperature", double() }, - { "tool_output_mode", uint8_t() }, - { "tool_digital_output0_mode", uint8_t() }, - { "tool_digital_output1_mode", uint8_t() }, - { "tcp_force_scalar", double() }, - { "joint_position_deviation_ratio", double() }, - { "collision_detection_ratio", double() }, - { "ft_raw_wrench", vector6d_t() }, - { "wrench_calc_from_currents", vector6d_t() }, - { "payload", double() }, - { "payload_cog", vector3d_t() }, - { "payload_inertia", vector6d_t() }, - { "script_control_line", uint32_t() }, - { "time_scale_source", int32_t() }, - { "target_gravity", vector3d_t() }, - { "target_base_acceleration", vector6d_t() }, - { "control_step", uint64_t() }, - { "target_base_wrench", vector6d_t() }, +/*! + * \brief Creates an empty value of the RTDE data type with the given name. + * + * \param type_name One of the RTDE data type names as reported by the robot in a setup + * acknowledgement + * + * \throws UrException if the name is not a known RTDE data type + */ +DataPackage::_rtde_type_variant variantFromTypeName(const std::string_view type_name) +{ + for (const auto& entry : g_type_names) + { + if (entry.name == type_name) + { + return variantFor(entry.type); + } + } - // NOT IN OFFICIAL DOCS - { "tool_digital_output_mask", uint8_t() }, - { "tool_digital_output", uint8_t() }, -}; + std::stringstream ss; + ss << "'" << type_name + << "' is not a known RTDE data type. Expected one of BOOL, UINT8, UINT32, UINT64, INT32, " + "DOUBLE, VECTOR3D, VECTOR6D, VECTOR6INT32 or VECTOR6UINT32."; + throw UrException(ss.str()); +} +} // namespace -void rtde_interface::DataPackage::initEmpty() +std::string toString(const DataType type) { - data_.clear(); - data_.reserve(recipe_.size()); - for (auto& item : recipe_) + for (const auto& entry : g_type_names) { - if (g_type_list.find(item) == g_type_list.end()) + if (entry.type == type) { - throw RTDEInvalidKeyException("Unknown item in recipe: " + item); + return std::string(entry.name); } - _rtde_type_variant entry = g_type_list[item]; - data_.push_back({ item, entry }); + } + throw UrException("Unhandled RTDE data type."); +} + +std::optional rtde_interface::DataPackage::getDataType(const std::string_view name) const +{ + const auto it = + std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { + return element.first == name; + }); + if (it == data_.end()) + { + return std::nullopt; + } + return typeOf(it->second); +} + +void rtde_interface::DataPackage::reportReadFailure(const std::string_view name, const _rtde_type_variant& field) +{ + if (std::holds_alternative(field)) + { + URCL_LOG_ERROR("Cannot read the data field '%.*s', as its data type isn't known yet. The data types of a recipe " + "are reported by the robot during the RTDE handshake, so a data package can only be read from " + "after it has received data at least once.", + static_cast(name.size()), name.data()); + return; + } + URCL_LOG_ERROR("Type of requested data doesn't match type of existing field for index '%.*s'. The robot reports " + "that field as %s.", + static_cast(name.size()), name.data(), typeNameOf(field).c_str()); +} + +void rtde_interface::DataPackage::initStorage() +{ + data_.resize(recipe_.size()); + for (size_t i = 0; i < recipe_.size(); ++i) + { + data_[i].first = recipe_[i]; + data_[i].second = std::monostate(); + } +} + +void rtde_interface::DataPackage::initEmpty(const std::vector& types) +{ + if (types.size() != recipe_.size()) + { + std::stringstream ss; + ss << "Cannot initialize an RTDE data package: got " << types.size() << " data types for a recipe with " + << recipe_.size() << " fields."; + throw UrException(ss.str()); + } + + // The storage was allocated by the constructor and every RTDE type lives inline in the variant, + // so deciding the types here cannot allocate. That is what makes it safe to type a package that + // an application is already holding, in the middle of a real-time loop. + if (data_.size() != recipe_.size()) + { + initStorage(); + } + for (size_t i = 0; i < recipe_.size(); ++i) + { + data_[i].second = variantFromTypeName(types[i]); + } +} + +void rtde_interface::DataPackage::initEmpty() +{ + for (auto& item : data_) + { + std::visit([](auto&& arg) { arg = std::decay_t(); }, item.second); } } bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) { + if (!isTyped()) + { + URCL_LOG_ERROR("Cannot parse into an RTDE data package before the data types of its recipe are known. Those are " + "reported by the robot during the RTDE handshake."); + return false; + } + if (protocol_version_ == 2) { bp.parse(recipe_id_); } for (size_t i = 0; i < recipe_.size(); ++i) { - std::visit([&bp](auto&& arg) { bp.parse(arg); }, data_[i].second); + std::visit( + [&bp](auto&& arg) { + if constexpr (!is_untyped_v) + { + bp.parse(arg); + } + }, + data_[i].second); } return true; } @@ -502,7 +307,18 @@ std::string rtde_interface::DataPackage::toString() const } else { - std::visit([&ss](auto&& arg) { ss << arg; }, item.second); + std::visit( + [&ss](auto&& arg) { + if constexpr (is_untyped_v) + { + ss << ""; + } + else + { + ss << arg; + } + }, + item.second); } ss << std::endl; } @@ -511,11 +327,29 @@ std::string rtde_interface::DataPackage::toString() const size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) { + if (!isTyped()) + { + URCL_LOG_ERROR("Cannot serialize an RTDE data package before the data types of its recipe are known. Those are " + "reported by the robot during the RTDE handshake."); + return 0; + } + uint16_t payload_size = sizeof(recipe_id_); for (auto& item : data_) { - payload_size += std::visit([](auto&& arg) -> uint16_t { return sizeof(arg); }, item.second); + payload_size += std::visit( + [](auto&& arg) -> uint16_t { + if constexpr (is_untyped_v) + { + return 0; + } + else + { + return sizeof(arg); + } + }, + item.second); } size_t size = 0; size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size); @@ -523,11 +357,66 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) 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); }, + [&buffer, &size](auto&& arg) -> size_t { + if constexpr (is_untyped_v) + { + return 0; + } + else + { + return comm::PackageSerializer::serialize(buffer + size, arg); + } + }, data_[i].second); } return size; } + +bool rtde_interface::DataPackage::resetData(const std::string_view name) +{ + const auto it = + std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { + return element.first == name; + }); + if (it == data_.end()) + { + return false; + } + std::visit([](auto&& arg) { arg = std::decay_t(); }, it->second); + return true; +} + +bool rtde_interface::DataPackage::copySetFieldsFrom(const DataPackage& other) +{ + bool all_copied = true; + for (const auto& source : other.data_) + { + if (std::holds_alternative(source.second)) + { + continue; + } + + const auto destination = + std::find_if(data_.begin(), data_.end(), [&source](const std::pair& element) { + return element.first == source.first; + }); + if (destination == data_.end()) + { + URCL_LOG_ERROR("The data field '%s' is not part of the recipe the robot acknowledged.", source.first.c_str()); + all_copied = false; + continue; + } + if (source.second.index() != destination->second.index()) + { + URCL_LOG_ERROR("The value passed for the data field '%s' is of type %s, but the robot reports that field as %s.", + source.first.c_str(), typeNameOf(source.second).c_str(), typeNameOf(destination->second).c_str()); + all_copied = false; + continue; + } + destination->second = source.second; + } + return all_copied; +} } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 3000db6ef..3c5f5d1df 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -40,6 +40,8 @@ namespace urcl { namespace rtde_interface { +// The pre-allocated package gets its storage here, but the field types are only known once the +// robot has acknowledged the output recipe, which is when setupOutputs() applies them. RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file, const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs, const uint32_t port) @@ -328,6 +330,8 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); + // The data types of the new recipe are unknown until the robot acknowledges it again, at which + // point setupOutputs() applies them to this package without allocating. preallocated_data_pkg_ = DataPackage(output_recipe_, protocol_version_); parser_ = RTDEParser(output_recipe_); @@ -380,7 +384,13 @@ bool RTDEClient::setupOutputs() std::vector variable_types = splitString(tmp_output->variable_types_, ","); std::vector available_variables; std::vector unavailable_variables; - assert(output_recipe_.size() == variable_types.size()); + if (output_recipe_.size() != variable_types.size()) + { + URCL_LOG_ERROR("The robot acknowledged the output recipe with %zu data types while the recipe contains %zu " + "fields. Cannot set up the RTDE outputs.", + variable_types.size(), output_recipe_.size()); + return false; + } for (std::size_t i = 0; i < variable_types.size(); ++i) { const std::string variable_name = output_recipe_[i]; @@ -424,7 +434,12 @@ bool RTDEClient::setupOutputs() } else { - // All variables are accounted for in the RTDE package + // All variables are accounted for in the RTDE package. The robot told us their data types, + // so this is the point where everything holding received data learns what it holds. The + // storage itself already exists, so this doesn't allocate and neither does the receive path + // from here on. + parser_.setRecipeTypes(variable_types); + preallocated_data_pkg_.initEmpty(variable_types); return true; } } @@ -471,7 +486,13 @@ bool RTDEClient::setupInputs() { std::vector variable_types = splitString(tmp_input->variable_types_, ","); - assert(input_recipe_.size() == variable_types.size()); + if (input_recipe_.size() != variable_types.size()) + { + URCL_LOG_ERROR("The robot acknowledged the input recipe with %zu data types while the recipe contains %zu " + "fields. Cannot set up the RTDE inputs.", + variable_types.size(), input_recipe_.size()); + return false; + } for (std::size_t i = 0; i < variable_types.size(); ++i) { URCL_LOG_DEBUG("%s confirmed as datatype: %s", input_recipe_[i].c_str(), variable_types[i].c_str()); @@ -486,6 +507,7 @@ bool RTDEClient::setupInputs() throw RTDEInputConflictException(input_recipe_[i]); } } + writer_.setRecipeTypes(variable_types); writer_.init(tmp_input->input_recipe_id_); return true; @@ -524,7 +546,8 @@ bool RTDEClient::isRobotBooted() if (!sendStart()) return false; - std::unique_ptr package = std::make_unique(output_recipe_, protocol_version_); + // Shaped like the packages we are about to receive, so the parser doesn't have to allocate one + std::unique_ptr package = std::make_unique(preallocated_data_pkg_); double timestamp = 0; int reading_count = 0; @@ -619,7 +642,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_, protocol_version_); + std::unique_ptr package = std::make_unique(preallocated_data_pkg_); unsigned int num_retries = 0; while (num_retries < MAX_REQUEST_RETRIES) { @@ -669,7 +692,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_, protocol_version_); + std::unique_ptr package = std::make_unique(preallocated_data_pkg_); std::chrono::time_point start = std::chrono::steady_clock::now(); int seconds = 5; while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds)) @@ -943,9 +966,21 @@ void RTDEClient::startBackgroundRead() URCL_LOG_WARN("Requested to start RTDEClient's background read, while it is already running. Doing nothing."); return; } + if (!preallocated_data_pkg_.isTyped()) + { + URCL_LOG_ERROR("Cannot start RTDEClient's background read before the RTDE communication has been set up, as the " + "data types of the output recipe are reported by the robot. Please call init() first."); + return; + } background_read_running_ = true; - data_buffer0_ = std::make_unique(output_recipe_, protocol_version_); - data_buffer1_ = std::make_unique(output_recipe_, protocol_version_); + // Copying the package the blocking read uses gives these the same recipe and data types without + // needing to know what those are. Its values could be from an earlier read, so drop them. + auto buffer0 = std::make_unique(preallocated_data_pkg_); + auto buffer1 = std::make_unique(preallocated_data_pkg_); + buffer0->initEmpty(); + buffer1->initEmpty(); + data_buffer0_ = std::move(buffer0); + data_buffer1_ = std::move(buffer1); background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); } diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 48bd60c25..2d9e62d81 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -27,6 +27,28 @@ namespace urcl { namespace rtde_interface { +// A package allocates its storage from the recipe and learns its field types from the robot's +// acknowledgement afterwards, which costs no memory. +std::unique_ptr RTDEParser::makeTypedDataPackage(const std::vector& recipe, + const std::vector& types, + const uint16_t protocol_version) +{ + auto package = std::make_unique(recipe, protocol_version); + package->initEmpty(types); + return package; +} + +bool RTDEParser::recipeTypesKnown() const +{ + if (recipe_types_.size() == recipe_.size()) + { + return true; + } + URCL_LOG_ERROR("Received an RTDE data package while the data types of the output recipe are unknown. Those are " + "reported by the robot when it acknowledges the recipe, so this means a data package arrived before " + "the RTDE handshake was completed."); + return false; +} bool RTDEParser::parse(comm::BinParser& bp, std::vector>& results) { @@ -54,7 +76,11 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package(new DataPackage(recipe_, protocol_version_)); + if (!recipeTypesKnown()) + { + return false; + } + std::unique_ptr package = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); if (!package->parseWith(bp)) { @@ -104,6 +130,10 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result { case PackageType::RTDE_DATA_PACKAGE: { + if (!recipeTypesKnown()) + { + return false; + } if (result == nullptr || result->getType() != PackageType::RTDE_DATA_PACKAGE) { if (result == nullptr) @@ -119,10 +149,30 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result "a DataPackage would be sent.", result->getType()); } - result = std::make_unique(recipe_, protocol_version_); + result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + } + + DataPackage* data_package = dynamic_cast(result.get()); + if (!data_package->isTyped()) + { + // A package built from a recipe alone doesn't know its field types yet. Applying the ones + // the robot reported doesn't allocate, so this happens right here rather than by handing + // the caller a replacement package. + try + { + data_package->initEmpty(recipe_types_); + } + catch (const UrException& e) + { + URCL_LOG_ERROR("The passed pre-allocated DataPackage does not fit the negotiated output recipe (%s). A new " + "DataPackage will have to be allocated.", + e.what()); + result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + data_package = dynamic_cast(result.get()); + } } - if (!dynamic_cast(result.get())->parseWith(bp)) + if (!data_package->parseWith(bp)) { URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index c0c48ccdb..989f3cfbb 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -28,6 +28,7 @@ #include "ur_client_library/rtde/rtde_writer.h" #include +#include "ur_client_library/helpers.h" #include "ur_client_library/log.h" namespace urcl @@ -74,6 +75,9 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) used_masks_.push_back(field); } } + // All storage the send path needs is allocated here. The buffers stay unusable until the robot + // has reported the data types of the recipe's fields, which setRecipeTypes() then applies without + // allocating again. data_buffer0_ = std::make_shared(recipe_); data_buffer1_ = std::make_shared(recipe_); @@ -81,6 +85,13 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) current_send_buffer_ = data_buffer1_; } +void RTDEWriter::setRecipeTypes(const std::vector& types) +{ + std::lock_guard lock_guard(store_mutex_); + data_buffer0_->initEmpty(types); + data_buffer1_->initEmpty(types); +} + void RTDEWriter::init(uint8_t recipe_id) { if (running_) @@ -92,6 +103,8 @@ void RTDEWriter::init(uint8_t recipe_id) std::lock_guard lock_guard(store_mutex_); data_buffer0_->setRecipeID(recipe_id); data_buffer1_->setRecipeID(recipe_id); + current_store_buffer_ = data_buffer0_; + current_send_buffer_ = data_buffer1_; } recipe_id_ = recipe_id; new_data_available_ = false; @@ -143,7 +156,20 @@ void RTDEWriter::stop() bool RTDEWriter::sendPackage(const DataPackage& package) { std::lock_guard guard(store_mutex_); - *current_store_buffer_ = package; + if (!current_store_buffer_->isTyped()) + { + URCL_LOG_ERROR("Cannot send RTDE input data before the RTDE communication has been set up, as the data types of " + "the input recipe are reported by the robot."); + return false; + } + + // Fields the caller didn't write are sent as zeros rather than as whatever the previous package + // left in the buffer, so that a package means the same thing no matter what was sent before it. + current_store_buffer_->initEmpty(); + if (!current_store_buffer_->copySetFieldsFrom(package)) + { + return false; + } markStorageToBeSent(); return true; } @@ -404,19 +430,7 @@ void RTDEWriter::resetMasks(const std::shared_ptr& buffer) { for (const auto& mask_name : used_masks_) { - // "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); - } + buffer->resetData(mask_name); } } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 313489a16..513ab0ff1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -212,6 +212,21 @@ target_link_libraries(rtde_parser_tests PRIVATE ur_client_library::urcl GTest::g gtest_add_tests(TARGET rtde_parser_tests ) +# Checks that exchanging RTDE data doesn't allocate once the recipes have been set up. Uses the +# in-process fake RTDE server, so it runs without a robot. +add_executable(rtde_allocation_tests test_rtde_allocations.cpp fake_rtde_server.cpp) +target_link_libraries(rtde_allocation_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET rtde_allocation_tests +) + +# Covers RTDEClient's public interface against the in-process fake RTDE server. The tests in +# test_rtde_client.cpp go further but need a reachable robot, so they only run with INTEGRATION_TESTS. +add_executable(rtde_client_fake_server_tests test_rtde_client_fake_server.cpp fake_rtde_server.cpp) +target_link_libraries(rtde_client_fake_server_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET rtde_client_fake_server_tests + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +) + add_executable(tcp_server_tests test_tcp_server.cpp) if (MSVC) target_compile_options(tcp_server_tests PRIVATE /Zc:lambda) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index c0219029c..4dac1144e 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -1,10 +1,494 @@ #include "fake_rtde_server.h" +#include "rtde_test_helpers.h" #include +#include #include "ur_client_library/comm/package_serializer.h" #include "ur_client_library/log.h" namespace urcl { +namespace +{ +// The RTDE data type of every field a robot knows about. On a real robot this information is part +// of the answer to a recipe setup request, which is where the client library takes it from, so the +// test double has to be able to answer the same way. +// +// tests/resources/generate_rtde_outputs.py reads the output fields out of this table, so keep the +// section comments below intact. +// clang-format off +const std::unordered_map g_variable_types{ + // INPUTS + { "speed_slider_mask", "UINT32" }, + { "speed_slider_fraction", "DOUBLE" }, + { "standard_digital_output_mask", "UINT8" }, + { "standard_digital_output", "UINT8" }, + { "configurable_digital_output_mask", "UINT8" }, + { "configurable_digital_output", "UINT8" }, + { "standard_analog_output_mask", "UINT8" }, + { "standard_analog_output_type", "UINT8" }, + { "standard_analog_output_0", "DOUBLE" }, + { "standard_analog_output_1", "DOUBLE" }, + { "external_force_torque", "VECTOR6D" }, + + // INPUT / OUTPUT + { "input_bit_registers0_to_31", "UINT32" }, + { "input_bit_registers32_to_63", "UINT32" }, + { "input_bit_register_64", "BOOL" }, + { "input_bit_register_65", "BOOL" }, + { "input_bit_register_66", "BOOL" }, + { "input_bit_register_67", "BOOL" }, + { "input_bit_register_68", "BOOL" }, + { "input_bit_register_69", "BOOL" }, + { "input_bit_register_70", "BOOL" }, + { "input_bit_register_71", "BOOL" }, + { "input_bit_register_72", "BOOL" }, + { "input_bit_register_73", "BOOL" }, + { "input_bit_register_74", "BOOL" }, + { "input_bit_register_75", "BOOL" }, + { "input_bit_register_76", "BOOL" }, + { "input_bit_register_77", "BOOL" }, + { "input_bit_register_78", "BOOL" }, + { "input_bit_register_79", "BOOL" }, + { "input_bit_register_80", "BOOL" }, + { "input_bit_register_81", "BOOL" }, + { "input_bit_register_82", "BOOL" }, + { "input_bit_register_83", "BOOL" }, + { "input_bit_register_84", "BOOL" }, + { "input_bit_register_85", "BOOL" }, + { "input_bit_register_86", "BOOL" }, + { "input_bit_register_87", "BOOL" }, + { "input_bit_register_88", "BOOL" }, + { "input_bit_register_89", "BOOL" }, + { "input_bit_register_90", "BOOL" }, + { "input_bit_register_91", "BOOL" }, + { "input_bit_register_92", "BOOL" }, + { "input_bit_register_93", "BOOL" }, + { "input_bit_register_94", "BOOL" }, + { "input_bit_register_95", "BOOL" }, + { "input_bit_register_96", "BOOL" }, + { "input_bit_register_97", "BOOL" }, + { "input_bit_register_98", "BOOL" }, + { "input_bit_register_99", "BOOL" }, + { "input_bit_register_100", "BOOL" }, + { "input_bit_register_101", "BOOL" }, + { "input_bit_register_102", "BOOL" }, + { "input_bit_register_103", "BOOL" }, + { "input_bit_register_104", "BOOL" }, + { "input_bit_register_105", "BOOL" }, + { "input_bit_register_106", "BOOL" }, + { "input_bit_register_107", "BOOL" }, + { "input_bit_register_108", "BOOL" }, + { "input_bit_register_109", "BOOL" }, + { "input_bit_register_110", "BOOL" }, + { "input_bit_register_111", "BOOL" }, + { "input_bit_register_112", "BOOL" }, + { "input_bit_register_113", "BOOL" }, + { "input_bit_register_114", "BOOL" }, + { "input_bit_register_115", "BOOL" }, + { "input_bit_register_116", "BOOL" }, + { "input_bit_register_117", "BOOL" }, + { "input_bit_register_118", "BOOL" }, + { "input_bit_register_119", "BOOL" }, + { "input_bit_register_120", "BOOL" }, + { "input_bit_register_121", "BOOL" }, + { "input_bit_register_122", "BOOL" }, + { "input_bit_register_123", "BOOL" }, + { "input_bit_register_124", "BOOL" }, + { "input_bit_register_125", "BOOL" }, + { "input_bit_register_126", "BOOL" }, + { "input_bit_register_127", "BOOL" }, + { "input_int_register_0", "INT32" }, + { "input_int_register_1", "INT32" }, + { "input_int_register_2", "INT32" }, + { "input_int_register_3", "INT32" }, + { "input_int_register_4", "INT32" }, + { "input_int_register_5", "INT32" }, + { "input_int_register_6", "INT32" }, + { "input_int_register_7", "INT32" }, + { "input_int_register_8", "INT32" }, + { "input_int_register_9", "INT32" }, + { "input_int_register_10", "INT32" }, + { "input_int_register_11", "INT32" }, + { "input_int_register_12", "INT32" }, + { "input_int_register_13", "INT32" }, + { "input_int_register_14", "INT32" }, + { "input_int_register_15", "INT32" }, + { "input_int_register_16", "INT32" }, + { "input_int_register_17", "INT32" }, + { "input_int_register_18", "INT32" }, + { "input_int_register_19", "INT32" }, + { "input_int_register_20", "INT32" }, + { "input_int_register_21", "INT32" }, + { "input_int_register_22", "INT32" }, + { "input_int_register_23", "INT32" }, + { "input_int_register_24", "INT32" }, + { "input_int_register_25", "INT32" }, + { "input_int_register_26", "INT32" }, + { "input_int_register_27", "INT32" }, + { "input_int_register_28", "INT32" }, + { "input_int_register_29", "INT32" }, + { "input_int_register_30", "INT32" }, + { "input_int_register_31", "INT32" }, + { "input_int_register_32", "INT32" }, + { "input_int_register_33", "INT32" }, + { "input_int_register_34", "INT32" }, + { "input_int_register_35", "INT32" }, + { "input_int_register_36", "INT32" }, + { "input_int_register_37", "INT32" }, + { "input_int_register_38", "INT32" }, + { "input_int_register_39", "INT32" }, + { "input_int_register_40", "INT32" }, + { "input_int_register_41", "INT32" }, + { "input_int_register_42", "INT32" }, + { "input_int_register_43", "INT32" }, + { "input_int_register_44", "INT32" }, + { "input_int_register_45", "INT32" }, + { "input_int_register_46", "INT32" }, + { "input_int_register_47", "INT32" }, + { "input_double_register_0", "DOUBLE" }, + { "input_double_register_1", "DOUBLE" }, + { "input_double_register_2", "DOUBLE" }, + { "input_double_register_3", "DOUBLE" }, + { "input_double_register_4", "DOUBLE" }, + { "input_double_register_5", "DOUBLE" }, + { "input_double_register_6", "DOUBLE" }, + { "input_double_register_7", "DOUBLE" }, + { "input_double_register_8", "DOUBLE" }, + { "input_double_register_9", "DOUBLE" }, + { "input_double_register_10", "DOUBLE" }, + { "input_double_register_11", "DOUBLE" }, + { "input_double_register_12", "DOUBLE" }, + { "input_double_register_13", "DOUBLE" }, + { "input_double_register_14", "DOUBLE" }, + { "input_double_register_15", "DOUBLE" }, + { "input_double_register_16", "DOUBLE" }, + { "input_double_register_17", "DOUBLE" }, + { "input_double_register_18", "DOUBLE" }, + { "input_double_register_19", "DOUBLE" }, + { "input_double_register_20", "DOUBLE" }, + { "input_double_register_21", "DOUBLE" }, + { "input_double_register_22", "DOUBLE" }, + { "input_double_register_23", "DOUBLE" }, + { "input_double_register_24", "DOUBLE" }, + { "input_double_register_25", "DOUBLE" }, + { "input_double_register_26", "DOUBLE" }, + { "input_double_register_27", "DOUBLE" }, + { "input_double_register_28", "DOUBLE" }, + { "input_double_register_29", "DOUBLE" }, + { "input_double_register_30", "DOUBLE" }, + { "input_double_register_31", "DOUBLE" }, + { "input_double_register_32", "DOUBLE" }, + { "input_double_register_33", "DOUBLE" }, + { "input_double_register_34", "DOUBLE" }, + { "input_double_register_35", "DOUBLE" }, + { "input_double_register_36", "DOUBLE" }, + { "input_double_register_37", "DOUBLE" }, + { "input_double_register_38", "DOUBLE" }, + { "input_double_register_39", "DOUBLE" }, + { "input_double_register_40", "DOUBLE" }, + { "input_double_register_41", "DOUBLE" }, + { "input_double_register_42", "DOUBLE" }, + { "input_double_register_43", "DOUBLE" }, + { "input_double_register_44", "DOUBLE" }, + { "input_double_register_45", "DOUBLE" }, + { "input_double_register_46", "DOUBLE" }, + { "input_double_register_47", "DOUBLE" }, + + // OUTPUTS + { "timestamp", "DOUBLE" }, + { "target_q", "VECTOR6D" }, + { "target_qd", "VECTOR6D" }, + { "target_qdd", "VECTOR6D" }, + { "target_current", "VECTOR6D" }, + { "target_moment", "VECTOR6D" }, + { "actual_q", "VECTOR6D" }, + { "actual_qd", "VECTOR6D" }, + { "actual_current", "VECTOR6D" }, + { "actual_current_window", "VECTOR6D" }, + { "actual_current_as_torque", "VECTOR6D" }, + { "joint_control_output", "VECTOR6D" }, + { "actual_TCP_pose", "VECTOR6D" }, + { "actual_TCP_speed", "VECTOR6D" }, + { "actual_TCP_force", "VECTOR6D" }, + { "target_TCP_pose", "VECTOR6D" }, + { "target_TCP_speed", "VECTOR6D" }, + { "tcp_offset", "VECTOR6D" }, + { "actual_TCP_acceleration", "VECTOR6D" }, + { "target_TCP_acceleration", "VECTOR6D" }, + { "actual_digital_input_bits", "UINT64" }, + { "actual_configurable_digital_input_bits", "UINT64" }, + { "joint_temperatures", "VECTOR6D" }, + { "actual_execution_time", "DOUBLE" }, + { "target_execution_time", "DOUBLE" }, + { "robot_mode", "INT32" }, + { "joint_mode", "VECTOR6INT32" }, + { "safety_mode", "INT32" }, + { "safety_status", "INT32" }, + { "actual_tool_accelerometer", "VECTOR3D" }, + { "speed_scaling", "DOUBLE" }, + { "target_speed_fraction", "DOUBLE" }, + { "actual_momentum", "DOUBLE" }, + { "actual_main_voltage", "DOUBLE" }, + { "actual_robot_voltage", "DOUBLE" }, + { "actual_robot_current", "DOUBLE" }, + { "actual_joint_voltage", "VECTOR6D" }, + { "actual_digital_output_bits", "UINT64" }, + { "actual_configurable_digital_output_bits", "UINT64" }, + { "runtime_state", "UINT32" }, + { "elbow_position", "VECTOR3D" }, + { "elbow_velocity", "VECTOR3D" }, + { "robot_status_bits", "UINT32" }, + { "safety_status_bits", "UINT32" }, + { "analog_io_types", "UINT32" }, + { "standard_analog_input0", "DOUBLE" }, + { "standard_analog_input1", "DOUBLE" }, + { "standard_analog_output0", "DOUBLE" }, + { "standard_analog_output1", "DOUBLE" }, + { "io_current", "DOUBLE" }, + { "output_bit_registers0_to_31", "UINT32" }, + { "output_bit_registers32_to_63", "UINT32" }, + { "output_bit_register_64", "BOOL" }, + { "output_bit_register_65", "BOOL" }, + { "output_bit_register_66", "BOOL" }, + { "output_bit_register_67", "BOOL" }, + { "output_bit_register_68", "BOOL" }, + { "output_bit_register_69", "BOOL" }, + { "output_bit_register_70", "BOOL" }, + { "output_bit_register_71", "BOOL" }, + { "output_bit_register_72", "BOOL" }, + { "output_bit_register_73", "BOOL" }, + { "output_bit_register_74", "BOOL" }, + { "output_bit_register_75", "BOOL" }, + { "output_bit_register_76", "BOOL" }, + { "output_bit_register_77", "BOOL" }, + { "output_bit_register_78", "BOOL" }, + { "output_bit_register_79", "BOOL" }, + { "output_bit_register_80", "BOOL" }, + { "output_bit_register_81", "BOOL" }, + { "output_bit_register_82", "BOOL" }, + { "output_bit_register_83", "BOOL" }, + { "output_bit_register_84", "BOOL" }, + { "output_bit_register_85", "BOOL" }, + { "output_bit_register_86", "BOOL" }, + { "output_bit_register_87", "BOOL" }, + { "output_bit_register_88", "BOOL" }, + { "output_bit_register_89", "BOOL" }, + { "output_bit_register_90", "BOOL" }, + { "output_bit_register_91", "BOOL" }, + { "output_bit_register_92", "BOOL" }, + { "output_bit_register_93", "BOOL" }, + { "output_bit_register_94", "BOOL" }, + { "output_bit_register_95", "BOOL" }, + { "output_bit_register_96", "BOOL" }, + { "output_bit_register_97", "BOOL" }, + { "output_bit_register_98", "BOOL" }, + { "output_bit_register_99", "BOOL" }, + { "output_bit_register_100", "BOOL" }, + { "output_bit_register_101", "BOOL" }, + { "output_bit_register_102", "BOOL" }, + { "output_bit_register_103", "BOOL" }, + { "output_bit_register_104", "BOOL" }, + { "output_bit_register_105", "BOOL" }, + { "output_bit_register_106", "BOOL" }, + { "output_bit_register_107", "BOOL" }, + { "output_bit_register_108", "BOOL" }, + { "output_bit_register_109", "BOOL" }, + { "output_bit_register_110", "BOOL" }, + { "output_bit_register_111", "BOOL" }, + { "output_bit_register_112", "BOOL" }, + { "output_bit_register_113", "BOOL" }, + { "output_bit_register_114", "BOOL" }, + { "output_bit_register_115", "BOOL" }, + { "output_bit_register_116", "BOOL" }, + { "output_bit_register_117", "BOOL" }, + { "output_bit_register_118", "BOOL" }, + { "output_bit_register_119", "BOOL" }, + { "output_bit_register_120", "BOOL" }, + { "output_bit_register_121", "BOOL" }, + { "output_bit_register_122", "BOOL" }, + { "output_bit_register_123", "BOOL" }, + { "output_bit_register_124", "BOOL" }, + { "output_bit_register_125", "BOOL" }, + { "output_bit_register_126", "BOOL" }, + { "output_bit_register_127", "BOOL" }, + { "output_int_register_0", "INT32" }, + { "output_int_register_1", "INT32" }, + { "output_int_register_2", "INT32" }, + { "output_int_register_3", "INT32" }, + { "output_int_register_4", "INT32" }, + { "output_int_register_5", "INT32" }, + { "output_int_register_6", "INT32" }, + { "output_int_register_7", "INT32" }, + { "output_int_register_8", "INT32" }, + { "output_int_register_9", "INT32" }, + { "output_int_register_10", "INT32" }, + { "output_int_register_11", "INT32" }, + { "output_int_register_12", "INT32" }, + { "output_int_register_13", "INT32" }, + { "output_int_register_14", "INT32" }, + { "output_int_register_15", "INT32" }, + { "output_int_register_16", "INT32" }, + { "output_int_register_17", "INT32" }, + { "output_int_register_18", "INT32" }, + { "output_int_register_19", "INT32" }, + { "output_int_register_20", "INT32" }, + { "output_int_register_21", "INT32" }, + { "output_int_register_22", "INT32" }, + { "output_int_register_23", "INT32" }, + { "output_int_register_24", "INT32" }, + { "output_int_register_25", "INT32" }, + { "output_int_register_26", "INT32" }, + { "output_int_register_27", "INT32" }, + { "output_int_register_28", "INT32" }, + { "output_int_register_29", "INT32" }, + { "output_int_register_30", "INT32" }, + { "output_int_register_31", "INT32" }, + { "output_int_register_32", "INT32" }, + { "output_int_register_33", "INT32" }, + { "output_int_register_34", "INT32" }, + { "output_int_register_35", "INT32" }, + { "output_int_register_36", "INT32" }, + { "output_int_register_37", "INT32" }, + { "output_int_register_38", "INT32" }, + { "output_int_register_39", "INT32" }, + { "output_int_register_40", "INT32" }, + { "output_int_register_41", "INT32" }, + { "output_int_register_42", "INT32" }, + { "output_int_register_43", "INT32" }, + { "output_int_register_44", "INT32" }, + { "output_int_register_45", "INT32" }, + { "output_int_register_46", "INT32" }, + { "output_int_register_47", "INT32" }, + { "output_double_register_0", "DOUBLE" }, + { "output_double_register_1", "DOUBLE" }, + { "output_double_register_2", "DOUBLE" }, + { "output_double_register_3", "DOUBLE" }, + { "output_double_register_4", "DOUBLE" }, + { "output_double_register_5", "DOUBLE" }, + { "output_double_register_6", "DOUBLE" }, + { "output_double_register_7", "DOUBLE" }, + { "output_double_register_8", "DOUBLE" }, + { "output_double_register_9", "DOUBLE" }, + { "output_double_register_10", "DOUBLE" }, + { "output_double_register_11", "DOUBLE" }, + { "output_double_register_12", "DOUBLE" }, + { "output_double_register_13", "DOUBLE" }, + { "output_double_register_14", "DOUBLE" }, + { "output_double_register_15", "DOUBLE" }, + { "output_double_register_16", "DOUBLE" }, + { "output_double_register_17", "DOUBLE" }, + { "output_double_register_18", "DOUBLE" }, + { "output_double_register_19", "DOUBLE" }, + { "output_double_register_20", "DOUBLE" }, + { "output_double_register_21", "DOUBLE" }, + { "output_double_register_22", "DOUBLE" }, + { "output_double_register_23", "DOUBLE" }, + { "output_double_register_24", "DOUBLE" }, + { "output_double_register_25", "DOUBLE" }, + { "output_double_register_26", "DOUBLE" }, + { "output_double_register_27", "DOUBLE" }, + { "output_double_register_28", "DOUBLE" }, + { "output_double_register_29", "DOUBLE" }, + { "output_double_register_30", "DOUBLE" }, + { "output_double_register_31", "DOUBLE" }, + { "output_double_register_32", "DOUBLE" }, + { "output_double_register_33", "DOUBLE" }, + { "output_double_register_34", "DOUBLE" }, + { "output_double_register_35", "DOUBLE" }, + { "output_double_register_36", "DOUBLE" }, + { "output_double_register_37", "DOUBLE" }, + { "output_double_register_38", "DOUBLE" }, + { "output_double_register_39", "DOUBLE" }, + { "output_double_register_40", "DOUBLE" }, + { "output_double_register_41", "DOUBLE" }, + { "output_double_register_42", "DOUBLE" }, + { "output_double_register_43", "DOUBLE" }, + { "output_double_register_44", "DOUBLE" }, + { "output_double_register_45", "DOUBLE" }, + { "output_double_register_46", "DOUBLE" }, + { "output_double_register_47", "DOUBLE" }, + { "actual_robot_energy_consumed", "DOUBLE" }, + { "actual_robot_braking_energy_dissipated", "DOUBLE" }, + { "encoder0_raw", "INT32" }, + { "encoder1_raw", "INT32" }, + { "euromap67_input_bits", "UINT32" }, + { "euromap67_output_bits", "UINT32" }, + { "euromap67_24V_voltage", "DOUBLE" }, + { "euromap67_24V_current", "DOUBLE" }, + { "tool_mode", "UINT32" }, + { "tool_analog_input_types", "UINT32" }, + { "tool_analog_input0", "DOUBLE" }, + { "tool_analog_input1", "DOUBLE" }, + { "tool_output_voltage", "INT32" }, + { "tool_output_current", "DOUBLE" }, + { "tool_temperature", "DOUBLE" }, + { "tool_output_mode", "UINT8" }, + { "tool_digital_output0_mode", "UINT8" }, + { "tool_digital_output1_mode", "UINT8" }, + { "tcp_force_scalar", "DOUBLE" }, + { "joint_position_deviation_ratio", "DOUBLE" }, + { "collision_detection_ratio", "DOUBLE" }, + { "ft_raw_wrench", "VECTOR6D" }, + { "wrench_calc_from_currents", "VECTOR6D" }, + { "payload", "DOUBLE" }, + { "payload_cog", "VECTOR3D" }, + { "payload_inertia", "VECTOR6D" }, + { "script_control_line", "UINT32" }, + { "time_scale_source", "INT32" }, + { "target_gravity", "VECTOR3D" }, + { "target_base_acceleration", "VECTOR6D" }, + { "control_step", "UINT64" }, + { "target_base_wrench", "VECTOR6D" }, + + // NOT IN OFFICIAL DOCS + { "tool_digital_output_mask", "UINT8" }, + { "tool_digital_output", "UINT8" }, +}; +// clang-format on + +// Mimics a robot's answer to a recipe setup request: the data type of every requested field, or +// "NOT_FOUND" for fields the robot doesn't know. +std::vector variableTypesFor(const std::vector& recipe) +{ + std::vector types; + types.reserve(recipe.size()); + for (const auto& name : recipe) + { + const auto it = g_variable_types.find(name); + types.push_back(it == g_variable_types.end() ? "NOT_FOUND" : it->second); + } + return types; +} + +std::string joinStrings(const std::vector& strings, const std::string& delimiter = ",") +{ + std::string result; + for (const auto& string : strings) + { + if (!result.empty()) + { + result += delimiter; + } + result += string; + } + return result; +} + +bool allVariablesFound(const std::vector& types) +{ + return std::find(types.begin(), types.end(), "NOT_FOUND") == types.end(); +} + +// Unlike a client, the server side knows the data types up front, so it applies them itself right +// after allocating the package. +std::unique_ptr makeTypedDataPackage(const std::vector& recipe, + const std::vector& types) +{ + auto package = std::make_unique(recipe); + package->initEmpty(types); + return package; +} +} // namespace RTDEServer::RTDEServer(const int port) : server_(port) { @@ -52,7 +536,7 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, send_buffer, rtde_interface::PackageType::RTDE_REQUEST_PROTOCOL_VERSION, sizeof(uint8_t)); send_size += serializer.serialize(send_buffer + send_size, accepted); - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); break; } @@ -69,7 +553,7 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, send_size += serializer.serialize(send_buffer + send_size, version); // bugfix send_size += serializer.serialize(send_buffer + send_size, version); // build - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); break; } @@ -80,24 +564,29 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, std::string variable_names_str; bp.parseRemainder(variable_names_str); output_recipe_ = splitString(variable_names_str); + const std::vector variable_types = variableTypesFor(output_recipe_); + const std::string variable_types_str = joinStrings(variable_types); - output_data_package_ = std::make_unique(output_recipe_); - output_data_package_->initEmpty(); + { + std::lock_guard data_lock(output_data_mutex_); + output_data_package_.reset(); + if (allVariablesFound(variable_types)) + { + output_data_package_ = makeTypedDataPackage(output_recipe_, variable_types); + } + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; send_size += rtde_interface::PackageHeader::serializeHeader( send_buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS, - static_cast(variable_names_str.length() + sizeof(uint8_t))); + static_cast(variable_types_str.length() + sizeof(uint8_t))); uint8_t recipe_id = 1; send_size += serializer.serialize(send_buffer + send_size, recipe_id); - send_size += serializer.serialize(send_buffer + send_size, - variable_names_str); // We return the variable - // names list directly. For the initialization process, it - // is only important, that no field is "NOT_FOUND". + send_size += serializer.serialize(send_buffer + send_size, variable_types_str); - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); URCL_LOG_INFO("Output recipe set"); break; @@ -107,23 +596,26 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, std::string variable_names_str; bp.parseRemainder(variable_names_str); input_recipe_ = splitString(variable_names_str); + const std::vector variable_types = variableTypesFor(input_recipe_); + const std::string variable_types_str = joinStrings(variable_types); - input_data_package_ = std::make_unique(input_recipe_); + input_data_package_.reset(); + if (allVariablesFound(variable_types)) + { + input_data_package_ = makeTypedDataPackage(input_recipe_, variable_types); + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; send_size += rtde_interface::PackageHeader::serializeHeader( send_buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS, - static_cast(variable_names_str.length() + sizeof(uint8_t))); + static_cast(variable_types_str.length() + sizeof(uint8_t))); uint8_t recipe_id = 1; send_size += serializer.serialize(send_buffer + send_size, recipe_id); - send_size += serializer.serialize(send_buffer + send_size, - variable_names_str); // We return the variable - // names list directly. For the initialization process, it - // is only important, that no field is "NOT_FOUND". + send_size += serializer.serialize(send_buffer + send_size, variable_types_str); - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); URCL_LOG_INFO("Input recipe set with %zu variables.", input_recipe_.size()); @@ -139,7 +631,7 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, bool accepted = true; send_size += serializer.serialize(send_buffer + send_size, accepted); - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); startSendingDataPackages(); break; @@ -154,7 +646,7 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, bool accepted = true; send_size += serializer.serialize(send_buffer + send_size, accepted); - size_t written; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); stopSendingDataPackages(); break; @@ -212,7 +704,7 @@ void RTDEServer::sendDataLoop() output_data_package_->setData("timestamp", timestamp); uint8_t buffer[65536]; size_t size = output_data_package_->serializePackage(buffer); - size_t written; + size_t written = 0; server_.write(client_socket_, buffer, size, written); } std::this_thread::sleep_for(std::chrono::duration(1.0 / output_frequency_)); @@ -227,7 +719,10 @@ void RTDEServer::actOnInput() double speed_slider_fraction = 0.0; input_data_package_->getData("speed_slider_fraction", speed_slider_fraction); std::lock_guard data_lock(output_data_mutex_); - output_data_package_->setData("target_speed_fraction", speed_slider_fraction); + if (output_data_package_ != nullptr) + { + output_data_package_->setData("target_speed_fraction", speed_slider_fraction); + } } } diff --git a/tests/resources/generate_rtde_outputs.py b/tests/resources/generate_rtde_outputs.py index cf4f527b9..7d9382299 100644 --- a/tests/resources/generate_rtde_outputs.py +++ b/tests/resources/generate_rtde_outputs.py @@ -33,7 +33,10 @@ import re URCL_PATH = pathlib.Path(__file__).parent.parent.parent.resolve() -PKG_PATH = [i for i in pathlib.Path(URCL_PATH.as_posix()).glob("**/data_package.cpp")] +# The client library takes the data types of the RTDE fields from the robot's answer to the recipe +# setup, so the list of all known fields only exists in the test double that has to emulate that +# answer. +PKG_PATH = [i for i in pathlib.Path(URCL_PATH.as_posix()).glob("**/fake_rtde_server.cpp")] assert len(PKG_PATH) == 1 diff --git a/tests/rtde_test_helpers.h b/tests/rtde_test_helpers.h new file mode 100644 index 000000000..f85be8126 --- /dev/null +++ b/tests/rtde_test_helpers.h @@ -0,0 +1,88 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#pragma once + +// Applying the data types from an RTDE setup acknowledgement is the library's own job, so the data +// package, the parser and the writer all keep those entry points out of their public interface. +// Tests, and the fake server standing in for the robot, reach them through these subclasses. + +#include +#include + +#include +#include +#include + +namespace urcl +{ +namespace test +{ +class TestableDataPackage : public rtde_interface::DataPackage +{ +public: + using rtde_interface::DataPackage::DataPackage; + using rtde_interface::DataPackage::initEmpty; +}; + +class TestableRTDEParser : public rtde_interface::RTDEParser +{ +public: + explicit TestableRTDEParser(const std::vector& recipe) : rtde_interface::RTDEParser(recipe) + { + } + + using rtde_interface::RTDEParser::setRecipeTypes; +}; + +class TestableRTDEWriter : public rtde_interface::RTDEWriter +{ +public: + TestableRTDEWriter(comm::URStream* stream, const std::vector& recipe) + : rtde_interface::RTDEWriter(stream, recipe) + { + } + + using rtde_interface::RTDEWriter::setRecipeTypes; +}; + +/*! + * \brief Builds a data package the way the library does: allocate from the recipe, then apply the + * data types the robot reported for it. + */ +inline TestableDataPackage typedPackage(const std::vector& recipe, const std::vector& types, + const uint16_t protocol_version = 2) +{ + TestableDataPackage package(recipe, protocol_version); + package.initEmpty(types); + return package; +} +} // namespace test +} // namespace urcl diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index c7f692234..b24de2cc7 100644 --- a/tests/test_pipeline.cpp +++ b/tests/test_pipeline.cpp @@ -40,6 +40,8 @@ #include #include +#include "rtde_test_helpers.h" + using namespace urcl; class PipelineTest : public ::testing::Test @@ -53,7 +55,8 @@ class PipelineTest : public ::testing::Test // Setup pipeline stream_.reset(new comm::URStream("127.0.0.1", 60002)); std::vector recipe = { "timestamp" }; - parser_.reset(new rtde_interface::RTDEParser(recipe)); + parser_.reset(new test::TestableRTDEParser(recipe)); + parser_->setRecipeTypes({ "DOUBLE" }); parser_->setProtocolVersion(2); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); @@ -72,7 +75,7 @@ class PipelineTest : public ::testing::Test std::unique_ptr server_; std::unique_ptr> stream_; - std::unique_ptr parser_; + std::unique_ptr parser_; std::unique_ptr> producer_; std::unique_ptr> pipeline_; comm::INotifier notifier_; diff --git a/tests/test_producer.cpp b/tests/test_producer.cpp index 32069be32..0770ab3f4 100644 --- a/tests/test_producer.cpp +++ b/tests/test_producer.cpp @@ -38,6 +38,8 @@ #include #include +#include "rtde_test_helpers.h" + using namespace urcl; class ProducerTest : public ::testing::Test @@ -62,7 +64,8 @@ TEST_F(ProducerTest, get_data_package) { comm::URStream stream("127.0.0.1", 60002); std::vector recipe = { "timestamp" }; - rtde_interface::RTDEParser parser(recipe); + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE" }); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); @@ -97,7 +100,8 @@ TEST_F(ProducerTest, connect_non_connected_robot) { comm::URStream stream("127.0.0.1", 12321); std::vector recipe = { "timestamp" }; - rtde_interface::RTDEParser parser(recipe); + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE" }); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp new file mode 100644 index 000000000..ab24bbfbf --- /dev/null +++ b/tests/test_rtde_allocations.cpp @@ -0,0 +1,294 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +// All memory an RTDE connection needs for exchanging data is allocated while the recipes are set +// up, using the data types the robot reports in its acknowledgements. These tests pin that down by +// counting allocations around a few hundred receive cycles against the fake RTDE server. + +#include + +#include +#include + +#include +#include + +#include "fake_rtde_server.h" + +using namespace urcl; + +namespace +{ +// Counting is per-thread: the fake server and, in the background-read case, the client's read +// thread run in the same process, and their allocations are none of this test's business. +thread_local std::size_t g_allocation_count = 0; +thread_local bool g_count_allocations = false; + +constexpr int g_FAKE_RTDE_PORT = 60005; +constexpr double g_RTDE_FREQUENCY = 125.0; +constexpr int g_WARMUP_CYCLES = 10; +constexpr int g_MEASURED_CYCLES = 50; + +/*! + * \brief Counts the allocations made on the current thread for as long as it is alive. + */ +class AllocationCounter +{ +public: + AllocationCounter() + { + g_allocation_count = 0; + g_count_allocations = true; + } + + ~AllocationCounter() + { + g_count_allocations = false; + } + + std::size_t count() const + { + return g_allocation_count; + } +}; +} // namespace + +// These replace the global allocation functions, so pairing malloc with free is correct here even +// though GCC cannot see across the replacement and flags it. +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wmismatched-new-delete" +#endif + +void* operator new(std::size_t size) +{ + if (g_count_allocations) + { + ++g_allocation_count; + } + void* memory = std::malloc(size == 0 ? 1 : size); + if (memory == nullptr) + { + throw std::bad_alloc(); + } + return memory; +} + +void* operator new[](std::size_t size) +{ + return operator new(size); +} + +void operator delete(void* memory) noexcept +{ + std::free(memory); +} + +void operator delete[](void* memory) noexcept +{ + std::free(memory); +} + +void operator delete(void* memory, std::size_t) noexcept +{ + std::free(memory); +} + +void operator delete[](void* memory, std::size_t) noexcept +{ + std::free(memory); +} + +#if defined(__GNUC__) && !defined(__clang__) +# pragma GCC diagnostic pop +#endif + +// Guards the tests below: if the counter stopped seeing allocations, they would pass vacuously. +TEST(AllocationCounterTest, counts_allocations) +{ + std::size_t allocations = 0; + std::vector values; + { + AllocationCounter counter; + values.resize(1024); + allocations = counter.count(); + } + EXPECT_GT(allocations, 0); +} + +class RTDEAllocationTest : public ::testing::Test +{ +protected: + void SetUp() override + { + server_ = std::make_unique(g_FAKE_RTDE_PORT); + // Skip the client's bootup check, which would otherwise read data for a second + server_->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + + client_ = std::make_unique("localhost", notifier_, output_recipe_, input_recipe_, + g_RTDE_FREQUENCY, false, g_FAKE_RTDE_PORT); + ASSERT_TRUE(client_->init()); + } + + void TearDown() override + { + client_.reset(); + server_.reset(); + } + + // A recipe covering all data types that appear on the receiving side + std::vector output_recipe_{ "timestamp", "actual_q", + "actual_TCP_force", "runtime_state", + "robot_status_bits", "actual_digital_input_bits", + "joint_mode", "payload_cog", + "tool_mode", "output_int_register_24" }; + std::vector input_recipe_{ "speed_slider_mask", "speed_slider_fraction" }; + + comm::INotifier notifier_; + std::unique_ptr server_; + std::unique_ptr client_; +}; + +TEST_F(RTDEAllocationTest, blocking_receive_does_not_allocate) +{ + ASSERT_TRUE(client_->start(false)); + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + + // The first cycles let every buffer along the way reach its final capacity + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + } + + // Deliberately no gtest macros inside the measured section, as those allocate themselves + int received = 0; + bool all_data_read = true; + double timestamp = 0.0; + vector6d_t actual_q{}; + std::bitset<18> digital_input_bits; + std::size_t allocations = 0; + { + AllocationCounter counter; + for (int i = 0; i < g_MEASURED_CYCLES; ++i) + { + if (!client_->getDataPackageBlocking(data_pkg)) + { + continue; + } + ++received; + all_data_read &= data_pkg->getData("timestamp", timestamp); + all_data_read &= data_pkg->getData("actual_q", actual_q); + all_data_read &= data_pkg->getData("actual_digital_input_bits", digital_input_bits); + } + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(all_data_read); + EXPECT_GT(received, 0); + EXPECT_GT(timestamp, 0.0); +} + +TEST_F(RTDEAllocationTest, background_receive_does_not_allocate) +{ + ASSERT_TRUE(client_->start(true)); + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + const std::chrono::milliseconds read_timeout{ 100 }; + + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + } + + int received = 0; + bool all_data_read = true; + double timestamp = 0.0; + std::size_t allocations = 0; + { + AllocationCounter counter; + for (int i = 0; i < g_MEASURED_CYCLES; ++i) + { + if (!client_->getDataPackage(data_pkg, read_timeout)) + { + continue; + } + ++received; + all_data_read &= data_pkg.getData("timestamp", timestamp); + } + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(all_data_read); + EXPECT_GT(received, 0); + EXPECT_GT(timestamp, 0.0); + + client_->pause(); +} + +TEST_F(RTDEAllocationTest, sending_input_data_does_not_allocate) +{ + ASSERT_TRUE(client_->start(true)); + rtde_interface::DataPackage input_pkg(client_->getInputRecipe()); + ASSERT_TRUE(input_pkg.setData("speed_slider_mask", static_cast(1))); + + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getWriter().sendSpeedSlider(0.5)); + } + + bool all_sent = true; + std::size_t allocations = 0; + { + AllocationCounter counter; + for (int i = 0; i < g_MEASURED_CYCLES; ++i) + { + all_sent &= input_pkg.setData("speed_slider_fraction", 0.5); + all_sent &= client_->getWriter().sendPackage(input_pkg); + } + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(all_sent); + + client_->pause(); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + // Logging allocates, and a log statement inside a measured section would rightfully be counted. + // Keep the routine chatter out of the way so the tests measure the data exchange itself. + setLogLevel(LogLevel::ERROR); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index 1fcff8b8f..f9938527d 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -290,17 +290,17 @@ TEST_F(RTDEClientTest, output_recipe_file) } } +// The robot is the authority on which fields exist and what type they have, so a typo in a recipe +// is reported when the robot rejects it during init(), not already at construction time. 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_THAT( - [&]() { - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); - }, - testing::ThrowsMessage(testing::HasSubstr("i_do_not_exist"))); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); + + EXPECT_THAT([&]() { client_->init(); }, testing::ThrowsMessage(testing::HasSubstr("i_do_not_" + "exist"))); } TEST_F(RTDEClientTest, output_recipe_with_invalid_key) @@ -308,12 +308,10 @@ TEST_F(RTDEClientTest, output_recipe_with_invalid_key) std::vector actual_output_recipe = resources_output_recipe_; actual_output_recipe.push_back("i_do_not_exist"); - EXPECT_THAT( - [&]() { - client_.reset( - new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, actual_output_recipe, resources_input_recipe_)); - }, - testing::ThrowsMessage(testing::HasSubstr("i_do_not_exist"))); + client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, actual_output_recipe, resources_input_recipe_)); + + EXPECT_THAT([&]() { client_->init(); }, testing::ThrowsMessage(testing::HasSubstr("i_do_not_" + "exist"))); TestableRTDEClient client(g_ROBOT_IP, notifier_, resources_output_recipe_, resources_input_recipe_); client.injectOutputRecipe(actual_output_recipe); @@ -445,7 +443,7 @@ 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 }; - auto data_pkg = rtde_interface::DataPackage(client_->getOutputRecipe()); + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); if (!client_->getDataPackage(data_pkg, read_timeout)) { std::cout << "Failed to get data package from robot" << std::endl; @@ -471,7 +469,7 @@ TEST_F(RTDEClientTest, destroy_client_after_server_stops_sending) URCL_LOG_INFO("Receiving data package from fake server to verify that connection is working."); const std::chrono::milliseconds read_timeout{ 100 }; - auto data_pkg = rtde_interface::DataPackage(client_->getOutputRecipe()); + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); double timestamp = 0.0; @@ -771,30 +769,35 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) { client_->init(); - std::vector incorrect_output_recipe = client_->getOutputRecipe(); + const VersionInformation robot_version = client_->getVersion(); + const std::vector output_recipe = client_->getOutputRecipe(); + std::vector incorrect_output_recipe = output_recipe; incorrect_output_recipe.push_back("unknown_rtde_variable"); + // Only one client can hold the RTDE input recipe at a time, so disconnect before setting up the + // next one. + client_.reset(); + // If unknown variables are not ignored, initialization should fail - EXPECT_THROW(client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, - resources_input_recipe_, 0.0, false)), - RTDEInvalidKeyException); + auto client = std::make_unique(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, false); + EXPECT_THROW(client->init(), RTDEInvalidKeyException); // 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)) + if ((robot_version.major == 5 && robot_version.minor < 23) || (robot_version.major == 10 && robot_version.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 rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, output_recipe, resources_input_recipe_, 0.0, true)); - EXPECT_TRUE(client_->init()); + std::vector newer_output_recipe = output_recipe; + newer_output_recipe.push_back("actual_robot_energy_consumed"); // That has been added in 5.23.0 / 10.11.0 + client = std::make_unique(g_ROBOT_IP, notifier_, newer_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 rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, incorrect_output_recipe, - resources_input_recipe_, 0.0, true)), - RTDEInvalidKeyException); + client = std::make_unique(g_ROBOT_IP, notifier_, incorrect_output_recipe, + resources_input_recipe_, 0.0, true); + EXPECT_THROW(client->init(), RTDEInvalidKeyException); } TEST_F(RTDEClientTest, empty_input_recipe) diff --git a/tests/test_rtde_client_fake_server.cpp b/tests/test_rtde_client_fake_server.cpp new file mode 100644 index 000000000..656aa7284 --- /dev/null +++ b/tests/test_rtde_client_fake_server.cpp @@ -0,0 +1,378 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +// Covers the public surface of RTDEClient against the fake RTDE server. The tests in +// test_rtde_client.cpp are more thorough but need a reachable robot, so they only run when +// INTEGRATION_TESTS is enabled; these run everywhere. + +#include + +#include +#include +#include + +#include "fake_rtde_server.h" + +using namespace urcl; + +namespace +{ +constexpr int g_FAKE_RTDE_PORT = 60006; +constexpr double g_RTDE_FREQUENCY = 125.0; +// The fake server answers the version query with 10.10.10.10, so the client takes the e-Series limit +constexpr double g_MAX_FREQUENCY = 500.0; +constexpr std::chrono::milliseconds g_READ_TIMEOUT{ 200 }; + +const std::vector g_OUTPUT_RECIPE{ "timestamp", "actual_q", "target_speed_fraction", "runtime_state" }; +const std::vector g_INPUT_RECIPE{ "speed_slider_mask", "speed_slider_fraction" }; +} // namespace + +class RTDEClientFakeServerTest : public ::testing::Test +{ +protected: + void SetUp() override + { + server_ = std::make_unique(g_FAKE_RTDE_PORT); + // Skip the client's bootup check, which would otherwise read data for a second + server_->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + client_ = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, g_RTDE_FREQUENCY); + } + + void TearDown() override + { + client_.reset(); + server_.reset(); + } + + std::unique_ptr makeClient(const std::vector& output_recipe, + const std::vector& input_recipe, + double target_frequency, + bool ignore_unavailable_outputs = false) + { + return std::make_unique("localhost", notifier_, output_recipe, input_recipe, + target_frequency, ignore_unavailable_outputs, g_FAKE_RTDE_PORT); + } + + comm::INotifier notifier_; + std::unique_ptr server_; + std::unique_ptr client_; +}; + +// The address is the one the socket resolved to, so it only exists once the socket is connected. +TEST_F(RTDEClientFakeServerTest, get_ip) +{ + EXPECT_TRUE(client_->getIP().empty()); + + ASSERT_TRUE(client_->init()); + + EXPECT_EQ(client_->getIP(), "127.0.0.1"); +} + +TEST_F(RTDEClientFakeServerTest, recipes_are_reported_as_given) +{ + EXPECT_EQ(client_->getOutputRecipe(), g_OUTPUT_RECIPE); + EXPECT_EQ(client_->getInputRecipe(), g_INPUT_RECIPE); +} + +// The client needs the timestamp to tell whether the robot has finished booting, so it adds the +// field to recipes that don't ask for it. +TEST_F(RTDEClientFakeServerTest, timestamp_is_added_to_the_output_recipe) +{ + auto client = makeClient({ "actual_q" }, g_INPUT_RECIPE, g_RTDE_FREQUENCY); + + const std::vector expected_recipe{ "actual_q", "timestamp" }; + EXPECT_EQ(client->getOutputRecipe(), expected_recipe); +} + +TEST_F(RTDEClientFakeServerTest, read_recipe_from_file) +{ + const std::vector recipe = rtde_interface::RTDEClient::readRecipe("resources/rtde_input_recipe.txt"); + + EXPECT_FALSE(recipe.empty()); + EXPECT_EQ(recipe.front(), "speed_slider_mask"); + + EXPECT_THROW(rtde_interface::RTDEClient::readRecipe("resources/there_is_no_such_recipe.txt"), UrException); +} + +TEST_F(RTDEClientFakeServerTest, client_state_follows_the_communication) +{ + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + + ASSERT_TRUE(client_->init()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); + + ASSERT_TRUE(client_->start()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::RUNNING); + + ASSERT_TRUE(client_->pause()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::PAUSED); +} + +TEST_F(RTDEClientFakeServerTest, init_is_idempotent) +{ + ASSERT_TRUE(client_->init()); + EXPECT_TRUE(client_->init()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); +} + +TEST_F(RTDEClientFakeServerTest, start_and_pause_out_of_order) +{ + EXPECT_FALSE(client_->start()); + EXPECT_FALSE(client_->pause()); + + ASSERT_TRUE(client_->init()); + EXPECT_FALSE(client_->pause()); + + ASSERT_TRUE(client_->start()); + ASSERT_TRUE(client_->pause()); + // A paused client can be started again + EXPECT_TRUE(client_->start()); + EXPECT_TRUE(client_->pause()); +} + +TEST_F(RTDEClientFakeServerTest, version_is_taken_from_the_robot) +{ + ASSERT_TRUE(client_->init()); + + const VersionInformation version = client_->getVersion(); + EXPECT_EQ(version.major, 10); + EXPECT_EQ(version.minor, 10); +} + +TEST_F(RTDEClientFakeServerTest, target_frequency_defaults_to_the_maximum) +{ + auto client = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, 0.0); + EXPECT_EQ(client->getTargetFrequency(), 0.0); + + ASSERT_TRUE(client->init()); + + EXPECT_EQ(client->getMaxFrequency(), g_MAX_FREQUENCY); + EXPECT_EQ(client->getTargetFrequency(), client->getMaxFrequency()); +} + +TEST_F(RTDEClientFakeServerTest, configured_target_frequency_is_kept) +{ + ASSERT_TRUE(client_->init()); + + EXPECT_EQ(client_->getMaxFrequency(), g_MAX_FREQUENCY); + EXPECT_EQ(client_->getTargetFrequency(), g_RTDE_FREQUENCY); +} + +TEST_F(RTDEClientFakeServerTest, target_frequency_outside_the_robots_range_throws) +{ + auto too_low = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, -1.0); + EXPECT_THROW(too_low->init(), UrException); + + auto too_high = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, g_MAX_FREQUENCY + 1.0); + EXPECT_THROW(too_high->init(), UrException); +} + +TEST_F(RTDEClientFakeServerTest, receive_with_background_read) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + double timestamp = 0.0; + ASSERT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Blocking reads would compete with the background thread for the same packages + auto blocking_pkg = std::make_unique(client_->getOutputRecipe()); + EXPECT_FALSE(client_->getDataPackageBlocking(blocking_pkg)); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, receive_with_background_read_into_a_unique_ptr) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + double timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, receive_without_background_read) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(false)); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + + double timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + // Without the background thread there is nothing for the non-blocking overload to read from + rtde_interface::DataPackage other_pkg(client_->getOutputRecipe()); + EXPECT_FALSE(client_->getDataPackage(other_pkg, g_READ_TIMEOUT)); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, background_read_can_be_stopped_and_started) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + client_->stopBackgroundRead(); + EXPECT_FALSE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + client_->startBackgroundRead(); + EXPECT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, background_read_before_init_is_refused) +{ + client_->startBackgroundRead(); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + EXPECT_FALSE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); +} + +TEST_F(RTDEClientFakeServerTest, deprecated_get_data_package_returns_a_usable_package) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + URCL_SILENCE_DEPRECATED_BEGIN + std::unique_ptr data_pkg = client_->getDataPackage(g_READ_TIMEOUT); + URCL_SILENCE_DEPRECATED_END + + ASSERT_NE(data_pkg, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + client_->pause(); +} + +// The fake server echoes the speed slider fraction back as target_speed_fraction, which is enough to +// see a value travel all the way through the writer and back. +TEST_F(RTDEClientFakeServerTest, write_and_read_back_input_data) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage input_pkg(client_->getInputRecipe()); + ASSERT_TRUE(input_pkg.setData("speed_slider_mask", 1)); + ASSERT_TRUE(input_pkg.setData("speed_slider_fraction", 0.25)); + ASSERT_TRUE(client_->getWriter().sendPackage(input_pkg)); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + double target_speed_fraction = 0.0; + for (int i = 0; i < 20 && target_speed_fraction == 0.0; ++i) + { + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + ASSERT_TRUE(data_pkg.getData("target_speed_fraction", target_speed_fraction)); + } + EXPECT_DOUBLE_EQ(target_speed_fraction, 0.25); + + client_->pause(); +} + +// The robot is the authority on which fields exist, so a typo in a recipe is caught from the +// acknowledgement rather than from a table inside the library. +TEST_F(RTDEClientFakeServerTest, unknown_output_field_throws) +{ + auto client = makeClient({ "timestamp", "not_a_field_the_robot_knows" }, g_INPUT_RECIPE, g_RTDE_FREQUENCY); + + EXPECT_THROW(client->init(), RTDEInvalidKeyException); +} + +TEST_F(RTDEClientFakeServerTest, unknown_output_field_can_be_ignored) +{ + auto client = + makeClient({ "timestamp", "actual_q", "not_a_field_the_robot_knows" }, g_INPUT_RECIPE, g_RTDE_FREQUENCY, true); + + ASSERT_TRUE(client->init()); + + const std::vector expected_recipe{ "timestamp", "actual_q" }; + EXPECT_EQ(client->getOutputRecipe(), expected_recipe); + + ASSERT_TRUE(client->start(true)); + rtde_interface::DataPackage data_pkg(client->getOutputRecipe()); + EXPECT_TRUE(client->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + client->pause(); +} + +TEST_F(RTDEClientFakeServerTest, unknown_input_field_throws) +{ + auto client = makeClient(g_OUTPUT_RECIPE, { "not_a_field_the_robot_knows" }, g_RTDE_FREQUENCY); + + EXPECT_THROW(client->init(), RTDEInvalidKeyException); +} + +// The other constructor takes recipe files, and a missing or empty output recipe is rejected right +// away rather than at handshake time. +TEST_F(RTDEClientFakeServerTest, recipe_files) +{ + EXPECT_NO_THROW(rtde_interface::RTDEClient("localhost", notifier_, "resources/rtde_output_recipe.txt", + "resources/rtde_input_recipe.txt", g_RTDE_FREQUENCY, false, + g_FAKE_RTDE_PORT)); + + EXPECT_THROW(rtde_interface::RTDEClient("localhost", notifier_, "", "resources/rtde_input_recipe.txt", + g_RTDE_FREQUENCY, false, g_FAKE_RTDE_PORT), + UrException); + + EXPECT_THROW(rtde_interface::RTDEClient("localhost", notifier_, "resources/empty.txt", + "resources/rtde_input_recipe.txt", g_RTDE_FREQUENCY, false, g_FAKE_RTDE_PORT), + UrException); + + EXPECT_THROW(rtde_interface::RTDEClient("localhost", notifier_, "resources/rtde_output_recipe.txt", + "/i/do/not/exist/urclrtdetest.txt", g_RTDE_FREQUENCY, false, + g_FAKE_RTDE_PORT), + UrException); +} + +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + + setLogLevel(LogLevel::ERROR); + + return RUN_ALL_TESTS(); +} diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index f641fcf21..20a184ef7 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -28,14 +28,19 @@ #include +#include #include +#include "rtde_test_helpers.h" + using namespace urcl; +using urcl::test::typedPackage; TEST(rtde_data_package, serialize_pkg) { std::vector recipe{ "speed_slider_mask" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "UINT32" }; + auto package = typedPackage(recipe, types); uint32_t value = 1; package.setData("speed_slider_mask", value); @@ -57,7 +62,8 @@ TEST(rtde_data_package, serialize_pkg) TEST(rtde_data_package, parse_pkg_protocolv2) { std::vector recipe{ "timestamp", "actual_q" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types); 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, @@ -90,7 +96,8 @@ TEST(rtde_data_package, parse_pkg_protocolv2) TEST(rtde_data_package, parse_pkg_protocolv1) { std::vector recipe{ "timestamp", "actual_q" }; - rtde_interface::DataPackage package(recipe, 1); + std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types, 1); 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, @@ -122,7 +129,8 @@ TEST(rtde_data_package, parse_pkg_protocolv1) TEST(rtde_data_package, get_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types); uint32_t speed_slider_mask; EXPECT_FALSE(package.getData("speed_slider_mask", speed_slider_mask)); @@ -131,7 +139,8 @@ TEST(rtde_data_package, get_data_not_part_of_recipe) TEST(rtde_data_package, set_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types); uint32_t speed_slider_mask = 1; EXPECT_FALSE(package.setData("speed_slider_mask", speed_slider_mask)); @@ -140,7 +149,8 @@ TEST(rtde_data_package, set_data_not_part_of_recipe) TEST(rtde_data_package, parse_and_get_bitset_data) { std::vector recipe{ "robot_status_bits" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "UINT32" }; + auto package = typedPackage(recipe, types); 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)); @@ -157,7 +167,8 @@ TEST(rtde_data_package, parse_and_get_bitset_data) TEST(rtde_data_package, parse_incorrect_data_size) { std::vector recipe{ "timestamp", "actual_q" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types); // 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 }; @@ -171,7 +182,8 @@ 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); + std::vector types{ "UINT32", "DOUBLE", "VECTOR6D", "UINT8", "UINT64" }; + auto package = typedPackage(recipe, types); 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 })); @@ -189,6 +201,238 @@ TEST(rtde_data_package, data_package_to_string) EXPECT_EQ(expected_str, pkg_str); } +TEST(rtde_data_package, every_rtde_data_type_can_be_applied) +{ + // The set of type names the robot may report is the only type knowledge the library still + // carries, so check that each one maps onto the C++ type an application expects to read. + std::vector recipe{ "f_bool", "f_uint8", "f_uint32", "f_uint64", "f_int32", + "f_double", "f_vector3d", "f_vector6d", "f_v6int32", "f_v6uint32" }; + std::vector types{ "BOOL", "UINT8", "UINT32", "UINT64", "INT32", + "DOUBLE", "VECTOR3D", "VECTOR6D", "VECTOR6INT32", "VECTOR6UINT32" }; + auto package = typedPackage(recipe, types); + + // Every field reports back the type the robot named for it + for (size_t i = 0; i < recipe.size(); ++i) + { + const auto type = package.getDataType(recipe[i]); + ASSERT_TRUE(type.has_value()) << "for field " << recipe[i]; + EXPECT_EQ(rtde_interface::toString(*type), types[i]) << "for field " << recipe[i]; + } + + bool bool_value; + uint8_t uint8_value; + uint32_t uint32_value; + uint64_t uint64_value; + int32_t int32_value; + double double_value; + vector3d_t vector3d_value; + vector6d_t vector6d_value; + vector6int32_t v6int32_value; + vector6uint32_t v6uint32_value; + + EXPECT_TRUE(package.getData("f_bool", bool_value)); + EXPECT_TRUE(package.getData("f_uint8", uint8_value)); + EXPECT_TRUE(package.getData("f_uint32", uint32_value)); + EXPECT_TRUE(package.getData("f_uint64", uint64_value)); + EXPECT_TRUE(package.getData("f_int32", int32_value)); + EXPECT_TRUE(package.getData("f_double", double_value)); + EXPECT_TRUE(package.getData("f_vector3d", vector3d_value)); + EXPECT_TRUE(package.getData("f_vector6d", vector6d_value)); + EXPECT_TRUE(package.getData("f_v6int32", v6int32_value)); + EXPECT_TRUE(package.getData("f_v6uint32", v6uint32_value)); + + // Each field holds exactly the type the robot named for it, and nothing else + EXPECT_FALSE(package.getData("f_bool", double_value)); + EXPECT_FALSE(package.getData("f_uint8", uint32_value)); + EXPECT_FALSE(package.getData("f_uint32", int32_value)); + EXPECT_FALSE(package.getData("f_uint64", uint32_value)); + EXPECT_FALSE(package.getData("f_int32", uint32_value)); + EXPECT_FALSE(package.getData("f_double", uint64_value)); + EXPECT_FALSE(package.getData("f_vector3d", vector6d_value)); + EXPECT_FALSE(package.getData("f_vector6d", vector3d_value)); + EXPECT_FALSE(package.getData("f_v6int32", v6uint32_value)); + EXPECT_FALSE(package.getData("f_v6uint32", v6int32_value)); +} + +TEST(rtde_data_package, unknown_data_types_are_rejected) +{ + std::vector recipe{ "timestamp" }; + test::TestableDataPackage package(recipe); + + // A field the robot doesn't know about is reported as NOT_FOUND, one that is already used by + // another recipe as IN_USE. Neither is a data type. + EXPECT_THROW(package.initEmpty({ "NOT_FOUND" }), UrException); + EXPECT_THROW(package.initEmpty({ "IN_USE" }), UrException); + EXPECT_THROW(package.initEmpty({ "double" }), UrException); +} + +TEST(rtde_data_package, type_count_has_to_match_recipe) +{ + std::vector recipe{ "timestamp", "actual_q" }; + test::TestableDataPackage package(recipe); + EXPECT_THROW(package.initEmpty({ "DOUBLE" }), UrException); + EXPECT_THROW(package.initEmpty({ "DOUBLE", "VECTOR6D", "DOUBLE" }), UrException); +} + +TEST(rtde_data_package, untyped_package_cannot_be_parsed_or_serialized) +{ + std::vector recipe{ "timestamp", "actual_q" }; + rtde_interface::DataPackage package(recipe); + + EXPECT_FALSE(package.getDataType("timestamp").has_value()); + + double timestamp = 0.0; + EXPECT_FALSE(package.getData("timestamp", timestamp)); + + uint8_t buffer[4096]; + EXPECT_EQ(package.serializePackage(buffer), 0); + + uint8_t data_package[] = { 0x01, 0x40, 0xd0, 0x75, 0x8c, 0x49, 0xba, 0x5e, 0x35 }; + comm::BinParser bp(data_package, sizeof(data_package)); + EXPECT_FALSE(package.parseWith(bp)); +} + +TEST(rtde_data_package, untyped_package_gets_typed_by_assignment) +{ + std::vector recipe{ "timestamp", "actual_q" }; + rtde_interface::DataPackage untyped_package(recipe); + auto typed_package = typedPackage(recipe, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(typed_package.setData("timestamp", 42.0)); + + untyped_package = typed_package; + + EXPECT_EQ(untyped_package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + double timestamp = 0.0; + ASSERT_TRUE(untyped_package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +// Applying the robot's answer to a package an application is already holding is what lets that +// application allocate the package wherever it likes, including before the connection exists. +TEST(rtde_data_package, applying_types_does_not_reallocate) +{ + std::vector recipe{ "timestamp", "actual_q" }; + test::TestableDataPackage package(recipe); + + double timestamp = 0.0; + ASSERT_FALSE(package.getData("timestamp", timestamp)); + + package.initEmpty({ "DOUBLE", "VECTOR6D" }); + + EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +// An input package is written before the recipe has been acknowledged, so setData() has to be able +// to decide a field's type itself. Whether it matches the robot is checked when the package is sent. +TEST(rtde_data_package, set_data_establishes_the_type_of_an_untyped_field) +{ + rtde_interface::DataPackage package({ "speed_slider_mask", "speed_slider_fraction" }); + + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + + double speed_slider_fraction = 0.0; + ASSERT_TRUE(package.getData("speed_slider_fraction", speed_slider_fraction)); + EXPECT_DOUBLE_EQ(speed_slider_fraction, 0.5); + + EXPECT_EQ(package.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); + + // The field that was never written keeps no type at all + uint32_t speed_slider_mask = 1; + EXPECT_FALSE(package.getData("speed_slider_mask", speed_slider_mask)); + EXPECT_FALSE(package.getDataType("speed_slider_mask").has_value()); +} + +TEST(rtde_data_package, get_data_type_reports_unknown_fields_and_untyped_fields) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + + EXPECT_FALSE(package.getDataType("not_in_the_recipe").has_value()); + + // Asking about a field of a package the robot hasn't acknowledged yet is the other way to get a + // negative answer, and it is what tells an application the package isn't usable yet. + rtde_interface::DataPackage untyped_package({ "timestamp" }); + EXPECT_FALSE(untyped_package.getDataType("timestamp").has_value()); +} + +// Once a field has a type, whether from the robot or from an earlier write, a differently typed +// write is a mistake rather than a retype. +TEST(rtde_data_package, set_data_checks_against_an_established_type) +{ + rtde_interface::DataPackage package({ "speed_slider_fraction" }); + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + + EXPECT_FALSE(package.setData("speed_slider_fraction", static_cast(1))); + + auto typed_package = typedPackage({ "timestamp" }, { "DOUBLE" }); + EXPECT_FALSE(typed_package.setData("timestamp", static_cast(1))); +} + +// Whether a package knows its types is answered by the fields themselves rather than by a flag +// recording that the robot answered, so writing every field of an untyped package is enough to make +// it serializable. Values written this way are still checked against the robot when the package is +// handed to RTDEWriter::sendPackage(). +TEST(rtde_data_package, writing_every_field_makes_a_package_serializable) +{ + rtde_interface::DataPackage package({ "speed_slider_mask", "speed_slider_fraction" }); + package.setRecipeID(1); + + uint8_t buffer[4096]; + ASSERT_EQ(package.serializePackage(buffer), 0) << "no field has a type yet"; + + ASSERT_TRUE(package.setData("speed_slider_mask", static_cast(1))); + EXPECT_EQ(package.serializePackage(buffer), 0) << "speed_slider_fraction still has no type"; + + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + // A two byte size, a one byte package type and the one byte recipe id, then the two fields + const size_t header_size = 4; + EXPECT_EQ(package.serializePackage(buffer), header_size + sizeof(uint32_t) + sizeof(double)); +} + +// Merging a partially written package into the send buffer belongs to RTDEWriter, so it is covered +// by the sendPackage() tests in test_rtde_writer.cpp. + +// Zeroing a package has to keep the types intact, otherwise the next serialization would use the +// wrong field sizes. +TEST(rtde_data_package, init_empty_keeps_types) +{ + auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + + package.initEmpty(); + + EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + double timestamp = 1.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 0.0); +} + +TEST(rtde_data_package, copy_keeps_types_and_values) +{ + auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + + rtde_interface::DataPackage copy(package); + + EXPECT_EQ(copy.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + double timestamp = 0.0; + ASSERT_TRUE(copy.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +TEST(rtde_data_package, get_data_with_wrong_type_fails) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + + // The robot dictates the types, so asking for the wrong one has to fail gracefully instead of + // throwing std::bad_variant_access. + uint32_t timestamp = 0; + EXPECT_FALSE(package.getData("timestamp", timestamp)); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 20a2a0eff..8a0e5ef1f 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -31,13 +31,15 @@ #include #include +#include "rtde_test_helpers.h" + using namespace urcl; TEST(rtde_parser, request_protocol_version) { // Accepted request protocol version unsigned char raw_data[] = { 0x00, 0x04, 0x56, 0x01 }; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); // test a non-preallocated product std::unique_ptr product; @@ -83,7 +85,7 @@ TEST(rtde_parser, get_urcontrol_version) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::GetUrcontrolVersion* data = dynamic_cast(product.get())) @@ -107,7 +109,7 @@ TEST(rtde_parser, control_package_pause) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackagePause* data = dynamic_cast(product.get())) @@ -128,7 +130,7 @@ TEST(rtde_parser, control_package_start) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackageStart* data = dynamic_cast(product.get())) @@ -150,7 +152,7 @@ TEST(rtde_parser, control_package_setup_inputs) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackageSetupInputs* data = @@ -174,7 +176,7 @@ TEST(rtde_parser, control_package_setup_outputs) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -200,7 +202,8 @@ TEST(rtde_parser, data_package) std::unique_ptr product; std::vector recipe = { "timestamp", "target_speed_fraction" }; - rtde_interface::RTDEParser parser(recipe); + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -220,6 +223,46 @@ TEST(rtde_parser, data_package) } } +TEST(rtde_parser, data_package_without_recipe_types_fails) +{ + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + // Without the types from the robot's acknowledgement the payload cannot be interpreted + std::unique_ptr product; + test::TestableRTDEParser parser({ "timestamp", "target_speed_fraction" }); + parser.setProtocolVersion(2); + + EXPECT_FALSE(parser.parse(bp, product)); +} + +TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) +{ + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + + // Applications that still create their packages from a recipe hand in an untyped package. It has + // to be usable afterwards, without being replaced by a freshly allocated one. + std::unique_ptr product = std::make_unique(recipe); + const rtde_interface::RTDEPackage* package_address = product.get(); + + ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); + + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + TEST(rtde_parser, test_to_string) { // Non-existent type @@ -227,7 +270,7 @@ TEST(rtde_parser, test_to_string) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); parser.parse(bp, product); std::stringstream expected; @@ -244,7 +287,7 @@ TEST(rtde_parser, test_buffer_too_short) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); EXPECT_FALSE(parser.parse(bp, product)); } @@ -255,7 +298,7 @@ TEST(rtde_parser, test_buffer_too_long) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - rtde_interface::RTDEParser parser({ "" }); + test::TestableRTDEParser parser({ "" }); EXPECT_FALSE(parser.parse(bp, product)); } @@ -265,7 +308,8 @@ TEST(rtde_parser, test_deprecated_parse_method) 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); + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); std::vector> products; diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index b95e9f088..a3de7a4ca 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -34,6 +34,9 @@ #include #include #include +#include + +#include "rtde_test_helpers.h" using namespace urcl; @@ -53,7 +56,8 @@ class RTDEWriterTest : public ::testing::Test stream_.reset(new comm::URStream("127.0.0.1", 60004)); stream_->connect(); - writer_.reset(new rtde_interface::RTDEWriter(stream_.get(), input_recipe_)); + writer_.reset(new test::TestableRTDEWriter(stream_.get(), input_recipe_)); + writer_->setRecipeTypes(input_recipe_types_); writer_->init(1); } @@ -124,7 +128,11 @@ class RTDEWriterTest : public ::testing::Test "input_int_register_25", "input_double_register_25", "external_force_torque" }; - std::unique_ptr writer_; + // The data types the robot would report for the recipe above when acknowledging it + std::vector input_recipe_types_ = { "UINT32", "DOUBLE", "UINT8", "UINT8", "UINT8", "UINT8", + "UINT8", "UINT8", "UINT8", "UINT8", "DOUBLE", "DOUBLE", + "BOOL", "INT32", "DOUBLE", "VECTOR6D" }; + std::unique_ptr writer_; std::unique_ptr server_; std::unique_ptr> stream_; std::unordered_map parsed_data_; @@ -191,6 +199,23 @@ TEST_F(RTDEWriterTest, send_speed_slider) EXPECT_FALSE(writer_->sendSpeedSlider(2)); } +TEST_F(RTDEWriterTest, masks_do_not_leak_into_the_following_package) +{ + // A mask tells the robot which of the fields in a package it should actually act on, so a mask + // left over from a previous send would make the robot re-apply a value the caller didn't ask for. + ASSERT_TRUE(writer_->sendSpeedSlider(0.5)); + ASSERT_TRUE(waitForMessageCallback(1000)); + ASSERT_EQ(std::get(parsed_data_["speed_slider_mask"]), 1); + + ASSERT_TRUE(writer_->sendStandardDigitalOutput(2, true)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + // Parsing the second package at all only works if resetting the mask kept its data type, since + // the type decides how many bytes the field takes up on the wire. + EXPECT_EQ(std::get(parsed_data_["speed_slider_mask"]), 0); + EXPECT_EQ(std::get(parsed_data_["standard_digital_output_mask"]), 4); +} + TEST_F(RTDEWriterTest, send_standard_digital_output) { uint8_t expected_standard_digital_output_mask = 4; @@ -520,6 +545,56 @@ TEST_F(RTDEWriterTest, send_data_package) EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); } +// The fields an application leaves alone are sent as zeros, so a package means the same thing no +// matter which values happened to be sent before it. +TEST_F(RTDEWriterTest, unset_fields_are_sent_as_zeros) +{ + ASSERT_TRUE(writer_->sendSpeedSlider(0.7)); + ASSERT_TRUE(waitForMessageCallback(1000)); + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + ASSERT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.7); + + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); + ASSERT_TRUE(writer_->sendPackage(data_package)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + ASSERT_TRUE(dataFieldExist("standard_analog_output_0")); + EXPECT_EQ(std::get(parsed_data_["standard_analog_output_0"]), 0.4); + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); +} + +// The robot is the authority on a field's type, so writing one with the wrong type has to be +// reported rather than serialized into a package the robot would misread. +TEST_F(RTDEWriterTest, send_data_package_with_wrong_field_type_fails) +{ + rtde_interface::DataPackage data_package(input_recipe_); + // The robot reports speed_slider_mask as UINT32 + ASSERT_TRUE(data_package.setData("speed_slider_mask", static_cast(1))); + + EXPECT_FALSE(writer_->sendPackage(data_package)); +} + +TEST_F(RTDEWriterTest, send_data_package_with_unknown_field_fails) +{ + rtde_interface::DataPackage data_package({ "not_a_field_the_robot_knows" }); + ASSERT_TRUE(data_package.setData("not_a_field_the_robot_knows", 1.0)); + + EXPECT_FALSE(writer_->sendPackage(data_package)); +} + +// Until the robot has reported the data types of the input recipe, there is nothing to serialize +// against. +TEST_F(RTDEWriterTest, send_data_package_before_types_are_known_fails) +{ + rtde_interface::RTDEWriter writer(stream_.get(), input_recipe_); + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", 0.5)); + + EXPECT_FALSE(writer.sendPackage(data_package)); +} + TEST_F(RTDEWriterTest, init_while_running_throws) { EXPECT_THROW(writer_->init(1), UrException); From 13b7c3503328486125bde68024779a15d0ee40b8 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 13:19:16 +0200 Subject: [PATCH 02/18] Fix leftover handshake state after a failed RTDE setup. A thrown init() left the client INITIALIZING so a retry reported success, packages kept protocol v2 after a v1 fallback, and sendPackage() cleared the store buffer before validation. Disconnect on those exceptions, apply the negotiated version when typing, and validate input first. --- include/ur_client_library/rtde/data_package.h | 29 +- include/ur_client_library/rtde/rtde_writer.h | 3 +- src/rtde/data_package.cpp | 42 ++- src/rtde/rtde_client.cpp | 35 ++- src/rtde/rtde_parser.cpp | 1 + src/rtde/rtde_writer.cpp | 16 +- tests/CMakeLists.txt | 18 +- tests/fake_rtde_server.cpp | 58 +++- tests/fake_rtde_server.h | 31 ++ tests/rtde_test_helpers.h | 1 + tests/test_primary_client_reconnect.cpp | 3 +- tests/test_rtde_client.cpp | 245 +-------------- tests/test_rtde_client_fake_server.cpp | 77 +++++ tests/test_rtde_client_reconnect.cpp | 282 ++++++++++++++++++ tests/test_rtde_data_package.cpp | 140 +++++++++ tests/test_rtde_parser.cpp | 64 ++++ tests/test_rtde_writer.cpp | 17 ++ 17 files changed, 783 insertions(+), 279 deletions(-) create mode 100644 tests/test_rtde_client_reconnect.cpp diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index fdb62577e..59a79b948 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -351,6 +351,18 @@ class DataPackage : public RTDEPackage */ void initEmpty(const std::vector& types); + /*! + * \brief Records the RTDE protocol version this package will parse and serialize. + * + * Version 2 data packages start with a recipe-id byte; version 1 packages do not. The + * constructor defaults to version 2, so this has to be called when the handshake falls back + * to version 1. Assignment of a uint16_t does not allocate. + */ + void setProtocolVersion(const uint16_t protocol_version) + { + protocol_version_ = protocol_version; + } + private: /*! * \brief Whether every field of this package has a data type. @@ -382,16 +394,17 @@ class DataPackage : public RTDEPackage bool resetData(const std::string_view name); /*! - * \brief Copies the fields that \p other has values for into this package. - * - * Fields \p other hasn't written are left untouched, which is what lets an application send an - * input package covering only part of the recipe. This package keeps its own types, so it is - * where a type disagreement between the application and the robot surfaces. + * \brief Whether every set field in \p other can be copied into this package. * - * \param other The package to copy values from + * Does not modify this package. Used to validate a source before clearing the destination. + */ + bool canCopySetFieldsFrom(const DataPackage& other) const; + + /*! + * \brief Copies the fields that \p other has values for into this package. * - * \returns True if every value could be copied, false if \p other names a field this package - * doesn't have or holds a value of a different type than the robot reported for it + * Fields \p other hasn't written are left untouched. The source must already have been checked + * with canCopySetFieldsFrom(); this only writes the matching values. */ bool copySetFieldsFrom(const DataPackage& other); diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 807b37183..59c922684 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -208,10 +208,11 @@ class RTDEWriter * passed to sendPackage() are checked. * * \param types The data types of the input recipe's fields, in the same order as the recipe + * \param protocol_version The RTDE protocol version negotiated with the robot * * \throws UrException if the number of types doesn't match the recipe or if a type is unknown */ - void setRecipeTypes(const std::vector& types); + void setRecipeTypes(const std::vector& types, uint16_t protocol_version = 2); private: void resetMasks(const std::shared_ptr& buffer); diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index bb0b2e75d..5cf0efd36 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -334,7 +334,11 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) return 0; } - uint16_t payload_size = sizeof(recipe_id_); + uint16_t payload_size = 0; + if (protocol_version_ == 2) + { + payload_size += sizeof(recipe_id_); + } for (auto& item : data_) { @@ -353,7 +357,10 @@ 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_); + if (protocol_version_ == 2) + { + size += comm::PackageSerializer::serialize(buffer + size, recipe_id_); + } for (size_t i = 0; i < data_.size(); ++i) { size += std::visit( @@ -387,9 +394,9 @@ bool rtde_interface::DataPackage::resetData(const std::string_view name) return true; } -bool rtde_interface::DataPackage::copySetFieldsFrom(const DataPackage& other) +bool rtde_interface::DataPackage::canCopySetFieldsFrom(const DataPackage& other) const { - bool all_copied = true; + bool all_compatible = true; for (const auto& source : other.data_) { if (std::holds_alternative(source.second)) @@ -404,19 +411,38 @@ bool rtde_interface::DataPackage::copySetFieldsFrom(const DataPackage& other) if (destination == data_.end()) { URCL_LOG_ERROR("The data field '%s' is not part of the recipe the robot acknowledged.", source.first.c_str()); - all_copied = false; + all_compatible = false; continue; } if (source.second.index() != destination->second.index()) { URCL_LOG_ERROR("The value passed for the data field '%s' is of type %s, but the robot reports that field as %s.", source.first.c_str(), typeNameOf(source.second).c_str(), typeNameOf(destination->second).c_str()); - all_copied = false; + all_compatible = false; + } + } + return all_compatible; +} + +bool rtde_interface::DataPackage::copySetFieldsFrom(const DataPackage& other) +{ + for (const auto& source : other.data_) + { + if (std::holds_alternative(source.second)) + { continue; } - destination->second = source.second; + + const auto destination = + std::find_if(data_.begin(), data_.end(), [&source](const std::pair& element) { + return element.first == source.first; + }); + if (destination != data_.end()) + { + destination->second = source.second; + } } - return all_copied; + return true; } } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 3c5f5d1df..1e1a45d07 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -120,18 +120,29 @@ bool RTDEClient::init(const size_t max_connection_attempts, const std::chrono::m unsigned int attempts = 0; std::stringstream ss; - while (!setupCommunication(max_connection_attempts, reconnection_timeout)) + try { - if (++attempts >= max_initialization_attempts) + while (!setupCommunication(max_connection_attempts, reconnection_timeout)) { + if (++attempts >= max_initialization_attempts) + { + disconnect(); + ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts"; + throw UrException(ss.str()); + } + // disconnect to start on a clean slate when trying to set up communication again disconnect(); - ss << "Failed to initialize RTDE client after " << max_initialization_attempts << " attempts"; - throw UrException(ss.str()); + URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); + std::this_thread::sleep_for(initialization_timeout); } - // disconnect to start on a clean slate when trying to set up communication again + } + catch (...) + { + // setupCommunication() can throw after setting INITIALIZING (invalid recipe, target frequency + // out of range). Leave the client disconnected and uninitialized so a later init() retries + // instead of returning true on a half-finished handshake. disconnect(); - URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in %d seconds", initialization_timeout.count() / 1000); - std::this_thread::sleep_for(initialization_timeout); + throw; } client_state_ = ClientState::INITIALIZED; // Set reconnection callback after we are initialized to ensure that a disconnect during initialization doesn't @@ -298,10 +309,9 @@ bool RTDEClient::queryURControlVersion() URCL_LOG_WARN("%s", ss.str().c_str()); } } - std::stringstream ss; - ss << "Could not query urcontrol version after " << MAX_REQUEST_RETRIES - << " tries. Please check the output of the " - "negotiation attempts above to get a hint what could be wrong."; + URCL_LOG_ERROR("Could not query urcontrol version after %u tries. Please check the output of the negotiation " + "attempts above to get a hint what could be wrong.", + MAX_REQUEST_RETRIES); return false; } @@ -439,6 +449,7 @@ bool RTDEClient::setupOutputs() // storage itself already exists, so this doesn't allocate and neither does the receive path // from here on. parser_.setRecipeTypes(variable_types); + preallocated_data_pkg_.setProtocolVersion(protocol_version_); preallocated_data_pkg_.initEmpty(variable_types); return true; } @@ -507,7 +518,7 @@ bool RTDEClient::setupInputs() throw RTDEInputConflictException(input_recipe_[i]); } } - writer_.setRecipeTypes(variable_types); + writer_.setRecipeTypes(variable_types, protocol_version_); writer_.init(tmp_input->input_recipe_id_); return true; diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 2d9e62d81..deb104d90 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -153,6 +153,7 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result } DataPackage* data_package = dynamic_cast(result.get()); + data_package->setProtocolVersion(protocol_version_); if (!data_package->isTyped()) { // A package built from a recipe alone doesn't know its field types yet. Applying the ones diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index 989f3cfbb..b1339d824 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -85,9 +85,11 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) current_send_buffer_ = data_buffer1_; } -void RTDEWriter::setRecipeTypes(const std::vector& types) +void RTDEWriter::setRecipeTypes(const std::vector& types, uint16_t protocol_version) { std::lock_guard lock_guard(store_mutex_); + data_buffer0_->setProtocolVersion(protocol_version); + data_buffer1_->setProtocolVersion(protocol_version); data_buffer0_->initEmpty(types); data_buffer1_->initEmpty(types); } @@ -163,13 +165,17 @@ bool RTDEWriter::sendPackage(const DataPackage& package) return false; } - // Fields the caller didn't write are sent as zeros rather than as whatever the previous package - // left in the buffer, so that a package means the same thing no matter what was sent before it. - current_store_buffer_->initEmpty(); - if (!current_store_buffer_->copySetFieldsFrom(package)) + // Validate before touching the store buffer, so a rejected package cannot wipe or partially + // overwrite input that is already queued. + if (!current_store_buffer_->canCopySetFieldsFrom(package)) { return false; } + + // Fields the caller didn't write are sent as zeros rather than as whatever the previous package + // left in the buffer, so that a package means the same thing no matter what was sent before it. + current_store_buffer_->initEmpty(); + current_store_buffer_->copySetFieldsFrom(package); markStorageToBeSent(); return true; } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 513ab0ff1..4ef2aeea2 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -41,9 +41,6 @@ if (INTEGRATION_TESTS) WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} EXTRA_ARGS ${INTEGRATION_TESTS_ROBOT_IP_ARG} ) - # Bound this teardown regression test so a hang fails CI fast instead of timing out the job. - set_tests_properties(RTDEClientTest.destructor_not_blocked_by_stuck_reconnect_thread - PROPERTIES TIMEOUT 60) if (CHECK_RTDE_DOCS_RECIPE) find_package(Python3 COMPONENTS Interpreter REQUIRED) add_custom_target(generate_outputs ALL COMMAND ${Python3_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/resources/generate_rtde_outputs.py) @@ -227,6 +224,21 @@ gtest_add_tests(TARGET rtde_client_fake_server_tests WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} ) +# Covers RTDEClient::reconnect() by taking the fake RTDE server away and giving it back, so it runs +# without a robot. Kept apart from the tests above because these wait on retry timing and are +# therefore slower. +add_executable(rtde_client_reconnect_tests test_rtde_client_reconnect.cpp fake_rtde_server.cpp) +target_link_libraries(rtde_client_reconnect_tests PRIVATE ur_client_library::urcl GTest::gtest_main) +gtest_add_tests(TARGET rtde_client_reconnect_tests + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} +) +# Bound these so a hang fails CI fast instead of timing out the job. +set_tests_properties(RTDEClientReconnectTest.destructor_not_blocked_by_stuck_reconnect_thread + RTDEClientReconnectTest.reconnects_when_the_server_comes_back_during_background_read + RTDEClientReconnectTest.reconnects_when_the_server_comes_back_during_blocking_read + RTDEClientReconnectTest.destroying_the_client_while_the_server_is_silent + PROPERTIES TIMEOUT 60) + add_executable(tcp_server_tests test_tcp_server.cpp) if (MSVC) target_compile_options(tcp_server_tests PRIVATE /Zc:lambda) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 4dac1144e..16ad26e53 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -506,6 +506,44 @@ RTDEServer::~RTDEServer() stopSendingDataPackages(); } +void RTDEServer::queueTextMessageBeforeVersionReply(const std::string& message) +{ + std::lock_guard lock(negotiation_mutex_); + pending_text_messages_.push_back(message); +} + +void RTDEServer::setHighestAcceptedProtocolVersion(const uint16_t highest_accepted) +{ + std::lock_guard lock(negotiation_mutex_); + highest_accepted_protocol_version_ = highest_accepted; +} + +std::vector RTDEServer::requestedProtocolVersions() +{ + std::lock_guard lock(negotiation_mutex_); + return requested_protocol_versions_; +} + +void RTDEServer::sendTextMessage(const socket_t filedescriptor, const std::string& message) +{ + const std::string source = "fake_rtde_server"; + const uint8_t warning_level = 1; + + comm::PackageSerializer serializer; + uint8_t send_buffer[4096]; + const size_t payload_size = 2 * sizeof(uint8_t) + message.size() + source.size() + sizeof(warning_level); + size_t send_size = rtde_interface::PackageHeader::serializeHeader( + send_buffer, rtde_interface::PackageType::RTDE_TEXT_MESSAGE, static_cast(payload_size)); + send_size += serializer.serialize(send_buffer + send_size, static_cast(message.size())); + send_size += serializer.serialize(send_buffer + send_size, message); + send_size += serializer.serialize(send_buffer + send_size, static_cast(source.size())); + send_size += serializer.serialize(send_buffer + send_size, source); + send_size += serializer.serialize(send_buffer + send_size, warning_level); + + size_t written = 0; + server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); +} + void RTDEServer::connectionCallback(const socket_t filedescriptor) { client_socket_ = filedescriptor; @@ -528,7 +566,14 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, { case rtde_interface::PackageType::RTDE_REQUEST_PROTOCOL_VERSION: { - bool accepted = true; + uint16_t requested_version = 0; + bp.parse(requested_version); + bool accepted; + { + std::lock_guard lock(negotiation_mutex_); + requested_protocol_versions_.push_back(requested_version); + accepted = requested_version <= highest_accepted_protocol_version_; + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; @@ -542,6 +587,17 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, } case rtde_interface::PackageType::RTDE_GET_URCONTROL_VERSION: { + // The client only asks once, so every queued message has to go out now for it to see them all + std::deque text_messages; + { + std::lock_guard lock(negotiation_mutex_); + text_messages.swap(pending_text_messages_); + } + for (const std::string& text_message : text_messages) + { + sendTextMessage(filedescriptor, text_message); + } + comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index b34eec510..a1fef6b75 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -23,6 +24,29 @@ class RTDEServer void setStartTime(const std::chrono::steady_clock::time_point& start_time); + /*! + * \brief Makes the server send \p message ahead of its answer to the URControl version query. + * + * Real controllers do this: a PolyScope X simulator reports "SafetySetup has not been confirmed + * yet" on every connect until it has been switched on. Queue more messages than the client is + * willing to retry and it will give up on the query. + */ + void queueTextMessageBeforeVersionReply(const std::string& message); + + /*! + * \brief Makes the server refuse every protocol version above \p highest_accepted. + * + * Lets a test drive the client's fallback to an older protocol version, the way a controller too + * old for the newest version would. + */ + void setHighestAcceptedProtocolVersion(const uint16_t highest_accepted); + + /*! + * \brief The protocol versions the client asked for, in the order it asked, so a test can see it + * work its way down. + */ + std::vector requestedProtocolVersions(); + private: std::vector input_recipe_; std::vector output_recipe_; @@ -50,8 +74,15 @@ class RTDEServer void actOnInput(); + void sendTextMessage(const socket_t filedescriptor, const std::string& message); + std::mutex output_data_mutex_; std::mutex thread_control_mutex_; + + std::mutex negotiation_mutex_; + std::deque pending_text_messages_; + uint16_t highest_accepted_protocol_version_ = 2; + std::vector requested_protocol_versions_; }; } // namespace urcl diff --git a/tests/rtde_test_helpers.h b/tests/rtde_test_helpers.h index f85be8126..a0b7511f0 100644 --- a/tests/rtde_test_helpers.h +++ b/tests/rtde_test_helpers.h @@ -50,6 +50,7 @@ class TestableDataPackage : public rtde_interface::DataPackage public: using rtde_interface::DataPackage::DataPackage; using rtde_interface::DataPackage::initEmpty; + using rtde_interface::DataPackage::setProtocolVersion; }; class TestableRTDEParser : public rtde_interface::RTDEParser diff --git a/tests/test_primary_client_reconnect.cpp b/tests/test_primary_client_reconnect.cpp index 1032bc19b..773430d93 100644 --- a/tests/test_primary_client_reconnect.cpp +++ b/tests/test_primary_client_reconnect.cpp @@ -45,7 +45,8 @@ using namespace urcl; // producer thread is stuck in its reconnect loop at teardown time. // // This is the PrimaryClient counterpart of -// RTDEClientTest.destructor_not_blocked_by_stuck_reconnect_thread (test_rtde_client.cpp). +// RTDEClientReconnectTest.destructor_not_blocked_by_stuck_reconnect_thread +// (test_rtde_client_reconnect.cpp). // // Root cause: when the robot drops the primary connection, TCPSocket::read() // returns false and leaves the socket in SocketState::LostConnection. URProducer's diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index f9938527d..d074ef485 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -32,7 +32,6 @@ #include #include #include -#include #include #include #include "ur_client_library/comm/tcp_server.h" @@ -299,6 +298,9 @@ TEST_F(RTDEClientTest, input_recipe_with_invalid_key) client_.reset(new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, resources_output_recipe_, actual_input_recipe)); + EXPECT_THAT([&]() { client_->init(); }, testing::ThrowsMessage(testing::HasSubstr("i_do_not_" + "exist"))); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); EXPECT_THAT([&]() { client_->init(); }, testing::ThrowsMessage(testing::HasSubstr("i_do_not_" "exist"))); } @@ -362,10 +364,10 @@ 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 + // A package built from a recipe alone is untyped until the first receive, so getData fails. rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); double timestamp; - EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_FALSE(data_pkg.getData("timestamp", timestamp)); ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); @@ -457,158 +459,6 @@ TEST_F(RTDEClientTest, get_data_package_fake_server) client_.reset(); } -TEST_F(RTDEClientTest, destroy_client_after_server_stops_sending) -{ - auto fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); - fake_rtde_server->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); - client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, - resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); - client_->init(); - client_->start(); - - URCL_LOG_INFO("Receiving data package from fake server to verify that connection is working."); - - const std::chrono::milliseconds read_timeout{ 100 }; - rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); - ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); - - double timestamp = 0.0; - EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); - EXPECT_GT(timestamp, 0.0); - - URCL_LOG_INFO("Stopping fake server from sending data packages."); - fake_rtde_server->stopSendingDataPackages(); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - - URCL_LOG_INFO("Destroying client while no more data packages are received from the server."); - client_.reset(); -} - -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 rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, - resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); - client_->init(0, std::chrono::milliseconds(123), 3, std::chrono::milliseconds(100)); - URCL_LOG_INFO("Client initiliazed"); - client_->start(); - - std::atomic keep_running = true; - std::thread data_consumer_thread([this, &keep_running]() { - rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); - const std::chrono::milliseconds read_timeout{ 100 }; - while (keep_running) - { - if (client_->getDataPackage(data_pkg, read_timeout)) - { - // 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(); - } - 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"); -} - -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(); @@ -855,91 +705,6 @@ TEST_F(RTDEClientTest, test_initialization) EXPECT_GE(std::chrono::duration_cast(elapsed).count(), 20); } -// Regression test for the bug where ~RTDEClient() could block indefinitely when -// the reconnect thread was stuck inside TCPSocket::setup(). Fixed by: (1) calling -// stream_.disconnect() (followed by RTDEClient::disconnect()) before joining reconnecting_thread_ -// in ~RTDEClient(), and (2) making TCPSocket::setup() abort on the deliberate-stop state, -// both during the (non-blocking) connect attempt and during the between-attempt wait. -// -// See also TCPSocketTest.setup_interruptible_by_close and -// TCPSocketTest.setup_interruptible_during_blocking_connect in test_tcp_socket.cpp -// for lower-level unit tests of the same fix that run without INTEGRATION_TESTS. -TEST_F(RTDEClientTest, destructor_not_blocked_by_stuck_reconnect_thread) -{ - // Use a large reconnection timeout so that the blocking window is clearly - // observable if the fix is absent (5 s sleep > 2 s assertion threshold). - const std::chrono::milliseconds large_reconnect_timeout(5000); - - auto fake_rtde_server = std::make_unique(g_FAKE_RTDE_PORT); - // Skip the bootup-timestamp check inside isRobotBooted(). - fake_rtde_server->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(52)); - - client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, - resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); - // Attempt init up to 10 times with a short between-attempt sleep to ensure - // the RTDE handshake succeeds even in environments where the fake server's - // response arrives slightly after the 1-second socket read timeout. - bool initialized = false; - for (int attempt = 0; attempt < 10 && !initialized; ++attempt) - { - try - { - // max_connection_attempts=0 (unlimited): TCPSocket::setup() sleeps - // large_reconnect_timeout between every failed connect attempt once the - // server is gone. Use a short initialization_timeout for fast retries. - client_->init(0, large_reconnect_timeout, 1, std::chrono::milliseconds(50)); - initialized = true; - } - catch (const UrException&) - { - // Recreate the client on each retry to start from a clean state. - client_.reset(new rtde_interface::RTDEClient("localhost", notifier_, resources_output_recipe_, - resources_input_recipe_, 100, false, g_FAKE_RTDE_PORT)); - } - } - if (!initialized) - { - GTEST_SKIP() << "Could not initialize RTDEClient with the fake server after 10 attempts; " - "this test requires a reliably responding RTDE server. " - "The TCPSocket-level regression test (TCPSocketTest.setup_interruptible_by_close) " - "verifies the underlying fix without a robot."; - } - - // start(true) arms the reconnect callback via the background read thread. - client_->start(true); - - // Drop the server — the background read thread detects the connection loss, - // calls reconnectCallback(), which launches reconnecting_thread_. That thread - // enters setupCommunication() -> TCPSocket::setup() and begins sleeping - // large_reconnect_timeout between retry attempts. - fake_rtde_server.reset(); - - // Give the reconnect thread time to reach the wait inside TCPSocket::setup(). - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - // The destructor must return quickly: disconnect() aborts setup()'s connect/wait, - // so the join completes in well under 2 s. Without the fix this would block for - // >= large_reconnect_timeout (5 s), or forever with unlimited attempts. - // Run the destructor on a worker with a watchdog so a regression fails fast with a - // clear message instead of hanging the test binary (the CTest TIMEOUT then reaps it). - std::packaged_task teardown([this]() { client_.reset(); }); - auto teardown_future = teardown.get_future(); - std::thread teardown_thread(std::move(teardown)); - - const auto t0 = std::chrono::steady_clock::now(); - if (teardown_future.wait_for(std::chrono::seconds(5)) == std::future_status::timeout) - { - teardown_thread.detach(); - FAIL() << "~RTDEClient() did not return within 5 s — reconnect thread was not aborted by disconnect()"; - } - teardown_thread.join(); - const auto elapsed = std::chrono::steady_clock::now() - t0; - - EXPECT_LT(elapsed, std::chrono::seconds(2)) - << "RTDEClient destructor blocked for " << std::chrono::duration_cast(elapsed).count() - << " ms — reconnect thread was not aborted by disconnect()"; -} - int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_client_fake_server.cpp b/tests/test_rtde_client_fake_server.cpp index 656aa7284..edf9bb7ea 100644 --- a/tests/test_rtde_client_fake_server.cpp +++ b/tests/test_rtde_client_fake_server.cpp @@ -166,6 +166,75 @@ TEST_F(RTDEClientFakeServerTest, version_is_taken_from_the_robot) EXPECT_EQ(version.minor, 10); } +// A PolyScope X simulator answers the version query with "SafetySetup has not been confirmed yet" +// on every connect until it has been switched on, so the client retries instead of giving up. +TEST_F(RTDEClientFakeServerTest, version_query_retries_past_a_safety_setup_text_message) +{ + server_->queueTextMessageBeforeVersionReply("SafetySetup has not been confirmed yet"); + + ASSERT_TRUE(client_->init()); + + const VersionInformation version = client_->getVersion(); + EXPECT_EQ(version.major, 10); +} + +// Any other text message is worth reporting, but is still only a reason to retry. +TEST_F(RTDEClientFakeServerTest, version_query_retries_past_an_unexpected_text_message) +{ + server_->queueTextMessageBeforeVersionReply("Something else entirely"); + server_->queueTextMessageBeforeVersionReply("And again"); + + ASSERT_TRUE(client_->init()); + + EXPECT_EQ(client_->getVersion().major, 10); +} + +// Retrying is bounded: MAX_REQUEST_RETRIES text messages in a row and the handshake fails rather +// than looping forever. +TEST_F(RTDEClientFakeServerTest, version_query_gives_up_after_too_many_text_messages) +{ + for (int i = 0; i < 10; ++i) + { + server_->queueTextMessageBeforeVersionReply("SafetySetup has not been confirmed yet"); + } + + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)), UrException); +} + +// A controller that does not know the newest protocol version refuses it, and the client works its +// way down instead of failing. +TEST_F(RTDEClientFakeServerTest, protocol_version_is_lowered_when_the_robot_refuses_it) +{ + server_->setHighestAcceptedProtocolVersion(1); + + // Whether the rest of the handshake completes over version 1 is beside the point here; what + // matters is that being refused made the client ask for a lower version. + try + { + client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)); + } + catch (const UrException&) + { + } + + const std::vector requested = server_->requestedProtocolVersions(); + ASSERT_GE(requested.size(), 2u) << "the client never asked for a second protocol version"; + EXPECT_EQ(requested[0], 2) << "the client should try the newest version first"; + EXPECT_EQ(requested[1], 1) << "the client should fall back to the next version down"; +} + +// If no version is acceptable the handshake has to fail, not spin. +TEST_F(RTDEClientFakeServerTest, init_fails_when_no_protocol_version_is_accepted) +{ + server_->setHighestAcceptedProtocolVersion(0); + + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)), UrException); + + const std::vector requested = server_->requestedProtocolVersions(); + EXPECT_FALSE(requested.empty()); + EXPECT_EQ(requested.back(), 1) << "the client should have tried every version down to the lowest"; +} + TEST_F(RTDEClientFakeServerTest, target_frequency_defaults_to_the_maximum) { auto client = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, 0.0); @@ -189,9 +258,13 @@ TEST_F(RTDEClientFakeServerTest, target_frequency_outside_the_robots_range_throw { auto too_low = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, -1.0); EXPECT_THROW(too_low->init(), UrException); + EXPECT_EQ(too_low->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + // The fake server allows only one client; drop the first connection before opening another. + too_low.reset(); auto too_high = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, g_MAX_FREQUENCY + 1.0); EXPECT_THROW(too_high->init(), UrException); + EXPECT_EQ(too_high->getClientState(), rtde_interface::ClientState::UNINITIALIZED); } TEST_F(RTDEClientFakeServerTest, receive_with_background_read) @@ -320,6 +393,8 @@ TEST_F(RTDEClientFakeServerTest, unknown_output_field_throws) auto client = makeClient({ "timestamp", "not_a_field_the_robot_knows" }, g_INPUT_RECIPE, g_RTDE_FREQUENCY); EXPECT_THROW(client->init(), RTDEInvalidKeyException); + EXPECT_EQ(client->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_THROW(client->init(), RTDEInvalidKeyException); } TEST_F(RTDEClientFakeServerTest, unknown_output_field_can_be_ignored) @@ -344,6 +419,8 @@ TEST_F(RTDEClientFakeServerTest, unknown_input_field_throws) auto client = makeClient(g_OUTPUT_RECIPE, { "not_a_field_the_robot_knows" }, g_RTDE_FREQUENCY); EXPECT_THROW(client->init(), RTDEInvalidKeyException); + EXPECT_EQ(client->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_THROW(client->init(), RTDEInvalidKeyException); } // The other constructor takes recipe files, and a missing or empty output recipe is rejected right diff --git a/tests/test_rtde_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp new file mode 100644 index 000000000..580a9d2c0 --- /dev/null +++ b/tests/test_rtde_client_reconnect.cpp @@ -0,0 +1,282 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +// Losing the RTDE connection and getting it back is handled by RTDEClient::reconnect(), running on +// its own thread. Everything it does happens between the client and the RTDE server, so the fake +// server is enough to drive it and these tests need no robot. + +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include "fake_rtde_server.h" + +using namespace urcl; + +namespace +{ +constexpr int g_FAKE_RTDE_PORT = 60007; +constexpr double g_RTDE_FREQUENCY = 100.0; + +const std::vector g_OUTPUT_RECIPE{ "timestamp", "actual_q", "target_speed_fraction", "runtime_state" }; +const std::vector g_INPUT_RECIPE{ "speed_slider_mask", "speed_slider_fraction" }; + +// How long to allow for a state transition that depends on the reconnect thread's retry timing. +constexpr std::chrono::seconds g_STATE_CHANGE_TIMEOUT{ 10 }; +} // namespace + +class RTDEClientReconnectTest : public ::testing::Test +{ +protected: + void TearDown() override + { + client_.reset(); + server_.reset(); + } + + /*! + * \brief Starts a fake server that the client's bootup check will accept straight away. + * + * RTDEClient::isRobotBooted() reads data for a second when the reported uptime is below 40 + * seconds, which would only slow these tests down. + */ + void startServer() + { + server_ = std::make_unique(g_FAKE_RTDE_PORT); + server_->setStartTime(std::chrono::steady_clock::now() - std::chrono::seconds(42)); + } + + void makeClient() + { + client_ = std::make_unique("localhost", notifier_, g_OUTPUT_RECIPE, g_INPUT_RECIPE, + g_RTDE_FREQUENCY, false, g_FAKE_RTDE_PORT); + } + + /*! + * \brief Waits for the client to reach \p expected, so the tests don't depend on how long a + * reconnect attempt happens to take. + */ + bool waitForState(const rtde_interface::ClientState expected) + { + const auto deadline = std::chrono::steady_clock::now() + g_STATE_CHANGE_TIMEOUT; + while (std::chrono::steady_clock::now() < deadline) + { + if (client_->getClientState() == expected) + { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + return client_->getClientState() == expected; + } + + comm::INotifier notifier_; + std::unique_ptr server_; + std::unique_ptr client_; +}; + +// Dropping the server has to take the client down, and bringing it back has to get the client all +// the way to RUNNING again without the application doing anything. +TEST_F(RTDEClientReconnectTest, reconnects_when_the_server_comes_back_during_background_read) +{ + startServer(); + makeClient(); + ASSERT_TRUE(client_->init(0, std::chrono::milliseconds(123), 3, std::chrono::milliseconds(100))); + client_->start(); + + // A reader in the background, because that is what arms the reconnect callback and what an + // application would be doing when the connection drops. + std::atomic keep_running{ true }; + std::thread data_consumer([this, &keep_running]() { + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + while (keep_running) + { + if (!client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + server_.reset(); + EXPECT_TRUE(waitForState(rtde_interface::ClientState::UNINITIALIZED)) << "the client did not notice the lost server"; + + startServer(); + EXPECT_TRUE(waitForState(rtde_interface::ClientState::RUNNING)) << "the client did not reconnect"; + + keep_running = false; + data_consumer.join(); + + // Data has to actually flow again, not just the state having been restored + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + EXPECT_TRUE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); +} + +// The same recovery, but for a client reading synchronously. reconnect() restores whichever read +// mode was in use, so both need covering. +TEST_F(RTDEClientReconnectTest, reconnects_when_the_server_comes_back_during_blocking_read) +{ + startServer(); + makeClient(); + ASSERT_TRUE(client_->init(0, std::chrono::milliseconds(123), 3, std::chrono::milliseconds(100))); + client_->start(false); + + std::atomic keep_running{ true }; + std::thread data_consumer([this, &keep_running]() { + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + while (keep_running) + { + if (!client_->getDataPackageBlocking(data_pkg)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + server_.reset(); + EXPECT_TRUE(waitForState(rtde_interface::ClientState::UNINITIALIZED)) << "the client did not notice the lost server"; + + startServer(); + EXPECT_TRUE(waitForState(rtde_interface::ClientState::RUNNING)) << "the client did not reconnect"; + + keep_running = false; + data_consumer.join(); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + EXPECT_TRUE(client_->getDataPackageBlocking(data_pkg)); +} + +// A server that stays connected but stops talking leaves the read path waiting, which the +// destructor has to be able to tear down. +TEST_F(RTDEClientReconnectTest, destroying_the_client_while_the_server_is_silent) +{ + startServer(); + makeClient(); + ASSERT_TRUE(client_->init()); + client_->start(); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_TRUE(client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))); + double timestamp = 0.0; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + server_->stopSendingDataPackages(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + client_.reset(); +} + +// Regression test for the bug where ~RTDEClient() could block indefinitely when the reconnect +// thread was stuck inside TCPSocket::setup(). Fixed by: (1) calling stream_.disconnect() (followed +// by RTDEClient::disconnect()) before joining reconnecting_thread_ in ~RTDEClient(), and (2) making +// TCPSocket::setup() abort on the deliberate-stop state, both during the (non-blocking) connect +// attempt and during the between-attempt wait. +// +// See also TCPSocketTest.setup_interruptible_by_close and +// TCPSocketTest.setup_interruptible_during_blocking_connect in test_tcp_socket.cpp for lower-level +// unit tests of the same fix. +TEST_F(RTDEClientReconnectTest, destructor_not_blocked_by_stuck_reconnect_thread) +{ + // Large enough that the blocking window is clearly observable if the fix is absent: the 5 s sleep + // would exceed the 2 s assertion threshold below. + const std::chrono::milliseconds large_reconnect_timeout(5000); + + startServer(); + + // Retry the handshake a few times, so a fake server response arriving just after the socket read + // timeout doesn't fail the test before it has tested anything. + bool initialized = false; + for (int attempt = 0; attempt < 10 && !initialized; ++attempt) + { + makeClient(); + try + { + // max_connection_attempts=0 (unlimited): TCPSocket::setup() sleeps large_reconnect_timeout + // between every failed connect attempt once the server is gone. The short initialization + // timeout keeps the retries here quick. + client_->init(0, large_reconnect_timeout, 1, std::chrono::milliseconds(50)); + initialized = true; + } + catch (const UrException&) + { + // Fall through and start over from a clean client + } + } + if (!initialized) + { + GTEST_SKIP() << "Could not initialize RTDEClient with the fake server after 10 attempts; " + "this test requires a reliably responding RTDE server. " + "The TCPSocket-level regression test (TCPSocketTest.setup_interruptible_by_close) " + "verifies the underlying fix without a robot."; + } + + // start(true) arms the reconnect callback via the background read thread. + client_->start(true); + + // Drop the server: the background read thread detects the connection loss and calls + // reconnectCallback(), which launches reconnecting_thread_. That thread enters + // setupCommunication() -> TCPSocket::setup() and begins sleeping large_reconnect_timeout between + // retry attempts. + server_.reset(); + + // Give the reconnect thread time to reach the wait inside TCPSocket::setup(). + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + // The destructor must return quickly: disconnect() aborts setup()'s connect/wait, so the join + // completes in well under 2 s. Without the fix this would block for >= large_reconnect_timeout + // (5 s), or forever with unlimited attempts. Run it on a worker with a watchdog so a regression + // fails fast with a clear message instead of hanging the test binary. + std::packaged_task teardown([this]() { client_.reset(); }); + auto teardown_future = teardown.get_future(); + std::thread teardown_thread(std::move(teardown)); + + const auto t0 = std::chrono::steady_clock::now(); + if (teardown_future.wait_for(std::chrono::seconds(5)) == std::future_status::timeout) + { + teardown_thread.detach(); + FAIL() << "~RTDEClient() did not return within 5 s — reconnect thread was not aborted by disconnect()"; + } + teardown_thread.join(); + const auto elapsed = std::chrono::steady_clock::now() - t0; + + EXPECT_LT(elapsed, std::chrono::seconds(2)) + << "RTDEClient destructor blocked for " << std::chrono::duration_cast(elapsed).count() + << " ms — reconnect thread was not aborted by disconnect()"; +} diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 20a184ef7..c14578a37 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -126,6 +126,49 @@ TEST(rtde_data_package, parse_pkg_protocolv1) EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); } +// A package constructed before the handshake defaults to protocol version 2. Applying the +// negotiated version afterwards must make a version-1 payload parse without a recipe-id byte. +TEST(rtde_data_package, applying_types_also_applies_the_protocol_version) +{ + std::vector recipe{ "timestamp", "actual_q" }; + test::TestableDataPackage package(recipe); + package.setProtocolVersion(1); + package.initEmpty({ "DOUBLE", "VECTOR6D" }); + + 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, + 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, + 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; + comm::BinParser bp(data_package, sizeof(data_package)); + + ASSERT_TRUE(package.parseWith(bp)); + double timestamp = 0.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_NEAR(timestamp, 16854.1919, 1e-4); +} + +TEST(rtde_data_package, serialize_pkg_protocolv1) +{ + std::vector recipe{ "speed_slider_mask" }; + std::vector types{ "UINT32" }; + auto package = typedPackage(recipe, types, 1); + + uint32_t value = 1; + package.setData("speed_slider_mask", value); + + uint8_t buffer[4096]; + size_t size = package.serializePackage(buffer); + + EXPECT_EQ(size, 7); + + uint8_t expected[] = { 0x0, 0x07, 0x55, 0x00, 0x00, 0x00, 0x01 }; + + for (size_t i = 0; i < size; ++i) + { + EXPECT_EQ(buffer[i], expected[i]); + } +} + TEST(rtde_data_package, get_data_not_part_of_recipe) { std::vector recipe{ "timestamp", "actual_q" }; @@ -254,6 +297,103 @@ TEST(rtde_data_package, every_rtde_data_type_can_be_applied) EXPECT_FALSE(package.getData("f_v6uint32", v6int32_value)); } +// The wire format of the rarer data types is otherwise only exercised against a real robot, so a +// serializer or parser that got one of them wrong would pass every other test here. Values are +// chosen to be asymmetric, so a byte-order mistake cannot round-trip by accident. +TEST(rtde_data_package, every_rtde_data_type_survives_a_serialize_parse_round_trip) +{ + const std::vector recipe{ "f_bool", "f_uint8", "f_uint32", "f_uint64", "f_int32", + "f_double", "f_vector3d", "f_vector6d", "f_v6int32", "f_v6uint32" }; + const std::vector types{ "BOOL", "UINT8", "UINT32", "UINT64", "INT32", + "DOUBLE", "VECTOR3D", "VECTOR6D", "VECTOR6INT32", "VECTOR6UINT32" }; + + const bool bool_value = true; + const uint8_t uint8_value = 0xa5; + const uint32_t uint32_value = 0x12345678; + const uint64_t uint64_value = 0x0123456789abcdef; + const int32_t int32_value = -123456789; + const double double_value = -1234.5678; + const vector3d_t vector3d_value{ 1.5, -2.5, 3.5 }; + const vector6d_t vector6d_value{ -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }; + const vector6int32_t v6int32_value{ -1, 2, -3, 4, -5, 6 }; + const vector6uint32_t v6uint32_value{ 1u, 2u, 3u, 4u, 5u, 0xffffffffu }; + + auto sent = typedPackage(recipe, types); + sent.setRecipeID(1); + ASSERT_TRUE(sent.setData("f_bool", bool_value)); + ASSERT_TRUE(sent.setData("f_uint8", uint8_value)); + ASSERT_TRUE(sent.setData("f_uint32", uint32_value)); + ASSERT_TRUE(sent.setData("f_uint64", uint64_value)); + ASSERT_TRUE(sent.setData("f_int32", int32_value)); + ASSERT_TRUE(sent.setData("f_double", double_value)); + ASSERT_TRUE(sent.setData("f_vector3d", vector3d_value)); + ASSERT_TRUE(sent.setData("f_vector6d", vector6d_value)); + ASSERT_TRUE(sent.setData("f_v6int32", v6int32_value)); + ASSERT_TRUE(sent.setData("f_v6uint32", v6uint32_value)); + + uint8_t buffer[4096]; + const size_t size = sent.serializePackage(buffer); + + // A two byte size and a one byte package type, then the recipe id and one entry per field + const size_t header_size = 3; + const size_t expected_payload = sizeof(uint8_t) + sizeof(bool) + sizeof(uint8_t) + sizeof(uint32_t) + + sizeof(uint64_t) + sizeof(int32_t) + sizeof(double) + sizeof(vector3d_t) + + sizeof(vector6d_t) + sizeof(vector6int32_t) + sizeof(vector6uint32_t); + EXPECT_EQ(size, header_size + expected_payload); + + // Round-tripping on its own would still pass if both directions agreed on the wrong byte order, + // so pin the integers to the network order the protocol uses. -123456789 is 0xf8a432eb. + const uint8_t expected_integers[] = { 0x01, // recipe id + 0x01, // f_bool + 0xa5, // f_uint8 + 0x12, 0x34, 0x56, 0x78, // f_uint32 + 0x01, 0x23, 0x45, 0x67, 0x89, 0xab, 0xcd, 0xef, // f_uint64 + 0xf8, 0xa4, 0x32, 0xeb }; // f_int32 + for (size_t i = 0; i < sizeof(expected_integers); ++i) + { + EXPECT_EQ(buffer[header_size + i], expected_integers[i]) << "at payload byte " << i; + } + + // parseWith() starts at the recipe id, which is where serializePackage() put it after the header + comm::BinParser bp(buffer + header_size, size - header_size); + auto received = typedPackage(recipe, types); + ASSERT_TRUE(received.parseWith(bp)); + EXPECT_TRUE(bp.empty()) << "the parser did not consume exactly what was serialized"; + + bool bool_read; + uint8_t uint8_read; + uint32_t uint32_read; + uint64_t uint64_read; + int32_t int32_read; + double double_read; + vector3d_t vector3d_read; + vector6d_t vector6d_read; + vector6int32_t v6int32_read; + vector6uint32_t v6uint32_read; + + ASSERT_TRUE(received.getData("f_bool", bool_read)); + ASSERT_TRUE(received.getData("f_uint8", uint8_read)); + ASSERT_TRUE(received.getData("f_uint32", uint32_read)); + ASSERT_TRUE(received.getData("f_uint64", uint64_read)); + ASSERT_TRUE(received.getData("f_int32", int32_read)); + ASSERT_TRUE(received.getData("f_double", double_read)); + ASSERT_TRUE(received.getData("f_vector3d", vector3d_read)); + ASSERT_TRUE(received.getData("f_vector6d", vector6d_read)); + ASSERT_TRUE(received.getData("f_v6int32", v6int32_read)); + ASSERT_TRUE(received.getData("f_v6uint32", v6uint32_read)); + + EXPECT_EQ(bool_read, bool_value); + EXPECT_EQ(uint8_read, uint8_value); + EXPECT_EQ(uint32_read, uint32_value); + EXPECT_EQ(uint64_read, uint64_value); + EXPECT_EQ(int32_read, int32_value); + EXPECT_EQ(double_read, double_value); + EXPECT_EQ(vector3d_read, vector3d_value); + EXPECT_EQ(vector6d_read, vector6d_value); + EXPECT_EQ(v6int32_read, v6int32_value); + EXPECT_EQ(v6uint32_read, v6uint32_value); +} + TEST(rtde_data_package, unknown_data_types_are_rejected) { std::vector recipe{ "timestamp" }; diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 8a0e5ef1f..f33157525 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -263,6 +263,29 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) EXPECT_DOUBLE_EQ(timestamp, 16412.206); } +TEST(rtde_parser, untyped_pre_allocated_data_package_takes_protocol_version_1) +{ + // Same payload as data_package, but without the recipe-id byte that only version 2 uses. + unsigned char raw_data[] = { 0x00, 0x13, 0x55, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, + 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(1); + + std::unique_ptr product = std::make_unique(recipe); + + ASSERT_TRUE(parser.parse(bp, product)); + + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + TEST(rtde_parser, test_to_string) { // Non-existent type @@ -338,6 +361,47 @@ TEST(rtde_parser, test_deprecated_parse_method) } } +// The robot reports problems with the connection as text messages, and RTDEClient acts on their +// content while negotiating, so the fields have to come out of the wire intact. +TEST(rtde_parser, text_message_protocol_v2) +{ + // size 0x000f, type 'M', message "hello", source "urcl", warning level 1 + unsigned char raw_data[] = { 0x00, 0x0f, 0x4d, 0x05, 'h', 'e', 'l', 'l', 'o', 0x04, 'u', 'r', 'c', 'l', 0x01 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + test::TestableRTDEParser parser({ "" }); + parser.setProtocolVersion(2); + + std::unique_ptr product; + ASSERT_TRUE(parser.parse(bp, product)); + + auto* message = dynamic_cast(product.get()); + ASSERT_NE(message, nullptr) << "the parser did not produce a TextMessage"; + EXPECT_EQ(message->message_, "hello"); + EXPECT_EQ(message->source_, "urcl"); + EXPECT_EQ(message->warning_level_, 1); + EXPECT_EQ(message->toString(), "message: hello\nsource: urcl\nwarning level: 1"); +} + +// Protocol version 1 puts a message type where version 2 has the lengths, and takes the rest of the +// package as the message. +TEST(rtde_parser, text_message_protocol_v1) +{ + // size 0x000a, type 'M', message type 3, message "legacy" + unsigned char raw_data[] = { 0x00, 0x0a, 0x4d, 0x03, 'l', 'e', 'g', 'a', 'c', 'y' }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + test::TestableRTDEParser parser({ "" }); + + std::unique_ptr product; + ASSERT_TRUE(parser.parse(bp, product)); + + auto* message = dynamic_cast(product.get()); + ASSERT_NE(message, nullptr) << "the parser did not produce a TextMessage"; + EXPECT_EQ(message->message_type_, 3); + EXPECT_EQ(message->message_, "legacy"); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index a3de7a4ca..6e8bd1b7c 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -576,6 +576,23 @@ TEST_F(RTDEWriterTest, send_data_package_with_wrong_field_type_fails) EXPECT_FALSE(writer_->sendPackage(data_package)); } +// A rejected package must not write the fields that did match into the store buffer, or a later +// specialized send would transmit those leftover values. +TEST_F(RTDEWriterTest, failed_send_package_does_not_overwrite_the_store_buffer) +{ + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", 0.9)); + ASSERT_TRUE(data_package.setData("speed_slider_mask", static_cast(1))); + + EXPECT_FALSE(writer_->sendPackage(data_package)); + + ASSERT_TRUE(writer_->sendStandardDigitalOutput(2, true)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); +} + TEST_F(RTDEWriterTest, send_data_package_with_unknown_field_fails) { rtde_interface::DataPackage data_package({ "not_a_field_the_robot_knows" }); From 497a212957c02dc42df02f8400e9ced63b5a20f2 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 13:48:31 +0200 Subject: [PATCH 03/18] Cover RTDE handshake refusals and reconnect giving up. A failed init, a refused start or pause, and a handshake that never completes were only exercised on the success path; the fake server can now refuse those replies so those failures stay failures. --- tests/CMakeLists.txt | 1 + tests/fake_rtde_server.cpp | 76 ++++++++++++++++++++++++-- tests/fake_rtde_server.h | 27 +++++++++ tests/test_rtde_client_fake_server.cpp | 73 +++++++++++++++++++++++++ tests/test_rtde_client_reconnect.cpp | 41 ++++++++++++++ 5 files changed, 214 insertions(+), 4 deletions(-) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 4ef2aeea2..e84068399 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -237,6 +237,7 @@ set_tests_properties(RTDEClientReconnectTest.destructor_not_blocked_by_stuck_rec RTDEClientReconnectTest.reconnects_when_the_server_comes_back_during_background_read RTDEClientReconnectTest.reconnects_when_the_server_comes_back_during_blocking_read RTDEClientReconnectTest.destroying_the_client_while_the_server_is_silent + RTDEClientReconnectTest.reconnect_gives_up_when_the_handshake_keeps_failing PROPERTIES TIMEOUT 60) add_executable(tcp_server_tests test_tcp_server.cpp) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 16ad26e53..f2ea47703 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -524,6 +524,30 @@ std::vector RTDEServer::requestedProtocolVersions() return requested_protocol_versions_; } +void RTDEServer::setAcceptStart(const bool accept) +{ + std::lock_guard lock(negotiation_mutex_); + accept_start_ = accept; +} + +void RTDEServer::setAcceptPause(const bool accept) +{ + std::lock_guard lock(negotiation_mutex_); + accept_pause_ = accept; +} + +void RTDEServer::queueTextMessageBeforeSetupOutputs(const std::string& message) +{ + std::lock_guard lock(negotiation_mutex_); + pending_setup_outputs_text_messages_.push_back(message); +} + +void RTDEServer::queueTextMessageBeforeSetupInputs(const std::string& message) +{ + std::lock_guard lock(negotiation_mutex_); + pending_setup_inputs_text_messages_.push_back(message); +} + void RTDEServer::sendTextMessage(const socket_t filedescriptor, const std::string& message) { const std::string source = "fake_rtde_server"; @@ -615,6 +639,21 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, } case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: { + std::string unexpected_message; + { + std::lock_guard lock(negotiation_mutex_); + if (!pending_setup_outputs_text_messages_.empty()) + { + unexpected_message = pending_setup_outputs_text_messages_.front(); + pending_setup_outputs_text_messages_.pop_front(); + } + } + if (!unexpected_message.empty()) + { + sendTextMessage(filedescriptor, unexpected_message); + break; + } + bp.parse(output_frequency_); URCL_LOG_DEBUG("Frequency is set to %f", output_frequency_); std::string variable_names_str; @@ -649,6 +688,21 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, } case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS: { + // The client sends the input recipe once, so every queued unexpected reply has to go out now. + std::deque unexpected_messages; + { + std::lock_guard lock(negotiation_mutex_); + unexpected_messages.swap(pending_setup_inputs_text_messages_); + } + if (!unexpected_messages.empty()) + { + for (const std::string& unexpected_message : unexpected_messages) + { + sendTextMessage(filedescriptor, unexpected_message); + } + break; + } + std::string variable_names_str; bp.parseRemainder(variable_names_str); input_recipe_ = splitString(variable_names_str); @@ -679,32 +733,46 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, } case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_START: { + bool accepted; + { + std::lock_guard lock(negotiation_mutex_); + accepted = accept_start_; + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; send_size += rtde_interface::PackageHeader::serializeHeader( send_buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_START, sizeof(uint8_t)); - bool accepted = true; send_size += serializer.serialize(send_buffer + send_size, accepted); size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); - startSendingDataPackages(); + if (accepted) + { + startSendingDataPackages(); + } break; } case rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_PAUSE: { + bool accepted; + { + std::lock_guard lock(negotiation_mutex_); + accepted = accept_pause_; + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; send_size += rtde_interface::PackageHeader::serializeHeader( send_buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_PAUSE, sizeof(uint8_t)); - bool accepted = true; send_size += serializer.serialize(send_buffer + send_size, accepted); size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); - stopSendingDataPackages(); + if (accepted) + { + stopSendingDataPackages(); + } break; } case rtde_interface::PackageType::RTDE_DATA_PACKAGE: diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index a1fef6b75..ae9f4eb02 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -47,6 +47,29 @@ class RTDEServer */ std::vector requestedProtocolVersions(); + /*! + * \brief Whether the next RTDE start request is accepted. A refused start must not leave the + * client believing it is streaming. + */ + void setAcceptStart(const bool accept); + + /*! + * \brief Whether the next RTDE pause request is accepted. + */ + void setAcceptPause(const bool accept); + + /*! + * \brief Answers the next output-recipe setup with a text message instead of the acknowledgement. + * + * Queue more messages than the client is willing to retry and setupOutputs() gives up. + */ + void queueTextMessageBeforeSetupOutputs(const std::string& message); + + /*! + * \brief Answers the next input-recipe setup with a text message instead of the acknowledgement. + */ + void queueTextMessageBeforeSetupInputs(const std::string& message); + private: std::vector input_recipe_; std::vector output_recipe_; @@ -81,8 +104,12 @@ class RTDEServer std::mutex negotiation_mutex_; std::deque pending_text_messages_; + std::deque pending_setup_outputs_text_messages_; + std::deque pending_setup_inputs_text_messages_; uint16_t highest_accepted_protocol_version_ = 2; std::vector requested_protocol_versions_; + bool accept_start_ = true; + bool accept_pause_ = true; }; } // namespace urcl diff --git a/tests/test_rtde_client_fake_server.cpp b/tests/test_rtde_client_fake_server.cpp index edf9bb7ea..38f4c6fd1 100644 --- a/tests/test_rtde_client_fake_server.cpp +++ b/tests/test_rtde_client_fake_server.cpp @@ -235,6 +235,79 @@ TEST_F(RTDEClientFakeServerTest, init_fails_when_no_protocol_version_is_accepted EXPECT_EQ(requested.back(), 1) << "the client should have tried every version down to the lowest"; } +// A thrown init() used to leave the client INITIALIZING, so a later call returned true without +// talking to the robot. After the handshake is allowed to succeed, the second init() has to +// actually finish it. +TEST_F(RTDEClientFakeServerTest, init_can_be_retried_after_a_failed_handshake) +{ + server_->setHighestAcceptedProtocolVersion(0); + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)), UrException); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + + server_->setHighestAcceptedProtocolVersion(2); + ASSERT_TRUE(client_->init()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); +} + +// isRobotBooted() reads packages until the reported uptime is 40 seconds or two seconds of data +// have arrived. Every other test skips that wait; this one leaves the fake server's clock at now +// and uses a low frequency so the loop body actually runs. +TEST_F(RTDEClientFakeServerTest, init_waits_out_the_bootup_period) +{ + client_.reset(); + server_.reset(); + server_ = std::make_unique(g_FAKE_RTDE_PORT); + const double bootup_frequency = 2.0; + client_ = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, bootup_frequency); + + ASSERT_TRUE(client_->init()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); +} + +// A robot that refuses to start RTDE must not leave the client believing it is streaming. +TEST_F(RTDEClientFakeServerTest, start_is_refused_when_the_robot_rejects_it) +{ + ASSERT_TRUE(client_->init()); + server_->setAcceptStart(false); + + EXPECT_FALSE(client_->start()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); +} + +TEST_F(RTDEClientFakeServerTest, pause_is_refused_when_the_robot_rejects_it) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start()); + server_->setAcceptPause(false); + + EXPECT_FALSE(client_->pause()); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::RUNNING); +} + +// setupOutputs() retries on unexpected replies and then gives up, rather than treating a text +// message as an acknowledgement. +TEST_F(RTDEClientFakeServerTest, init_fails_when_setup_outputs_never_gets_an_acknowledgement) +{ + for (unsigned i = 0; i < rtde_interface::MAX_REQUEST_RETRIES; ++i) + { + server_->queueTextMessageBeforeSetupOutputs("not a setup-outputs reply"); + } + + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)), UrException); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); +} + +TEST_F(RTDEClientFakeServerTest, init_fails_when_setup_inputs_never_gets_an_acknowledgement) +{ + for (unsigned i = 0; i < rtde_interface::MAX_REQUEST_RETRIES; ++i) + { + server_->queueTextMessageBeforeSetupInputs("not a setup-inputs reply"); + } + + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10)), UrException); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); +} + TEST_F(RTDEClientFakeServerTest, target_frequency_defaults_to_the_maximum) { auto client = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, 0.0); diff --git a/tests/test_rtde_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp index 580a9d2c0..e598a0929 100644 --- a/tests/test_rtde_client_reconnect.cpp +++ b/tests/test_rtde_client_reconnect.cpp @@ -280,3 +280,44 @@ TEST_F(RTDEClientReconnectTest, destructor_not_blocked_by_stuck_reconnect_thread << "RTDEClient destructor blocked for " << std::chrono::duration_cast(elapsed).count() << " ms — reconnect thread was not aborted by disconnect()"; } + +// A server that accepts the TCP connection but never finishes the handshake must not be retried +// forever. After max_initialization_attempts_ the client gives up and stays uninitialized. +TEST_F(RTDEClientReconnectTest, reconnect_gives_up_when_the_handshake_keeps_failing) +{ + startServer(); + makeClient(); + ASSERT_TRUE(client_->init(0, std::chrono::milliseconds(50), 2, std::chrono::milliseconds(50))); + client_->start(); + + std::atomic keep_running{ true }; + std::thread data_consumer([this, &keep_running]() { + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + while (keep_running) + { + if (!client_->getDataPackage(data_pkg, std::chrono::milliseconds(100))) + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + } + }); + + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + server_.reset(); + EXPECT_TRUE(waitForState(rtde_interface::ClientState::UNINITIALIZED)); + + // A listener is back, so reconnect counts each failed handshake instead of waiting on connect. + startServer(); + server_->setHighestAcceptedProtocolVersion(0); + + // Two failed handshakes plus the short sleep between them, then give up. + std::this_thread::sleep_for(std::chrono::milliseconds(400)); + keep_running = false; + data_consumer.join(); + + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED) << "the client kept retrying after " + "exhausting its initialization " + "attempts"; +} From 161a4a8d05b3ce0e2bb17d5968682a146385d044 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 14:14:09 +0200 Subject: [PATCH 04/18] Fix macOS and Alpine failures in the RTDE allocation tests. Clang rejects deleting RTDEServer through unique_ptr unless the destructor is virtual, and Alpine's allocator never hits the replaced operator new from vector::resize, so the smoke test now allocates with new. --- tests/fake_rtde_server.h | 2 +- tests/test_rtde_allocations.cpp | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index ae9f4eb02..7c8bb6db2 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -17,7 +17,7 @@ class RTDEServer RTDEServer() = delete; explicit RTDEServer(const int port); - ~RTDEServer(); + virtual ~RTDEServer(); void startSendingDataPackages(); void stopSendingDataPackages(); diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index ab24bbfbf..ceee58365 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -131,14 +131,16 @@ void operator delete[](void* memory, std::size_t) noexcept #endif // Guards the tests below: if the counter stopped seeing allocations, they would pass vacuously. +// Allocate with new, not a container: on some libstdc++ / musl builds std::allocator uses malloc +// and would never hit the replaced operator new that the RTDE tests count. TEST(AllocationCounterTest, counts_allocations) { std::size_t allocations = 0; - std::vector values; { AllocationCounter counter; - values.resize(1024); + int* value = new int{ 1 }; allocations = counter.count(); + delete value; } EXPECT_GT(allocations, 0); } From b9f52d84f50f77828db4f5b9631d5aa51d548086 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 14:32:25 +0200 Subject: [PATCH 05/18] Remove the unused string alternative from the RTDE field variant so an allocating type cannot be reintroduced. --- doc/migration_notes.rst | 3 +++ include/ur_client_library/rtde/data_package.h | 15 +++++++++++---- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/doc/migration_notes.rst b/doc/migration_notes.rst index a98a9c599..5e40d3a94 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -28,6 +28,9 @@ Three consequences are worth knowing about: returns ``false`` and logs which type the robot reported for that field, matching what its documentation always promised. Code that caught that exception should check the return value instead. +- **``getData()``/``setData()`` with a ``std::string`` is now a compile error.** That alternative + was never a protocol type, so those calls used to compile and return ``false`` at runtime. Nothing + could have relied on them working. On a ``DataPackage`` that hasn't been typed yet, meaning it has neither received data nor been written to, ``getData()`` fails with an explanatory message instead of returning stale values, and diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 59a79b948..1b217c413 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -116,12 +117,18 @@ class DataPackage : public RTDEPackage /*! * \brief The type a data field can hold. * - * std::monostate is the state of a field whose type isn't decided yet, which is how a package - * constructed from a recipe alone starts out. It is also what distinguishes the fields an - * application has written from the ones it left alone. + * The typed alternatives are exactly the members of DataType. std::monostate is the state of a + * field whose type isn't decided yet, which is how a package constructed from a recipe alone + * starts out. It is also what distinguishes the fields an application has written from the ones + * it left alone. */ using _rtde_type_variant = std::variant; + vector3d_t, vector6d_t, vector6int32_t, vector6uint32_t>; + + // A data package is created before the connection exists and then retyped in place from the + // robot's acknowledgement, so no alternative may own heap memory: retyping has to stay a + // write into the variant's inline storage. + static_assert(std::is_trivially_copyable_v<_rtde_type_variant>, "An RTDE data field must not own heap memory."); DataPackage() = delete; From d24b3c7eee538248d38586aa4cda56fd21bb3213 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 28 Aug 2026 16:21:13 +0200 Subject: [PATCH 06/18] Check documented RTDE outputs against the controller and keep the allocation tests honest. The docs-vs-copy comparison never asked a robot whether it accepts the names. The allocation guard was compiled away on Alpine and aborted under ASAN, so the zero-allocation assertions could pass without measuring. The parser now always re-applies the robot's types rather than trusting isTyped() after setData(). --- doc/migration_notes.rst | 5 +- include/ur_client_library/rtde/data_package.h | 11 ++ src/rtde/rtde_parser.cpp | 15 ++- tests/resources/generate_rtde_outputs.py | 18 ++- tests/test_rtde_allocations.cpp | 125 +++++++++++++++++- tests/test_rtde_client.cpp | 75 ++++++----- tests/test_rtde_data_package.cpp | 2 +- tests/test_rtde_parser.cpp | 58 ++++++++ 8 files changed, 260 insertions(+), 49 deletions(-) diff --git a/doc/migration_notes.rst b/doc/migration_notes.rst index 5e40d3a94..51a29bddb 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -13,12 +13,13 @@ instead of from a table of field names maintained inside the library. No applica change for this: ``DataPackage`` is still constructed from a recipe, still allocates all of its storage there, and is typed by the robot's answer afterwards, which costs no memory. -Three consequences are worth knowing about: +Four consequences are worth knowing about: - **A field name the robot doesn't know is reported later.** Since the library no longer has its own list of field names, a typo is caught when the robot rejects the recipe during ``RTDEClient::init()`` rather than while constructing the ``RTDEClient``. It is still an - ``RTDEInvalidKeyException``, and ``ignore_unavailable_outputs`` still strips such fields instead. + ``RTDEInvalidKeyException``. ``ignore_unavailable_outputs`` now also strips a name no robot knows: + without a list of its own, the library cannot tell a typo from a field of a newer robot. - **A wrongly typed input field is reported when the package is sent.** ``DataPackage::setData()`` decides a field's type from the value passed to it, so it can no longer tell on its own that the robot expects something else. ``RTDEWriter::sendPackage()`` checks the package against the robot's diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 1b217c413..d08e92a5e 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -391,6 +391,17 @@ class DataPackage : public RTDEPackage }); } + /*! + * \brief Whether this package was constructed from the same field names, in the same order. + * + * Used by the parser to tell a same-length but different recipe from the one the robot + * acknowledged, so it can replace the package rather than applying types onto the wrong names. + */ + bool matchesRecipe(const std::vector& recipe) const + { + return recipe_ == recipe; + } + /*! * \brief Resets a data field to a default-constructed value of its own type. * diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index deb104d90..7e6151f2f 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -154,11 +154,18 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result DataPackage* data_package = dynamic_cast(result.get()); data_package->setProtocolVersion(protocol_version_); - if (!data_package->isTyped()) + // Always apply the types the robot reported. isTyped() is also true after setData() on every + // field, which does not mean those types came from the robot, and parseWith() would then + // interpret the payload as the wrong layout. Applying types does not allocate. + if (!data_package->matchesRecipe(recipe_)) + { + URCL_LOG_ERROR("The passed pre-allocated DataPackage does not fit the negotiated output recipe. A new " + "DataPackage will have to be allocated."); + result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + data_package = dynamic_cast(result.get()); + } + else { - // A package built from a recipe alone doesn't know its field types yet. Applying the ones - // the robot reported doesn't allocate, so this happens right here rather than by handing - // the caller a replacement package. try { data_package->initEmpty(recipe_types_); diff --git a/tests/resources/generate_rtde_outputs.py b/tests/resources/generate_rtde_outputs.py index 7d9382299..15e520590 100644 --- a/tests/resources/generate_rtde_outputs.py +++ b/tests/resources/generate_rtde_outputs.py @@ -63,9 +63,19 @@ with open(OUTPUT_PATH, "w") as output_file: output_file.writelines(outputs) -# Get outputs from official docs +# Get outputs from official docs. The page currently has the input table first and the output +# table second; if that layout changes, fail here rather than writing a recipe of the wrong names. page = pd.read_html("https://docs.universal-robots.com/tutorials/communication-protocol-tutorials/rtde-guide.html") +if len(page) < 2: + raise RuntimeError(f"Expected at least two tables on the RTDE docs page, found {len(page)}.") table = page[1] +required_columns = {"Name", "Type", "Comment"} +missing = required_columns - set(table.columns) +if missing: + raise RuntimeError( + f"The RTDE docs table at index 1 is missing columns {sorted(missing)}; " + f"got {list(table.columns)}. The page layout may have changed." + ) outputs = [] for _, row in table.iterrows(): name = row["Name"] @@ -78,5 +88,11 @@ else: outputs.append(name + "\n") +if len(outputs) <= 100: + raise RuntimeError( + f"The RTDE docs output scrape produced only {len(outputs)} field names; expected more than " + "100. The scrape likely picked the wrong table." + ) + with open(WEB_OUTPUT_PATH, "w") as web_output_file: web_output_file.writelines(outputs) diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index ceee58365..8aa0d7d5f 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -39,17 +39,24 @@ #include #include +#include #include "fake_rtde_server.h" +#include "rtde_test_helpers.h" using namespace urcl; namespace { -// Counting is per-thread: the fake server and, in the background-read case, the client's read -// thread run in the same process, and their allocations are none of this test's business. +// Counting is per-thread so the fake server's allocations are not attributed to the client. +// blocking_receive covers parse on this thread. background_receive and sending_input_data measure +// the calling thread (copy out of the queue / copy into the store buffer); parse and serialize +// themselves are pinned by the same-thread tests below. thread_local std::size_t g_allocation_count = 0; thread_local bool g_count_allocations = false; +// Stores the pointer from the guard allocation so the compiler cannot prove the new/delete pair +// is unused and omit the call to the replaced operator new (GCC's allocation DCE at -O2). +void* volatile g_allocation_sink = nullptr; constexpr int g_FAKE_RTDE_PORT = 60005; constexpr double g_RTDE_FREQUENCY = 125.0; @@ -126,25 +133,125 @@ void operator delete[](void* memory, std::size_t) noexcept std::free(memory); } +void* operator new(std::size_t size, const std::nothrow_t&) noexcept +{ + if (g_count_allocations) + { + ++g_allocation_count; + } + return std::malloc(size == 0 ? 1 : size); +} + +void* operator new[](std::size_t size, const std::nothrow_t&) noexcept +{ + return operator new(size, std::nothrow); +} + +void operator delete(void* memory, const std::nothrow_t&) noexcept +{ + std::free(memory); +} + +void operator delete[](void* memory, const std::nothrow_t&) noexcept +{ + std::free(memory); +} + +void operator delete(void* memory, std::size_t, const std::nothrow_t&) noexcept +{ + std::free(memory); +} + +void operator delete[](void* memory, std::size_t, const std::nothrow_t&) noexcept +{ + std::free(memory); +} + #if defined(__GNUC__) && !defined(__clang__) # pragma GCC diagnostic pop #endif // Guards the tests below: if the counter stopped seeing allocations, they would pass vacuously. -// Allocate with new, not a container: on some libstdc++ / musl builds std::allocator uses malloc -// and would never hit the replaced operator new that the RTDE tests count. +// Call operator new directly rather than writing `new int`: a new-expression may be omitted even +// when the pointer escapes, which is what Alpine's gcc 15 does at -O2. Allocate with operator new +// rather than a container: on some libstdc++ / musl builds std::allocator uses malloc and would +// never hit the replaced operator new that the RTDE tests count. TEST(AllocationCounterTest, counts_allocations) { std::size_t allocations = 0; { AllocationCounter counter; - int* value = new int{ 1 }; + g_allocation_sink = ::operator new(sizeof(int)); allocations = counter.count(); - delete value; + ::operator delete(g_allocation_sink); + g_allocation_sink = nullptr; } EXPECT_GT(allocations, 0); } +TEST(DataPackageAllocationTest, applying_types_does_not_allocate) +{ + test::TestableDataPackage package({ "timestamp", "actual_q" }); + const std::vector types{ "DOUBLE", "VECTOR6D" }; + + std::size_t allocations = 0; + { + AllocationCounter counter; + package.initEmpty(types); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); +} + +TEST(DataPackageAllocationTest, parsing_a_preallocated_package_does_not_allocate) +{ + 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" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + std::unique_ptr product = std::make_unique(recipe); + + std::size_t allocations = 0; + bool parsed = false; + { + AllocationCounter counter; + comm::BinParser bp(raw_data, sizeof(raw_data)); + parsed = parser.parse(bp, product); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(parsed); + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + +TEST(DataPackageAllocationTest, serializing_a_typed_package_does_not_allocate) +{ + auto package = test::typedPackage({ "speed_slider_mask" }, { "UINT32" }); + ASSERT_TRUE(package.setData("speed_slider_mask", static_cast(1))); + package.setRecipeID(1); + uint8_t buffer[4096]; + + std::size_t allocations = 0; + size_t size = 0; + { + AllocationCounter counter; + size = package.serializePackage(buffer); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_EQ(size, 8); +} + class RTDEAllocationTest : public ::testing::Test { protected: @@ -229,6 +336,9 @@ TEST_F(RTDEAllocationTest, background_receive_does_not_allocate) ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); } + // Measured on this thread: copying the latest package out of the queue. Parse happens on the + // background reader and is covered by blocking_receive and parsing_a_preallocated_package. + int received = 0; bool all_data_read = true; double timestamp = 0.0; @@ -266,6 +376,9 @@ TEST_F(RTDEAllocationTest, sending_input_data_does_not_allocate) ASSERT_TRUE(client_->getWriter().sendSpeedSlider(0.5)); } + // Measured on this thread: setData and copying into the store buffer. serializePackage runs on + // the writer thread and is covered by serializing_a_typed_package. + bool all_sent = true; std::size_t allocations = 0; { diff --git a/tests/test_rtde_client.cpp b/tests/test_rtde_client.cpp index d074ef485..eb36e57d9 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include #include "ur_client_library/comm/tcp_server.h" @@ -569,49 +570,51 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist) } #ifdef CHECK_RTDE_DOCS_RECIPE -TEST_F(RTDEClientTest, check_rtde_data_fields_match_docs) +TEST_F(RTDEClientTest, docs_output_fields_are_supported_by_the_controller) { - std::ifstream docs_file(docs_output_recipe_file_); - std::ifstream pkg_file(exhaustive_output_recipe_file_); - std::vector docs_outputs; - std::string line; - while (std::getline(docs_file, line)) + // Only the newest robot can be expected to know every documented field. + const char* env_var = std::getenv("URSIM_VERSION"); + if (env_var == nullptr || std::string(env_var) != "latest") { - docs_outputs.push_back(line); + GTEST_SKIP() << "Not running against the latest URSim version."; } - std::vector pkg_outputs; - while (std::getline(pkg_file, line)) + + // A scrape that silently produced nothing would let this pass without requesting a single field. + ASSERT_GT(rtde_interface::RTDEClient::readRecipe(docs_output_recipe_file_).size(), 100u); + + client_.reset( + new rtde_interface::RTDEClient(g_ROBOT_IP, notifier_, docs_output_recipe_file_, input_recipe_file_, 0.0, false)); + try { - pkg_outputs.push_back(line); + ASSERT_TRUE(client_->init()); } - std::sort(docs_outputs.begin(), docs_outputs.end()); - std::sort(pkg_outputs.begin(), pkg_outputs.end()); - if (!std::is_permutation(docs_outputs.begin(), docs_outputs.end(), pkg_outputs.begin(), pkg_outputs.end())) + catch (const RTDEInvalidKeyException& e) { - std::cout << "Data package output fields do not match output fields in documentation" << std::endl; - std::unordered_map diff; - std::cout << "Differences: " << std::endl; - for (auto name : docs_outputs) - { - diff[name] += 1; - } - for (auto name : pkg_outputs) + // invalid_keys holds every documented name the controller answered NOT_FOUND for, so one run + // names all of them instead of stopping at the first. + std::stringstream names; + for (std::size_t i = 0; i < e.invalid_keys.size(); ++i) { - diff[name] -= 1; - } - for (auto elem : diff) - { - if (elem.second > 0) - { - std::cout << elem.first << " exists in documentation, but not in data package dict." << std::endl; - } - if (elem.second < 0) + if (i != 0) { - std::cout << elem.first << " exists in data package dict, but not in documentation." << std::endl; + names << ", "; } + names << e.invalid_keys[i]; } - GTEST_FAIL(); + FAIL() << "The controller does not support these documented output fields: " << names.str(); } + + client_->start(); + + const std::chrono::milliseconds read_timeout{ 100 }; + 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_GT(timestamp, 0.0); + + client_->pause(); } #endif @@ -643,11 +646,13 @@ TEST_F(RTDEClientTest, check_unknown_rtde_output_variable) EXPECT_TRUE(client->init()); } - // Passing a completely unknown variable should still lead to an exception, even if unknown - // variables are ignored. + // Ignoring unavailable fields now also covers a name no robot knows: without a list of its own, + // the library cannot tell a typo from a field of a newer robot. Asking the controller about the + // documented fields is what guards the names instead. client = std::make_unique(g_ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, 0.0, true); - EXPECT_THROW(client->init(), RTDEInvalidKeyException); + EXPECT_TRUE(client->init()); + EXPECT_THAT(client->getOutputRecipe(), testing::Not(testing::Contains("unknown_rtde_variable"))); } TEST_F(RTDEClientTest, empty_input_recipe) diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index c14578a37..dfeaae323 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -449,7 +449,7 @@ TEST(rtde_data_package, untyped_package_gets_typed_by_assignment) // Applying the robot's answer to a package an application is already holding is what lets that // application allocate the package wherever it likes, including before the connection exists. -TEST(rtde_data_package, applying_types_does_not_reallocate) +TEST(rtde_data_package, applying_types_makes_the_package_usable) { std::vector recipe{ "timestamp", "actual_q" }; test::TestableDataPackage package(recipe); diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index f33157525..99ed8c30e 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -263,6 +263,64 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) EXPECT_DOUBLE_EQ(timestamp, 16412.206); } +// setData() on every field makes isTyped() true, but those types did not come from the robot. +// The parser has to re-apply the acknowledged types rather than parse the payload as integers. +TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_retyped_from_the_robot) +{ + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + + auto package = std::make_unique(recipe); + ASSERT_TRUE(package->setData("timestamp", static_cast(1))); + ASSERT_TRUE(package->setData("target_speed_fraction", static_cast(2))); + + std::unique_ptr product = std::move(package); + const rtde_interface::RTDEPackage* package_address = product.get(); + + ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); + + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + +// A same-length recipe with different field names must not have the robot's types applied onto it +// in order; the parser replaces the package instead. +TEST(rtde_parser, pre_allocated_package_with_a_different_recipe_is_replaced) +{ + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + + std::unique_ptr product = + std::make_unique(std::vector{ "foo", "bar" }); + const rtde_interface::RTDEPackage* package_address = product.get(); + + ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_NE(product.get(), package_address); + + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + TEST(rtde_parser, untyped_pre_allocated_data_package_takes_protocol_version_1) { // Same payload as data_package, but without the recipe-id byte that only version 2 uses. From c1c9dd3af46c7445067404b1af1f83eaeb4649f7 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 31 Aug 2026 07:31:22 +0200 Subject: [PATCH 07/18] Handle batched RTDE packages in the fake server so a pause after a write burst is not dropped. --- tests/fake_rtde_server.cpp | 45 ++++++++++++++++++++++---- tests/fake_rtde_server.h | 4 +++ tests/test_rtde_client_fake_server.cpp | 37 +++++++++++++++++++++ 3 files changed, 80 insertions(+), 6 deletions(-) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index f2ea47703..4785866a5 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -571,21 +571,54 @@ void RTDEServer::sendTextMessage(const socket_t filedescriptor, const std::strin void RTDEServer::connectionCallback(const socket_t filedescriptor) { client_socket_ = filedescriptor; + receive_buffer_.clear(); URCL_LOG_INFO("Client connected to RTDE server on FD %d", filedescriptor); } void RTDEServer::disconnectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Client disconnected from RTDE server on FD %d", filedescriptor); + receive_buffer_.clear(); stopSendingDataPackages(); } -void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, char* buffer, int nbytesrecv) +void RTDEServer::messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv) { - comm::BinParser bp(reinterpret_cast(buffer), nbytesrecv); - rtde_interface::PackageHeader::_package_size_type size; - rtde_interface::PackageType type; - bp.parse(size); - bp.parse(type); + // TCPServer hands over whatever one recv() returned. That can be several RTDE packages, or + // only the start of one. Keep leftovers so a later read can finish a package, and dispatch + // each complete package on its own rather than dropping everything after the first header. + receive_buffer_.insert(receive_buffer_.end(), reinterpret_cast(buffer), + reinterpret_cast(buffer) + nbytesrecv); + + constexpr size_t header_size = + sizeof(rtde_interface::PackageHeader::_package_size_type) + sizeof(rtde_interface::PackageType); + size_t offset = 0; + while (receive_buffer_.size() - offset >= sizeof(rtde_interface::PackageHeader::_package_size_type)) + { + const size_t package_size = rtde_interface::PackageHeader::getPackageLength(receive_buffer_.data() + offset); + if (package_size < header_size) + { + URCL_LOG_ERROR("Received an RTDE package shorter than the 3-byte header (%zu bytes). Dropping the buffer.", + package_size); + receive_buffer_.clear(); + return; + } + if (receive_buffer_.size() - offset < package_size) + { + break; + } + comm::BinParser bp(receive_buffer_.data() + offset, package_size); + rtde_interface::PackageHeader::_package_size_type size; + rtde_interface::PackageType type; + bp.parse(size); + bp.parse(type); + handlePackage(filedescriptor, type, bp); + offset += package_size; + } + receive_buffer_.erase(receive_buffer_.begin(), receive_buffer_.begin() + static_cast(offset)); +} + +void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::PackageType type, comm::BinParser& bp) +{ switch (type) { case rtde_interface::PackageType::RTDE_REQUEST_PROTOCOL_VERSION: diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index 7c8bb6db2..bd14a4c3d 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -4,6 +4,7 @@ #include #include +#include "ur_client_library/comm/bin_parser.h" #include "ur_client_library/comm/tcp_server.h" #include "ur_client_library/rtde/rtde_package.h" #include "ur_client_library/rtde/rtde_parser.h" @@ -86,6 +87,8 @@ class RTDEServer virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv); + void handlePackage(const socket_t filedescriptor, rtde_interface::PackageType type, comm::BinParser& bp); + void sendDataLoop(); std::atomic send_loop_running_; @@ -94,6 +97,7 @@ class RTDEServer std::chrono::steady_clock::time_point start_time_; socket_t client_socket_; + std::vector receive_buffer_; void actOnInput(); diff --git a/tests/test_rtde_client_fake_server.cpp b/tests/test_rtde_client_fake_server.cpp index 38f4c6fd1..97f21c24e 100644 --- a/tests/test_rtde_client_fake_server.cpp +++ b/tests/test_rtde_client_fake_server.cpp @@ -34,9 +34,14 @@ #include +#include +#include + +#include #include #include #include +#include #include "fake_rtde_server.h" @@ -223,6 +228,38 @@ TEST_F(RTDEClientFakeServerTest, protocol_version_is_lowered_when_the_robot_refu EXPECT_EQ(requested[1], 1) << "the client should fall back to the next version down"; } +// TCP can deliver several RTDE packages in one recv(). The fake server used to handle only the +// first and drop the rest, which is what made a pause request vanish after a burst of input data. +TEST_F(RTDEClientFakeServerTest, two_requests_in_one_write_are_both_recorded) +{ + comm::URStream stream("localhost", g_FAKE_RTDE_PORT); + ASSERT_TRUE(stream.connect(1, std::chrono::milliseconds(100))); + + uint8_t buffer[16]; + const size_t first = rtde_interface::RequestProtocolVersionRequest::generateSerializedRequest(buffer, 2); + const size_t second = rtde_interface::RequestProtocolVersionRequest::generateSerializedRequest(buffer + first, 1); + const size_t total = first + second; + size_t written = 0; + ASSERT_TRUE(stream.write(buffer, total, written)); + ASSERT_EQ(written, total); + + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(1); + std::vector requested; + while (std::chrono::steady_clock::now() < deadline) + { + requested = server_->requestedProtocolVersions(); + if (requested.size() >= 2) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + ASSERT_EQ(requested.size(), 2u); + EXPECT_EQ(requested[0], 2); + EXPECT_EQ(requested[1], 1); +} + // If no version is acceptable the handshake has to fail, not spin. TEST_F(RTDEClientFakeServerTest, init_fails_when_no_protocol_version_is_accepted) { From 5f33f8211f9bced57f22668a3f5a15512b7d999c Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Tue, 1 Sep 2026 10:57:57 +0200 Subject: [PATCH 08/18] Let the bitset getData reuse the generic lookup instead of duplicating it. --- include/ur_client_library/rtde/data_package.h | 16 +++------------- 1 file 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 d08e92a5e..ad64004fd 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -270,22 +270,12 @@ class DataPackage : public RTDEPackage bool getData(const std::string_view name, std::bitset& val) const { static_assert(sizeof(T) * 8 >= N, "Bitset is too large for underlying variable"); - - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it == data_.end()) + T recipe_type; + if (!getData(name, recipe_type)) { return false; } - const T* value = std::get_if(&it->second); - if (value == nullptr) - { - reportReadFailure(name, it->second); - return false; - } - val = std::bitset(*value); + val = std::bitset(recipe_type); return true; } From fccce1d5be9c2637ce58f983fa523bc2d3b6f45f Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Wed, 2 Sep 2026 15:52:15 +0200 Subject: [PATCH 09/18] Make the RTDE send and receive paths constant-cost per cycle. Each data package now carries an FNV-1a hash of its field names and of each field's type, and stores its values contiguously. sendPackage() is therefore a hash compare plus a memcpy of the whole recipe, rather than a per-field walk that allocated a fresh package every cycle, and copying became a deterministic full overwrite instead of merging in only the fields the caller had set. The parser recognises a pre-allocated package that already has the negotiated layout and parses straight into it, allocating a correctly typed one only otherwise. Looking a field up by name goes through a name-to-index map instead of a linear scan of the recipe. --- include/ur_client_library/rtde/data_package.h | 186 +++++++++----- include/ur_client_library/rtde/rtde_parser.h | 6 +- include/ur_client_library/rtde/rtde_writer.h | 21 +- src/rtde/data_package.cpp | 237 ++++++++++------- src/rtde/rtde_client.cpp | 9 +- src/rtde/rtde_parser.cpp | 53 ++-- src/rtde/rtde_writer.cpp | 43 ++-- tests/fake_rtde_server.cpp | 17 +- tests/fake_rtde_server.h | 1 + tests/rtde_test_helpers.h | 14 +- tests/test_rtde_allocations.cpp | 10 +- tests/test_rtde_data_package.cpp | 240 +++++++++++++++--- tests/test_rtde_parser.cpp | 48 +++- tests/test_rtde_writer.cpp | 27 +- 14 files changed, 615 insertions(+), 297 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index ad64004fd..652aaa411 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -29,12 +29,14 @@ #ifndef UR_CLIENT_LIBRARY_DATA_PACKAGE_H_INCLUDED #define UR_CLIENT_LIBRARY_DATA_PACKAGE_H_INCLUDED -#include #include +#include +#include #include #include #include #include +#include #include #include @@ -46,8 +48,6 @@ namespace urcl { namespace rtde_interface { -class RTDEWriter; - /*! * \brief Possible values for the runtime state */ @@ -119,8 +119,7 @@ class DataPackage : public RTDEPackage * * The typed alternatives are exactly the members of DataType. std::monostate is the state of a * field whose type isn't decided yet, which is how a package constructed from a recipe alone - * starts out. It is also what distinguishes the fields an application has written from the ones - * it left alone. + * starts out. */ using _rtde_type_variant = std::variant; @@ -135,10 +134,15 @@ class DataPackage : public RTDEPackage DataPackage(const DataPackage& other) : RTDEPackage(PackageType::RTDE_DATA_PACKAGE) , recipe_id_(other.recipe_id_) - , data_(other.data_) , recipe_(other.recipe_) + , values_(other.values_) + , zeros_(other.zeros_) , protocol_version_(other.protocol_version_) + , recipe_hash_(other.recipe_hash_) + , layout_hash_(other.layout_hash_) + , fully_typed_(other.fully_typed_) { + rebuildFieldIndex(); } /*! @@ -149,9 +153,19 @@ class DataPackage : public RTDEPackage */ DataPackage& operator=(const DataPackage& other) { - this->data_ = other.data_; - this->recipe_ = other.recipe_; + // The name-to-index map holds string_views into recipe_. Replacing recipe_ would dangle those + // views and would allocate, so a same-recipe assignment (the receive path) leaves both alone. + if (recipe_hash_ != other.recipe_hash_ || recipe_.size() != other.recipe_.size()) + { + this->recipe_ = other.recipe_; + this->recipe_hash_ = other.recipe_hash_; + rebuildFieldIndex(); + } + this->values_ = other.values_; + this->zeros_ = other.zeros_; this->protocol_version_ = other.protocol_version_; + this->layout_hash_ = other.layout_hash_; + this->fully_typed_ = other.fully_typed_; return *this; } @@ -217,6 +231,9 @@ class DataPackage : public RTDEPackage /*! * \brief Serializes the package. * + * Version 2 data packages start with a recipe-id byte; version 1 packages do not. The writer + * records the negotiated version with setProtocolVersion() before serializing. + * * \param buffer Buffer to fill with the serialization * * \returns The total size of the serialized package @@ -237,18 +254,15 @@ class DataPackage : public RTDEPackage template bool getData(const std::string_view name, T& val) const { - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it == data_.end()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { return false; } - const T* value = std::get_if(&it->second); + const T* value = std::get_if(&values_[*index]); if (value == nullptr) { - reportReadFailure(name, it->second); + reportReadFailure(recipe_[*index], values_[*index]); return false; } val = *value; @@ -297,15 +311,13 @@ class DataPackage : public RTDEPackage template bool setData(const std::string_view name, const T& val) { - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it == data_.end()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { return false; } - if (!std::holds_alternative(it->second) && !std::holds_alternative(it->second)) + _rtde_type_variant& field = values_[*index]; + if (!std::holds_alternative(field) && !std::holds_alternative(field)) { // TODO: It might be better to replace the return type by void and use exceptions for the // error case. @@ -313,7 +325,13 @@ class DataPackage : public RTDEPackage static_cast(name.size()), name.data()); return false; } - it->second = val; + const bool type_changed = std::holds_alternative(field); + field = val; + if (type_changed) + { + zeros_[*index] = T(); + updateLayoutHash(); + } return true; } @@ -327,13 +345,16 @@ class DataPackage : public RTDEPackage recipe_id_ = recipe_id; } -protected: - // Applying the robot's setup acknowledgement to a package is the library's job: the parser does - // it on the way in, the writer when the input recipe is acknowledged, and the client for the - // package it reads into. An application never has the types to pass here. - friend class RTDEWriter; - friend class RTDEClient; - friend class RTDEParser; + /*! + * \brief Records the RTDE protocol version this package will serialize. + * + * Version 2 data packages start with a recipe-id byte; version 1 packages do not. The + * constructor defaults to version 2. + */ + void setProtocolVersion(const uint16_t protocol_version) + { + protocol_version_ = protocol_version; + } /*! * \brief Applies the data types reported by the robot, resetting all values to zero. @@ -346,21 +367,35 @@ class DataPackage : public RTDEPackage * * \throws UrException if the number of types doesn't match the recipe or if a type is unknown */ - void initEmpty(const std::vector& types); + void setTypes(const std::vector& types); /*! - * \brief Records the RTDE protocol version this package will parse and serialize. + * \brief Overwrites every field of this package with the corresponding field of \p other. * - * Version 2 data packages start with a recipe-id byte; version 1 packages do not. The - * constructor defaults to version 2, so this has to be called when the handshake falls back - * to version 1. Assignment of a uint16_t does not allocate. + * This package must already be typed. \p other has to carry the same field names and the same + * type on every field, which is what a package has after the robot's acknowledgement or after + * every subscribed field has been written. Recipe id and protocol version are left untouched. + * + * The success path is a layout-hash compare and a memcpy of the value array. The hashes are a + * 64-bit identity of the field names and each field's variant index; a collision would skip a + * validation that should have failed, which is accepted for this path. + * + * \param other The package to copy from + * + * \returns True on success, false if this package is untyped, if \p other was built from a + * different recipe or if a field in \p other has a different type */ - void setProtocolVersion(const uint16_t protocol_version) - { - protocol_version_ = protocol_version; - } + bool copyFrom(const DataPackage& other); + + /*! + * \brief Resets a data field to a default-constructed value of its own type. + * + * \param name The string identifier for the data field as used in the documentation. + * + * \returns True on success, false if the field cannot be found inside the package. + */ + bool resetData(const std::string_view name); -private: /*! * \brief Whether every field of this package has a data type. * @@ -368,58 +403,70 @@ class DataPackage : public RTDEPackage * acknowledgement has been applied to it or setData() has been used to write to every field. An * untyped package cannot be parsed into or serialized, and getData() fails on it. * - * There is no separate flag for this: a field whose type is undecided holds a std::monostate, so - * the fields themselves are the answer. The scan is over recipe-many variant tags and costs far - * less than the parse it guards. - * * \returns True if the package carries type information for all of its fields */ bool isTyped() const { - return std::none_of(data_.begin(), data_.end(), [](const std::pair& field) { - return std::holds_alternative(field.second); - }); + return fully_typed_; } /*! - * \brief Whether this package was constructed from the same field names, in the same order. + * \brief FNV-1a identity of this package's field names, in order. * - * Used by the parser to tell a same-length but different recipe from the one the robot - * acknowledged, so it can replace the package rather than applying types onto the wrong names. + * Not sent on the wire. Used together with layoutHash() to identify a recipe without comparing + * field-name strings. */ - bool matchesRecipe(const std::vector& recipe) const + uint64_t recipeHash() const { - return recipe_ == recipe; + return recipe_hash_; } /*! - * \brief Resets a data field to a default-constructed value of its own type. + * \brief FNV-1a identity of this package's field names and each field's current variant index. * - * \param name The string identifier for the data field as used in the documentation. - * - * \returns True on success, false if the field cannot be found inside the package. + * Not sent on the wire. Combined from the recipe hash and the type of every field, so it changes + * when a field first acquires a type and when setTypes() is applied, and does not change when a + * value is overwritten, reset or parsed. */ - bool resetData(const std::string_view name); + uint64_t layoutHash() const + { + return layout_hash_; + } /*! - * \brief Whether every set field in \p other can be copied into this package. + * \brief The layout hash a package would have after applying \p types to \p recipe. + * + * Used by the parser to recognise a package that already carries the negotiated output layout. * - * Does not modify this package. Used to validate a source before clearing the destination. + * \param recipe Field names, in order + * \param types Data type names as reported by the robot, in the same order as \p recipe + * + * \throws UrException if the number of types doesn't match the recipe or if a type is unknown */ - bool canCopySetFieldsFrom(const DataPackage& other) const; + static uint64_t layoutHashFor(const std::vector& recipe, const std::vector& types); +private: /*! - * \brief Copies the fields that \p other has values for into this package. + * \brief Allocates one slot per recipe field, with the type left undecided. + */ + void initStorage(); + + /*! + * \brief Recomputes layout_hash_ and fully_typed_ from the current values. + */ + void updateLayoutHash(); + + /*! + * \brief Rebuilds the name-to-index map from recipe_. * - * Fields \p other hasn't written are left untouched. The source must already have been checked - * with canCopySetFieldsFrom(); this only writes the matching values. + * The keys are string_views into recipe_, so this must run after recipe_ is in its final place. */ - bool copySetFieldsFrom(const DataPackage& other); + void rebuildFieldIndex(); /*! - * \brief Allocates one slot per recipe field, with the type left undecided. + * \brief The recipe index of \p name, or empty if the name is not in this package. */ - void initStorage(); + std::optional fieldIndex(const std::string_view name) const; /*! * \brief Logs why reading \p field didn't produce the requested type. @@ -427,9 +474,14 @@ class DataPackage : public RTDEPackage static void reportReadFailure(const std::string_view name, const _rtde_type_variant& field); uint8_t recipe_id_ = 0; - std::vector> data_; std::vector recipe_; - uint16_t protocol_version_; + std::unordered_map field_index_; + std::vector<_rtde_type_variant> values_; + std::vector<_rtde_type_variant> zeros_; + uint16_t protocol_version_ = 2; + uint64_t recipe_hash_ = 0; + uint64_t layout_hash_ = 0; + bool fully_typed_ = false; }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index d4326f731..51a34240b 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -113,15 +113,17 @@ class RTDEParser : public comm::Parser void setRecipeTypes(const std::vector& types) { recipe_types_ = types; + typed_layout_hash_ = DataPackage::layoutHashFor(recipe_, recipe_types_); } private: static std::unique_ptr makeTypedDataPackage(const std::vector& recipe, - const std::vector& types, - const uint16_t protocol_version); + const std::vector& types); + bool parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const; std::vector recipe_; std::vector recipe_types_; + uint64_t typed_layout_hash_ = 0; bool recipeTypesKnown() const; PackageType getPackageTypeFromHeader(comm::BinParser& bp) const; RTDEPackage* createNewPackageFromType(PackageType type) const; diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 59c922684..4f95cc4ed 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -95,10 +95,9 @@ class RTDEWriter * 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. * - * Only the fields \p package has values for are taken over; the rest of the input recipe is sent - * as zeros. The values are checked against the data types the robot reported for the input - * recipe, so a field written with the wrong type is reported here rather than silently corrupting - * the package. + * Every field of \p package is copied into the send buffer. The package has to carry the same + * field names and types as the input recipe the robot acknowledged, so a field written with the + * wrong type is reported here rather than silently corrupting the package. * * \param package The package to send, constructed from the client's input recipe * @@ -198,7 +197,7 @@ class RTDEWriter bool sendExternalForceTorque(const vector6d_t& external_force_torque); protected: - // Relays the data types from the robot's setup acknowledgement, which only the client receives. + // Relays the data types and the protocol version from the handshake, which only the client sees. friend class RTDEClient; /*! @@ -208,11 +207,18 @@ class RTDEWriter * passed to sendPackage() are checked. * * \param types The data types of the input recipe's fields, in the same order as the recipe - * \param protocol_version The RTDE protocol version negotiated with the robot * * \throws UrException if the number of types doesn't match the recipe or if a type is unknown */ - void setRecipeTypes(const std::vector& types, uint16_t protocol_version = 2); + void setRecipeTypes(const std::vector& types); + + /*! + * \brief Records the RTDE protocol version negotiated with the robot. + * + * Version 2 data packages start with a recipe-id byte; version 1 packages do not. Defaults to + * version 2. The client sets this after protocol negotiation. + */ + void setProtocolVersion(uint16_t protocol_version); private: void resetMasks(const std::shared_ptr& buffer); @@ -222,6 +228,7 @@ class RTDEWriter comm::URStream* stream_; std::vector recipe_; uint8_t recipe_id_; + uint16_t protocol_version_ = 2; std::shared_ptr data_buffer0_; std::shared_ptr data_buffer1_; std::shared_ptr current_store_buffer_; diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 5cf0efd36..e7522eaff 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -28,7 +28,8 @@ #include "ur_client_library/rtde/data_package.h" -#include +#include +#include #include "ur_client_library/exceptions.h" @@ -70,6 +71,50 @@ constexpr struct { DataType::VECTOR6UINT32, "VECTOR6UINT32" }, }; +constexpr uint64_t g_FNV_OFFSET_BASIS = 14695981039346656037ULL; +constexpr uint64_t g_FNV_PRIME = 1099511628211ULL; + +uint64_t fnv1a(uint64_t hash, const uint8_t* data, const size_t length) +{ + for (size_t i = 0; i < length; ++i) + { + hash ^= data[i]; + hash *= g_FNV_PRIME; + } + return hash; +} + +uint64_t fnv1aByte(uint64_t hash, const uint8_t value) +{ + hash ^= value; + hash *= g_FNV_PRIME; + return hash; +} + +uint64_t hashRecipe(const std::vector& recipe) +{ + uint64_t hash = g_FNV_OFFSET_BASIS; + const uint64_t count = recipe.size(); + hash = fnv1a(hash, reinterpret_cast(&count), sizeof(count)); + for (const auto& name : recipe) + { + hash = fnv1a(hash, reinterpret_cast(name.data()), name.size()); + // A separator so that "ab"+"c" and "a"+"bc" cannot produce the same digest. + hash = fnv1aByte(hash, 0); + } + return hash; +} + +uint64_t hashLayout(const uint64_t recipe_hash, const std::vector& values) +{ + uint64_t hash = recipe_hash; + for (const auto& value : values) + { + hash = fnv1aByte(hash, static_cast(value.index())); + } + return hash; +} + /*! * \brief The data type a field holds, or an empty optional if it has none yet. */ @@ -185,6 +230,16 @@ DataPackage::_rtde_type_variant variantFromTypeName(const std::string_view type_ "DOUBLE, VECTOR3D, VECTOR6D, VECTOR6INT32 or VECTOR6UINT32."; throw UrException(ss.str()); } + +void copyValues(std::vector& destination, + const std::vector& source) +{ + if (destination.empty()) + { + return; + } + std::memcpy(destination.data(), source.data(), destination.size() * sizeof(DataPackage::_rtde_type_variant)); +} } // namespace std::string toString(const DataType type) @@ -199,17 +254,34 @@ std::string toString(const DataType type) throw UrException("Unhandled RTDE data type."); } +void rtde_interface::DataPackage::rebuildFieldIndex() +{ + field_index_.clear(); + field_index_.reserve(recipe_.size()); + for (size_t i = 0; i < recipe_.size(); ++i) + { + field_index_.emplace(recipe_[i], i); + } +} + +std::optional rtde_interface::DataPackage::fieldIndex(const std::string_view name) const +{ + const auto it = field_index_.find(name); + if (it == field_index_.end()) + { + return std::nullopt; + } + return it->second; +} + std::optional rtde_interface::DataPackage::getDataType(const std::string_view name) const { - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it == data_.end()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { return std::nullopt; } - return typeOf(it->second); + return typeOf(values_[*index]); } void rtde_interface::DataPackage::reportReadFailure(const std::string_view name, const _rtde_type_variant& field) @@ -229,43 +301,79 @@ void rtde_interface::DataPackage::reportReadFailure(const std::string_view name, void rtde_interface::DataPackage::initStorage() { - data_.resize(recipe_.size()); - for (size_t i = 0; i < recipe_.size(); ++i) + values_.assign(recipe_.size(), std::monostate()); + zeros_.assign(recipe_.size(), std::monostate()); + rebuildFieldIndex(); + recipe_hash_ = hashRecipe(recipe_); + updateLayoutHash(); +} + +void rtde_interface::DataPackage::updateLayoutHash() +{ + layout_hash_ = hashLayout(recipe_hash_, values_); + fully_typed_ = std::none_of(values_.begin(), values_.end(), [](const _rtde_type_variant& field) { + return std::holds_alternative(field); + }); +} + +uint64_t rtde_interface::DataPackage::layoutHashFor(const std::vector& recipe, + const std::vector& types) +{ + if (types.size() != recipe.size()) { - data_[i].first = recipe_[i]; - data_[i].second = std::monostate(); + std::stringstream ss; + ss << "Cannot compute the layout hash of an RTDE data package: got " << types.size() + << " data types for a recipe with " << recipe.size() << " fields."; + throw UrException(ss.str()); + } + + uint64_t hash = hashRecipe(recipe); + for (const auto& type_name : types) + { + hash = fnv1aByte(hash, static_cast(variantFromTypeName(type_name).index())); } + return hash; } -void rtde_interface::DataPackage::initEmpty(const std::vector& types) +void rtde_interface::DataPackage::setTypes(const std::vector& types) { if (types.size() != recipe_.size()) { std::stringstream ss; - ss << "Cannot initialize an RTDE data package: got " << types.size() << " data types for a recipe with " + ss << "Cannot set the data types of an RTDE data package: got " << types.size() << " data types for a recipe with " << recipe_.size() << " fields."; throw UrException(ss.str()); } - // The storage was allocated by the constructor and every RTDE type lives inline in the variant, - // so deciding the types here cannot allocate. That is what makes it safe to type a package that - // an application is already holding, in the middle of a real-time loop. - if (data_.size() != recipe_.size()) - { - initStorage(); - } for (size_t i = 0; i < recipe_.size(); ++i) { - data_[i].second = variantFromTypeName(types[i]); + values_[i] = variantFromTypeName(types[i]); + zeros_[i] = values_[i]; } + updateLayoutHash(); } void rtde_interface::DataPackage::initEmpty() { - for (auto& item : data_) + copyValues(values_, zeros_); +} + +bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) +{ + if (!isTyped()) { - std::visit([](auto&& arg) { arg = std::decay_t(); }, item.second); + URCL_LOG_ERROR("Cannot copy into an RTDE data package before the data types of its recipe are known. Those are " + "reported by the robot during the RTDE handshake."); + return false; + } + if (layout_hash_ != other.layout_hash_ || values_.size() != other.values_.size()) + { + URCL_LOG_ERROR("Cannot copy from an RTDE data package whose layout does not match this one."); + return false; } + + copyValues(values_, other.values_); + return true; } bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) @@ -277,10 +385,6 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) return false; } - if (protocol_version_ == 2) - { - bp.parse(recipe_id_); - } for (size_t i = 0; i < recipe_.size(); ++i) { std::visit( @@ -290,7 +394,7 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) bp.parse(arg); } }, - data_[i].second); + values_[i]); } return true; } @@ -298,12 +402,12 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) std::string rtde_interface::DataPackage::toString() const { std::stringstream ss; - for (auto& item : data_) + for (size_t i = 0; i < recipe_.size(); ++i) { - ss << item.first << ": "; - if (std::holds_alternative(item.second)) + ss << recipe_[i] << ": "; + if (std::holds_alternative(values_[i])) { - ss << int(std::get(item.second)); + ss << int(std::get(values_[i])); } else { @@ -318,7 +422,7 @@ std::string rtde_interface::DataPackage::toString() const ss << arg; } }, - item.second); + values_[i]); } ss << std::endl; } @@ -340,7 +444,7 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) payload_size += sizeof(recipe_id_); } - for (auto& item : data_) + for (const auto& value : values_) { payload_size += std::visit( [](auto&& arg) -> uint16_t { @@ -353,7 +457,7 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) return sizeof(arg); } }, - item.second); + value); } size_t size = 0; size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size); @@ -361,7 +465,7 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) { size += comm::PackageSerializer::serialize(buffer + size, recipe_id_); } - for (size_t i = 0; i < data_.size(); ++i) + for (size_t i = 0; i < values_.size(); ++i) { size += std::visit( [&buffer, &size](auto&& arg) -> size_t { @@ -374,7 +478,7 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) return comm::PackageSerializer::serialize(buffer + size, arg); } }, - data_[i].second); + values_[i]); } return size; @@ -382,67 +486,14 @@ size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) bool rtde_interface::DataPackage::resetData(const std::string_view name) { - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it == data_.end()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { return false; } - std::visit([](auto&& arg) { arg = std::decay_t(); }, it->second); + values_[*index] = zeros_[*index]; return true; } -bool rtde_interface::DataPackage::canCopySetFieldsFrom(const DataPackage& other) const -{ - bool all_compatible = true; - for (const auto& source : other.data_) - { - if (std::holds_alternative(source.second)) - { - continue; - } - - const auto destination = - std::find_if(data_.begin(), data_.end(), [&source](const std::pair& element) { - return element.first == source.first; - }); - if (destination == data_.end()) - { - URCL_LOG_ERROR("The data field '%s' is not part of the recipe the robot acknowledged.", source.first.c_str()); - all_compatible = false; - continue; - } - if (source.second.index() != destination->second.index()) - { - URCL_LOG_ERROR("The value passed for the data field '%s' is of type %s, but the robot reports that field as %s.", - source.first.c_str(), typeNameOf(source.second).c_str(), typeNameOf(destination->second).c_str()); - all_compatible = false; - } - } - return all_compatible; -} - -bool rtde_interface::DataPackage::copySetFieldsFrom(const DataPackage& other) -{ - for (const auto& source : other.data_) - { - if (std::holds_alternative(source.second)) - { - continue; - } - - const auto destination = - std::find_if(data_.begin(), data_.end(), [&source](const std::pair& element) { - return element.first == source.first; - }); - if (destination != data_.end()) - { - destination->second = source.second; - } - } - return true; -} } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 1e1a45d07..5d6f3dfc4 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -233,6 +233,7 @@ uint16_t RTDEClient::negotiateProtocolVersion() { URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); parser_.setProtocolVersion(protocol_version); + writer_.setProtocolVersion(protocol_version); return protocol_version; } break; @@ -342,9 +343,10 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) output_recipe_.assign(new_recipe.begin(), new_recipe.end()); // The data types of the new recipe are unknown until the robot acknowledges it again, at which // point setupOutputs() applies them to this package without allocating. - preallocated_data_pkg_ = DataPackage(output_recipe_, protocol_version_); + preallocated_data_pkg_ = DataPackage(output_recipe_); parser_ = RTDEParser(output_recipe_); + parser_.setProtocolVersion(protocol_version_); prod_ = std::make_unique>(stream_, parser_); } @@ -449,8 +451,7 @@ bool RTDEClient::setupOutputs() // storage itself already exists, so this doesn't allocate and neither does the receive path // from here on. parser_.setRecipeTypes(variable_types); - preallocated_data_pkg_.setProtocolVersion(protocol_version_); - preallocated_data_pkg_.initEmpty(variable_types); + preallocated_data_pkg_.setTypes(variable_types); return true; } } @@ -518,7 +519,7 @@ bool RTDEClient::setupInputs() throw RTDEInputConflictException(input_recipe_[i]); } } - writer_.setRecipeTypes(variable_types, protocol_version_); + writer_.setRecipeTypes(variable_types); writer_.init(tmp_input->input_recipe_id_); return true; diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 7e6151f2f..3b562a8ff 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -30,14 +30,24 @@ namespace rtde_interface // A package allocates its storage from the recipe and learns its field types from the robot's // acknowledgement afterwards, which costs no memory. std::unique_ptr RTDEParser::makeTypedDataPackage(const std::vector& recipe, - const std::vector& types, - const uint16_t protocol_version) + const std::vector& types) { - auto package = std::make_unique(recipe, protocol_version); - package->initEmpty(types); + auto package = std::make_unique(recipe); + package->setTypes(types); return package; } +bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const +{ + if (protocol_version_ == 2) + { + uint8_t recipe_id = 0; + bp.parse(recipe_id); + package.setRecipeID(recipe_id); + } + return package.parseWith(bp); +} + bool RTDEParser::recipeTypesKnown() const { if (recipe_types_.size() == recipe_.size()) @@ -80,9 +90,9 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + std::unique_ptr package = makeTypedDataPackage(recipe_, recipe_types_); - if (!package->parseWith(bp)) + if (!parseDataPackagePayload(bp, *package)) { URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; @@ -149,38 +159,19 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result "a DataPackage would be sent.", result->getType()); } - result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + result = makeTypedDataPackage(recipe_, recipe_types_); } DataPackage* data_package = dynamic_cast(result.get()); - data_package->setProtocolVersion(protocol_version_); - // Always apply the types the robot reported. isTyped() is also true after setData() on every - // field, which does not mean those types came from the robot, and parseWith() would then - // interpret the payload as the wrong layout. Applying types does not allocate. - if (!data_package->matchesRecipe(recipe_)) + if (data_package->layoutHash() != typed_layout_hash_) { - URCL_LOG_ERROR("The passed pre-allocated DataPackage does not fit the negotiated output recipe. A new " - "DataPackage will have to be allocated."); - result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); + URCL_LOG_WARN("The passed pre-allocated DataPackage does not have the negotiated output layout. A new " + "DataPackage will have to be allocated."); + result = makeTypedDataPackage(recipe_, recipe_types_); data_package = dynamic_cast(result.get()); } - else - { - try - { - data_package->initEmpty(recipe_types_); - } - catch (const UrException& e) - { - URCL_LOG_ERROR("The passed pre-allocated DataPackage does not fit the negotiated output recipe (%s). A new " - "DataPackage will have to be allocated.", - e.what()); - result = makeTypedDataPackage(recipe_, recipe_types_, protocol_version_); - data_package = dynamic_cast(result.get()); - } - } - if (!data_package->parseWith(bp)) + if (!parseDataPackagePayload(bp, *data_package)) { URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index b1339d824..b125ee386 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -28,7 +28,6 @@ #include "ur_client_library/rtde/rtde_writer.h" #include -#include "ur_client_library/helpers.h" #include "ur_client_library/log.h" namespace urcl @@ -70,7 +69,7 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) used_masks_.clear(); for (const auto& field : recipe) { - if (field.size() >= 5 && field.substr(field.size() - 5) == "_mask") + if (field.size() >= 5 && field.compare(field.size() - 5, 5, "_mask") == 0) { used_masks_.push_back(field); } @@ -80,18 +79,32 @@ void RTDEWriter::setInputRecipe(const std::vector& recipe) // allocating again. data_buffer0_ = std::make_shared(recipe_); data_buffer1_ = std::make_shared(recipe_); + data_buffer0_->setProtocolVersion(protocol_version_); + data_buffer1_->setProtocolVersion(protocol_version_); current_store_buffer_ = data_buffer0_; current_send_buffer_ = data_buffer1_; } -void RTDEWriter::setRecipeTypes(const std::vector& types, uint16_t protocol_version) +void RTDEWriter::setProtocolVersion(uint16_t protocol_version) { std::lock_guard lock_guard(store_mutex_); - data_buffer0_->setProtocolVersion(protocol_version); - data_buffer1_->setProtocolVersion(protocol_version); - data_buffer0_->initEmpty(types); - data_buffer1_->initEmpty(types); + protocol_version_ = protocol_version; + if (data_buffer0_ != nullptr) + { + data_buffer0_->setProtocolVersion(protocol_version); + } + if (data_buffer1_ != nullptr) + { + data_buffer1_->setProtocolVersion(protocol_version); + } +} + +void RTDEWriter::setRecipeTypes(const std::vector& types) +{ + std::lock_guard lock_guard(store_mutex_); + data_buffer0_->setTypes(types); + data_buffer1_->setTypes(types); } void RTDEWriter::init(uint8_t recipe_id) @@ -158,24 +171,10 @@ void RTDEWriter::stop() bool RTDEWriter::sendPackage(const DataPackage& package) { std::lock_guard guard(store_mutex_); - if (!current_store_buffer_->isTyped()) - { - URCL_LOG_ERROR("Cannot send RTDE input data before the RTDE communication has been set up, as the data types of " - "the input recipe are reported by the robot."); - return false; - } - - // Validate before touching the store buffer, so a rejected package cannot wipe or partially - // overwrite input that is already queued. - if (!current_store_buffer_->canCopySetFieldsFrom(package)) + if (!current_store_buffer_->copyFrom(package)) { return false; } - - // Fields the caller didn't write are sent as zeros rather than as whatever the previous package - // left in the buffer, so that a package means the same thing no matter what was sent before it. - current_store_buffer_->initEmpty(); - current_store_buffer_->copySetFieldsFrom(package); markStorageToBeSent(); return true; } diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 4785866a5..f61fad57b 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -482,10 +482,12 @@ bool allVariablesFound(const std::vector& types) // Unlike a client, the server side knows the data types up front, so it applies them itself right // after allocating the package. std::unique_ptr makeTypedDataPackage(const std::vector& recipe, - const std::vector& types) + const std::vector& types, + const uint16_t protocol_version = 2) { auto package = std::make_unique(recipe); - package->initEmpty(types); + package->setTypes(types); + package->setProtocolVersion(protocol_version); return package; } } // namespace @@ -630,6 +632,10 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa std::lock_guard lock(negotiation_mutex_); requested_protocol_versions_.push_back(requested_version); accepted = requested_version <= highest_accepted_protocol_version_; + if (accepted) + { + negotiated_protocol_version_ = requested_version; + } } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; @@ -700,7 +706,7 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa output_data_package_.reset(); if (allVariablesFound(variable_types)) { - output_data_package_ = makeTypedDataPackage(output_recipe_, variable_types); + output_data_package_ = makeTypedDataPackage(output_recipe_, variable_types, negotiated_protocol_version_); } } @@ -815,6 +821,11 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa throw std::runtime_error("Fake RTDE Server received a data package before input recipe was setup. This should " "not happen."); } + if (negotiated_protocol_version_ == 2) + { + uint8_t recipe_id = 0; + bp.parse(recipe_id); + } input_data_package_->parseWith(bp); actOnInput(); break; diff --git a/tests/fake_rtde_server.h b/tests/fake_rtde_server.h index bd14a4c3d..d18b7bfd3 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -111,6 +111,7 @@ class RTDEServer std::deque pending_setup_outputs_text_messages_; std::deque pending_setup_inputs_text_messages_; uint16_t highest_accepted_protocol_version_ = 2; + uint16_t negotiated_protocol_version_ = 2; std::vector requested_protocol_versions_; bool accept_start_ = true; bool accept_pause_ = true; diff --git a/tests/rtde_test_helpers.h b/tests/rtde_test_helpers.h index a0b7511f0..b5b268e9c 100644 --- a/tests/rtde_test_helpers.h +++ b/tests/rtde_test_helpers.h @@ -30,9 +30,8 @@ #pragma once -// Applying the data types from an RTDE setup acknowledgement is the library's own job, so the data -// package, the parser and the writer all keep those entry points out of their public interface. -// Tests, and the fake server standing in for the robot, reach them through these subclasses. +// The parser and the writer keep setRecipeTypes() out of their public interface. Tests, and the +// fake server standing in for the robot, reach it through these subclasses. #include #include @@ -49,8 +48,6 @@ class TestableDataPackage : public rtde_interface::DataPackage { public: using rtde_interface::DataPackage::DataPackage; - using rtde_interface::DataPackage::initEmpty; - using rtde_interface::DataPackage::setProtocolVersion; }; class TestableRTDEParser : public rtde_interface::RTDEParser @@ -78,11 +75,10 @@ class TestableRTDEWriter : public rtde_interface::RTDEWriter * \brief Builds a data package the way the library does: allocate from the recipe, then apply the * data types the robot reported for it. */ -inline TestableDataPackage typedPackage(const std::vector& recipe, const std::vector& types, - const uint16_t protocol_version = 2) +inline TestableDataPackage typedPackage(const std::vector& recipe, const std::vector& types) { - TestableDataPackage package(recipe, protocol_version); - package.initEmpty(types); + TestableDataPackage package(recipe); + package.setTypes(types); return package; } } // namespace test diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index 8aa0d7d5f..f89e6558b 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -197,7 +197,7 @@ TEST(DataPackageAllocationTest, applying_types_does_not_allocate) std::size_t allocations = 0; { AllocationCounter counter; - package.initEmpty(types); + package.setTypes(types); allocations = counter.count(); } @@ -210,10 +210,14 @@ TEST(DataPackageAllocationTest, parsing_a_preallocated_package_does_not_allocate 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" }; + const std::vector types = { "DOUBLE", "DOUBLE" }; test::TestableRTDEParser parser(recipe); - parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setRecipeTypes(types); parser.setProtocolVersion(2); - std::unique_ptr product = std::make_unique(recipe); + // Same as after the handshake: the package already has the negotiated layout, so parse must not + // allocate a replacement. + std::unique_ptr product = + std::make_unique(test::typedPackage(recipe, types)); std::size_t allocations = 0; bool parsed = false; diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index dfeaae323..bedd987f3 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -27,6 +27,7 @@ //---------------------------------------------------------------------- #include +#include #include #include @@ -65,10 +66,11 @@ TEST(rtde_data_package, parse_pkg_protocolv2) std::vector types{ "DOUBLE", "VECTOR6D" }; auto package = typedPackage(recipe, types); - 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, - 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, 0x85, 0x87, - 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; + // Field payload only. The parser consumes the v2 recipe-id byte before parseWith(). + 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, + 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, + 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; comm::BinParser bp(data_package, sizeof(data_package)); @@ -97,19 +99,27 @@ TEST(rtde_data_package, parse_pkg_protocolv1) { std::vector recipe{ "timestamp", "actual_q" }; std::vector types{ "DOUBLE", "VECTOR6D" }; - auto package = typedPackage(recipe, types, 1); - 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, - 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, + // Full v1 package: header then fields, no recipe-id. The parser owns that distinction. + uint8_t data_package[] = { 0x00, 0x3b, 0x55, 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, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; comm::BinParser bp(data_package, sizeof(data_package)); - EXPECT_TRUE(package.parseWith(bp)); + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes(types); + parser.setProtocolVersion(1); + + std::unique_ptr product = std::make_unique(recipe); + ASSERT_TRUE(parser.parse(bp, product)); + + rtde_interface::DataPackage* package = dynamic_cast(product.get()); + ASSERT_NE(package, nullptr); vector6d_t expected_q = { -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }; vector6d_t actual_q; - package.getData("actual_q", actual_q); + package->getData("actual_q", actual_q); double abs = 1e-4; EXPECT_NEAR(expected_q[0], actual_q[0], abs); @@ -121,42 +131,22 @@ TEST(rtde_data_package, parse_pkg_protocolv1) double expected_timestamp = 16854.1919; double actual_timestamp; - package.getData("timestamp", actual_timestamp); + package->getData("timestamp", actual_timestamp); EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); } -// A package constructed before the handshake defaults to protocol version 2. Applying the -// negotiated version afterwards must make a version-1 payload parse without a recipe-id byte. -TEST(rtde_data_package, applying_types_also_applies_the_protocol_version) -{ - std::vector recipe{ "timestamp", "actual_q" }; - test::TestableDataPackage package(recipe); - package.setProtocolVersion(1); - package.initEmpty({ "DOUBLE", "VECTOR6D" }); - - 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, - 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, - 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; - comm::BinParser bp(data_package, sizeof(data_package)); - - ASSERT_TRUE(package.parseWith(bp)); - double timestamp = 0.0; - ASSERT_TRUE(package.getData("timestamp", timestamp)); - EXPECT_NEAR(timestamp, 16854.1919, 1e-4); -} - TEST(rtde_data_package, serialize_pkg_protocolv1) { std::vector recipe{ "speed_slider_mask" }; std::vector types{ "UINT32" }; - auto package = typedPackage(recipe, types, 1); + auto package = typedPackage(recipe, types); uint32_t value = 1; package.setData("speed_slider_mask", value); uint8_t buffer[4096]; + package.setProtocolVersion(1); size_t size = package.serializePackage(buffer); EXPECT_EQ(size, 7); @@ -354,8 +344,9 @@ TEST(rtde_data_package, every_rtde_data_type_survives_a_serialize_parse_round_tr EXPECT_EQ(buffer[header_size + i], expected_integers[i]) << "at payload byte " << i; } - // parseWith() starts at the recipe id, which is where serializePackage() put it after the header - comm::BinParser bp(buffer + header_size, size - header_size); + // serializePackage() writes the v2 recipe-id after the header; parseWith() starts at the fields. + const size_t recipe_id_size = sizeof(uint8_t); + comm::BinParser bp(buffer + header_size + recipe_id_size, size - header_size - recipe_id_size); auto received = typedPackage(recipe, types); ASSERT_TRUE(received.parseWith(bp)); EXPECT_TRUE(bp.empty()) << "the parser did not consume exactly what was serialized"; @@ -401,17 +392,17 @@ TEST(rtde_data_package, unknown_data_types_are_rejected) // A field the robot doesn't know about is reported as NOT_FOUND, one that is already used by // another recipe as IN_USE. Neither is a data type. - EXPECT_THROW(package.initEmpty({ "NOT_FOUND" }), UrException); - EXPECT_THROW(package.initEmpty({ "IN_USE" }), UrException); - EXPECT_THROW(package.initEmpty({ "double" }), UrException); + EXPECT_THROW(package.setTypes({ "NOT_FOUND" }), UrException); + EXPECT_THROW(package.setTypes({ "IN_USE" }), UrException); + EXPECT_THROW(package.setTypes({ "double" }), UrException); } TEST(rtde_data_package, type_count_has_to_match_recipe) { std::vector recipe{ "timestamp", "actual_q" }; test::TestableDataPackage package(recipe); - EXPECT_THROW(package.initEmpty({ "DOUBLE" }), UrException); - EXPECT_THROW(package.initEmpty({ "DOUBLE", "VECTOR6D", "DOUBLE" }), UrException); + EXPECT_THROW(package.setTypes({ "DOUBLE" }), UrException); + EXPECT_THROW(package.setTypes({ "DOUBLE", "VECTOR6D", "DOUBLE" }), UrException); } TEST(rtde_data_package, untyped_package_cannot_be_parsed_or_serialized) @@ -457,7 +448,7 @@ TEST(rtde_data_package, applying_types_makes_the_package_usable) double timestamp = 0.0; ASSERT_FALSE(package.getData("timestamp", timestamp)); - package.initEmpty({ "DOUBLE", "VECTOR6D" }); + package.setTypes({ "DOUBLE", "VECTOR6D" }); EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); ASSERT_TRUE(package.setData("timestamp", 42.0)); @@ -531,7 +522,7 @@ TEST(rtde_data_package, writing_every_field_makes_a_package_serializable) EXPECT_EQ(package.serializePackage(buffer), header_size + sizeof(uint32_t) + sizeof(double)); } -// Merging a partially written package into the send buffer belongs to RTDEWriter, so it is covered +// Overwriting the send buffer with a complete package belongs to RTDEWriter, so it is covered // by the sendPackage() tests in test_rtde_writer.cpp. // Zeroing a package has to keep the types intact, otherwise the next serialization would use the @@ -573,6 +564,171 @@ TEST(rtde_data_package, get_data_with_wrong_type_fails) EXPECT_FALSE(package.getData("timestamp", timestamp)); } +TEST(rtde_data_package, layout_hash_changes_when_types_are_set) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + const uint64_t untyped = package.layoutHash(); + const uint64_t recipe = package.recipeHash(); + + package.setTypes({ "DOUBLE", "VECTOR6D" }); + + EXPECT_EQ(package.recipeHash(), recipe); + EXPECT_NE(package.layoutHash(), untyped); + EXPECT_EQ(package.layoutHash(), + rtde_interface::DataPackage::layoutHashFor({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" })); +} + +TEST(rtde_data_package, layout_hash_changes_on_first_set_data_to_an_untyped_field) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + const uint64_t untyped = package.layoutHash(); + + ASSERT_TRUE(package.setData("timestamp", 1.0)); + const uint64_t after_first = package.layoutHash(); + EXPECT_NE(after_first, untyped); + + ASSERT_TRUE(package.setData("timestamp", 2.0)); + EXPECT_EQ(package.layoutHash(), after_first); +} + +TEST(rtde_data_package, layout_hash_does_not_change_on_reset_init_empty_or_parse) +{ + auto package = typedPackage({ "timestamp", "target_speed_fraction" }, { "DOUBLE", "DOUBLE" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + const uint64_t hash = package.layoutHash(); + + ASSERT_TRUE(package.resetData("timestamp")); + EXPECT_EQ(package.layoutHash(), hash); + + package.initEmpty(); + EXPECT_EQ(package.layoutHash(), hash); + + uint8_t data[] = { 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(data, sizeof(data)); + ASSERT_TRUE(package.parseWith(bp)); + EXPECT_EQ(package.layoutHash(), hash); +} + +TEST(rtde_data_package, copy_from_overwrites_every_field) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + auto source = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(source.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + ASSERT_TRUE(destination.copyFrom(source)); + + double fraction = 0.0; + uint32_t mask = 0; + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + ASSERT_TRUE(destination.getData("speed_slider_mask", mask)); + EXPECT_DOUBLE_EQ(fraction, 0.5); + EXPECT_EQ(mask, 1u); + + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.7)); + ASSERT_TRUE(source.setData("speed_slider_mask", static_cast(0))); + ASSERT_TRUE(destination.copyFrom(source)); + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + ASSERT_TRUE(destination.getData("speed_slider_mask", mask)); + EXPECT_DOUBLE_EQ(fraction, 0.7); + EXPECT_EQ(mask, 0u); +} + +TEST(rtde_data_package, copy_from_rejects_a_source_whose_types_changed) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + auto source = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(source.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + ASSERT_TRUE(destination.copyFrom(source)); + + auto wrong = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT8", "DOUBLE" }); + ASSERT_TRUE(wrong.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(wrong.setData("speed_slider_fraction", 0.9)); + EXPECT_FALSE(destination.copyFrom(wrong)); + + double fraction = 0.0; + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + EXPECT_DOUBLE_EQ(fraction, 0.5); +} + +TEST(rtde_data_package, copy_from_rejects_an_untyped_source_field) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + + EXPECT_FALSE(destination.copyFrom(source)); +} + +TEST(rtde_data_package, copy_from_rejects_when_the_destination_is_retyped) +{ + auto destination = typedPackage({ "timestamp" }, { "DOUBLE" }); + auto source = typedPackage({ "timestamp" }, { "DOUBLE" }); + ASSERT_TRUE(source.setData("timestamp", 1.0)); + ASSERT_TRUE(destination.copyFrom(source)); + + destination.setTypes({ "UINT32" }); + EXPECT_FALSE(destination.copyFrom(source)); +} + +TEST(rtde_data_package, failed_copy_from_does_not_overwrite) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(destination.setData("speed_slider_fraction", 0.5)); + + auto wrong = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT8", "DOUBLE" }); + ASSERT_TRUE(wrong.setData("speed_slider_fraction", 0.9)); + EXPECT_FALSE(destination.copyFrom(wrong)); + + auto source = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.25)); + ASSERT_TRUE(destination.copyFrom(source)); + + double fraction = 0.0; + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + EXPECT_DOUBLE_EQ(fraction, 0.25); +} + +TEST(rtde_data_package, copy_from_a_different_recipe_fails_after_a_successful_copy) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + auto source = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + ASSERT_TRUE(destination.copyFrom(source)); + + rtde_interface::DataPackage other({ "standard_analog_output_0" }); + ASSERT_TRUE(other.setData("standard_analog_output_0", 0.1)); + EXPECT_FALSE(destination.copyFrom(other)); +} + +TEST(rtde_data_package, same_recipe_assignment_keeps_name_lookup) +{ + auto source = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(source.setData("timestamp", 42.0)); + + auto destination = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + destination = source; + + double timestamp = 0.0; + ASSERT_TRUE(destination.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + EXPECT_EQ(destination.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); +} + +TEST(rtde_data_package, assignment_from_a_different_recipe_rebuilds_name_lookup) +{ + auto source = typedPackage({ "actual_q" }, { "VECTOR6D" }); + ASSERT_TRUE(source.setData("actual_q", vector6d_t{ 1, 2, 3, 4, 5, 6 })); + + auto destination = typedPackage({ "timestamp" }, { "DOUBLE" }); + destination = source; + + vector6d_t actual_q{}; + ASSERT_TRUE(destination.getData("actual_q", actual_q)); + EXPECT_DOUBLE_EQ(actual_q[0], 1.0); + EXPECT_FALSE(destination.getDataType("timestamp").has_value()); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 99ed8c30e..430a7d5db 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -237,7 +237,7 @@ TEST(rtde_parser, data_package_without_recipe_types_fails) EXPECT_FALSE(parser.parse(bp, product)); } -TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) +TEST(rtde_parser, untyped_pre_allocated_data_package_is_replaced) { unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; @@ -248,13 +248,9 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); - // Applications that still create their packages from a recipe hand in an untyped package. It has - // to be usable afterwards, without being replaced by a freshly allocated one. std::unique_ptr product = std::make_unique(recipe); - const rtde_interface::RTDEPackage* package_address = product.get(); ASSERT_TRUE(parser.parse(bp, product)); - EXPECT_EQ(product.get(), package_address); rtde_interface::DataPackage* data = dynamic_cast(product.get()); ASSERT_NE(data, nullptr); @@ -264,8 +260,7 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) } // setData() on every field makes isTyped() true, but those types did not come from the robot. -// The parser has to re-apply the acknowledged types rather than parse the payload as integers. -TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_retyped_from_the_robot) +TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_replaced) { unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; @@ -281,10 +276,8 @@ TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_retyped_from_the_robot) ASSERT_TRUE(package->setData("target_speed_fraction", static_cast(2))); std::unique_ptr product = std::move(package); - const rtde_interface::RTDEPackage* package_address = product.get(); ASSERT_TRUE(parser.parse(bp, product)); - EXPECT_EQ(product.get(), package_address); rtde_interface::DataPackage* data = dynamic_cast(product.get()); ASSERT_NE(data, nullptr); @@ -294,8 +287,6 @@ TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_retyped_from_the_robot) EXPECT_DOUBLE_EQ(timestamp, 16412.206); } -// A same-length recipe with different field names must not have the robot's types applied onto it -// in order; the parser replaces the package instead. TEST(rtde_parser, pre_allocated_package_with_a_different_recipe_is_replaced) { unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, @@ -441,6 +432,41 @@ TEST(rtde_parser, text_message_protocol_v2) EXPECT_EQ(message->toString(), "message: hello\nsource: urcl\nwarning level: 1"); } +// A second parse into a package that already has the negotiated layout must not replace it or +// re-apply types. That is the receive-path hash hit. +TEST(rtde_parser, already_typed_package_is_parsed_in_place_without_being_replaced) +{ + 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" }; + test::TestableRTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + + std::unique_ptr product = std::make_unique(recipe); + { + comm::BinParser bp(raw_data, sizeof(raw_data)); + ASSERT_TRUE(parser.parse(bp, product)); + } + const rtde_interface::RTDEPackage* package_address = product.get(); + + unsigned char second[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xc3, 0x88, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(second, sizeof(second)); + ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); + + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + double target_speed_fraction = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + ASSERT_TRUE(data->getData("target_speed_fraction", target_speed_fraction)); + EXPECT_DOUBLE_EQ(timestamp, 10000.0); + EXPECT_DOUBLE_EQ(target_speed_fraction, 0.5); +} + // Protocol version 1 puts a message type where version 2 has the lengths, and takes the rest of the // package as the message. TEST(rtde_parser, text_message_protocol_v1) diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index 6e8bd1b7c..9f0895770 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -519,6 +519,7 @@ TEST_F(RTDEWriterTest, send_data_package) const uint8_t standard_digital_output_mask = 0b00000001; // pin 1 rtde_interface::DataPackage data_package(input_recipe_); + data_package.setTypes(input_recipe_types_); 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( @@ -545,9 +546,9 @@ TEST_F(RTDEWriterTest, send_data_package) EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); } -// The fields an application leaves alone are sent as zeros, so a package means the same thing no -// matter which values happened to be sent before it. -TEST_F(RTDEWriterTest, unset_fields_are_sent_as_zeros) +// A complete package overwrites the send buffer, so fields the application did not set this time +// go out as zeros rather than as whatever was sent before. +TEST_F(RTDEWriterTest, send_data_package_overwrites_every_field) { ASSERT_TRUE(writer_->sendSpeedSlider(0.7)); ASSERT_TRUE(waitForMessageCallback(1000)); @@ -555,6 +556,7 @@ TEST_F(RTDEWriterTest, unset_fields_are_sent_as_zeros) ASSERT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.7); rtde_interface::DataPackage data_package(input_recipe_); + data_package.setTypes(input_recipe_types_); ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); ASSERT_TRUE(writer_->sendPackage(data_package)); ASSERT_TRUE(waitForMessageCallback(1000)); @@ -565,6 +567,25 @@ TEST_F(RTDEWriterTest, unset_fields_are_sent_as_zeros) EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); } +// A package that still has untyped fields is not the same layout the robot acknowledged. +TEST_F(RTDEWriterTest, send_data_package_with_untyped_fields_fails) +{ + rtde_interface::DataPackage data_package(input_recipe_); + ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); + + EXPECT_FALSE(writer_->sendPackage(data_package)); +} + +// A package has to be built from the recipe that was registered, since that is what decides which +// field is which. +TEST_F(RTDEWriterTest, send_data_package_built_from_a_partial_recipe_fails) +{ + rtde_interface::DataPackage data_package({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", 0.7)); + + EXPECT_FALSE(writer_->sendPackage(data_package)); +} + // The robot is the authority on a field's type, so writing one with the wrong type has to be // reported rather than serialized into a package the robot would misread. TEST_F(RTDEWriterTest, send_data_package_with_wrong_field_type_fails) From 9156cd92272dfa8168b504cbf29cfd8e65634848 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Wed, 2 Sep 2026 16:03:09 +0200 Subject: [PATCH 10/18] Make setRecipeTypes() public on the RTDE parser and writer. setProtocolVersion() was already public on both, and the two are set from the same handshake, so keeping only setRecipeTypes() behind a friend declaration drew a line that did not correspond to anything. The subclasses the tests used to reach the protected member are gone with it: TestableRTDEParser and TestableRTDEWriter held nothing but a using declaration, and TestableDataPackage only inherited constructors that were public anyway. Tests now name the library types directly. --- include/ur_client_library/rtde/rtde_parser.h | 4 --- include/ur_client_library/rtde/rtde_writer.h | 4 --- tests/fake_rtde_server.cpp | 2 +- tests/rtde_test_helpers.h | 37 ++----------------- tests/test_pipeline.cpp | 4 +-- tests/test_producer.cpp | 4 +-- tests/test_rtde_allocations.cpp | 4 +-- tests/test_rtde_data_package.cpp | 9 ++--- tests/test_rtde_parser.cpp | 38 ++++++++++---------- tests/test_rtde_writer.cpp | 4 +-- 10 files changed, 36 insertions(+), 74 deletions(-) diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 51a34240b..ed0ad2c85 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -97,10 +97,6 @@ class RTDEParser : public comm::Parser return protocol_version_; } -protected: - // Relays the data types from the robot's setup acknowledgement, which only the client receives. - friend class RTDEClient; - /*! * \brief Registers the data types belonging to the recipe, as reported by the robot in the RTDE * setup acknowledgement. diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 4f95cc4ed..485752dac 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -196,10 +196,6 @@ class RTDEWriter */ bool sendExternalForceTorque(const vector6d_t& external_force_torque); -protected: - // Relays the data types and the protocol version from the handshake, which only the client sees. - friend class RTDEClient; - /*! * \brief Applies the data types the robot reported for the input recipe. * diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index f61fad57b..8eb4faf3a 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -485,7 +485,7 @@ std::unique_ptr makeTypedDataPackage(const std::vec const std::vector& types, const uint16_t protocol_version = 2) { - auto package = std::make_unique(recipe); + auto package = std::make_unique(recipe); package->setTypes(types); package->setProtocolVersion(protocol_version); return package; diff --git a/tests/rtde_test_helpers.h b/tests/rtde_test_helpers.h index b5b268e9c..5794f68b5 100644 --- a/tests/rtde_test_helpers.h +++ b/tests/rtde_test_helpers.h @@ -30,54 +30,23 @@ #pragma once -// The parser and the writer keep setRecipeTypes() out of their public interface. Tests, and the -// fake server standing in for the robot, reach it through these subclasses. - #include #include #include -#include -#include namespace urcl { namespace test { -class TestableDataPackage : public rtde_interface::DataPackage -{ -public: - using rtde_interface::DataPackage::DataPackage; -}; - -class TestableRTDEParser : public rtde_interface::RTDEParser -{ -public: - explicit TestableRTDEParser(const std::vector& recipe) : rtde_interface::RTDEParser(recipe) - { - } - - using rtde_interface::RTDEParser::setRecipeTypes; -}; - -class TestableRTDEWriter : public rtde_interface::RTDEWriter -{ -public: - TestableRTDEWriter(comm::URStream* stream, const std::vector& recipe) - : rtde_interface::RTDEWriter(stream, recipe) - { - } - - using rtde_interface::RTDEWriter::setRecipeTypes; -}; - /*! * \brief Builds a data package the way the library does: allocate from the recipe, then apply the * data types the robot reported for it. */ -inline TestableDataPackage typedPackage(const std::vector& recipe, const std::vector& types) +inline rtde_interface::DataPackage typedPackage(const std::vector& recipe, + const std::vector& types) { - TestableDataPackage package(recipe); + rtde_interface::DataPackage package(recipe); package.setTypes(types); return package; } diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index b24de2cc7..ede09f0b5 100644 --- a/tests/test_pipeline.cpp +++ b/tests/test_pipeline.cpp @@ -55,7 +55,7 @@ class PipelineTest : public ::testing::Test // Setup pipeline stream_.reset(new comm::URStream("127.0.0.1", 60002)); std::vector recipe = { "timestamp" }; - parser_.reset(new test::TestableRTDEParser(recipe)); + parser_.reset(new rtde_interface::RTDEParser(recipe)); parser_->setRecipeTypes({ "DOUBLE" }); parser_->setProtocolVersion(2); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); @@ -75,7 +75,7 @@ class PipelineTest : public ::testing::Test std::unique_ptr server_; std::unique_ptr> stream_; - std::unique_ptr parser_; + std::unique_ptr parser_; std::unique_ptr> producer_; std::unique_ptr> pipeline_; comm::INotifier notifier_; diff --git a/tests/test_producer.cpp b/tests/test_producer.cpp index 0770ab3f4..a256a388d 100644 --- a/tests/test_producer.cpp +++ b/tests/test_producer.cpp @@ -64,7 +64,7 @@ TEST_F(ProducerTest, get_data_package) { comm::URStream stream("127.0.0.1", 60002); std::vector recipe = { "timestamp" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE" }); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); @@ -100,7 +100,7 @@ TEST_F(ProducerTest, connect_non_connected_robot) { comm::URStream stream("127.0.0.1", 12321); std::vector recipe = { "timestamp" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE" }); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index f89e6558b..25d45c2f2 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -191,7 +191,7 @@ TEST(AllocationCounterTest, counts_allocations) TEST(DataPackageAllocationTest, applying_types_does_not_allocate) { - test::TestableDataPackage package({ "timestamp", "actual_q" }); + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); const std::vector types{ "DOUBLE", "VECTOR6D" }; std::size_t allocations = 0; @@ -211,7 +211,7 @@ TEST(DataPackageAllocationTest, parsing_a_preallocated_package_does_not_allocate 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; std::vector recipe = { "timestamp", "target_speed_fraction" }; const std::vector types = { "DOUBLE", "DOUBLE" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes(types); parser.setProtocolVersion(2); // Same as after the handshake: the package already has the negotiated layout, so parse must not diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index bedd987f3..5532f3517 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -31,6 +31,7 @@ #include #include +#include #include "rtde_test_helpers.h" @@ -107,7 +108,7 @@ TEST(rtde_data_package, parse_pkg_protocolv1) 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; comm::BinParser bp(data_package, sizeof(data_package)); - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes(types); parser.setProtocolVersion(1); @@ -388,7 +389,7 @@ TEST(rtde_data_package, every_rtde_data_type_survives_a_serialize_parse_round_tr TEST(rtde_data_package, unknown_data_types_are_rejected) { std::vector recipe{ "timestamp" }; - test::TestableDataPackage package(recipe); + rtde_interface::DataPackage package(recipe); // A field the robot doesn't know about is reported as NOT_FOUND, one that is already used by // another recipe as IN_USE. Neither is a data type. @@ -400,7 +401,7 @@ TEST(rtde_data_package, unknown_data_types_are_rejected) TEST(rtde_data_package, type_count_has_to_match_recipe) { std::vector recipe{ "timestamp", "actual_q" }; - test::TestableDataPackage package(recipe); + rtde_interface::DataPackage package(recipe); EXPECT_THROW(package.setTypes({ "DOUBLE" }), UrException); EXPECT_THROW(package.setTypes({ "DOUBLE", "VECTOR6D", "DOUBLE" }), UrException); } @@ -443,7 +444,7 @@ TEST(rtde_data_package, untyped_package_gets_typed_by_assignment) TEST(rtde_data_package, applying_types_makes_the_package_usable) { std::vector recipe{ "timestamp", "actual_q" }; - test::TestableDataPackage package(recipe); + rtde_interface::DataPackage package(recipe); double timestamp = 0.0; ASSERT_FALSE(package.getData("timestamp", timestamp)); diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 430a7d5db..3e8526b0d 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -39,7 +39,7 @@ TEST(rtde_parser, request_protocol_version) { // Accepted request protocol version unsigned char raw_data[] = { 0x00, 0x04, 0x56, 0x01 }; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); // test a non-preallocated product std::unique_ptr product; @@ -85,7 +85,7 @@ TEST(rtde_parser, get_urcontrol_version) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::GetUrcontrolVersion* data = dynamic_cast(product.get())) @@ -109,7 +109,7 @@ TEST(rtde_parser, control_package_pause) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackagePause* data = dynamic_cast(product.get())) @@ -130,7 +130,7 @@ TEST(rtde_parser, control_package_start) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackageStart* data = dynamic_cast(product.get())) @@ -152,7 +152,7 @@ TEST(rtde_parser, control_package_setup_inputs) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.parse(bp, product); if (rtde_interface::ControlPackageSetupInputs* data = @@ -176,7 +176,7 @@ TEST(rtde_parser, control_package_setup_outputs) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -202,7 +202,7 @@ TEST(rtde_parser, data_package) std::unique_ptr product; std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -231,7 +231,7 @@ TEST(rtde_parser, data_package_without_recipe_types_fails) // Without the types from the robot's acknowledgement the payload cannot be interpreted std::unique_ptr product; - test::TestableRTDEParser parser({ "timestamp", "target_speed_fraction" }); + rtde_interface::RTDEParser parser({ "timestamp", "target_speed_fraction" }); parser.setProtocolVersion(2); EXPECT_FALSE(parser.parse(bp, product)); @@ -244,7 +244,7 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_replaced) comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); @@ -267,7 +267,7 @@ TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_replaced) comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); @@ -294,7 +294,7 @@ TEST(rtde_parser, pre_allocated_package_with_a_different_recipe_is_replaced) comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); @@ -320,7 +320,7 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_takes_protocol_version_1) comm::BinParser bp(raw_data, sizeof(raw_data)); std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(1); @@ -342,7 +342,7 @@ TEST(rtde_parser, test_to_string) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.parse(bp, product); std::stringstream expected; @@ -359,7 +359,7 @@ TEST(rtde_parser, test_buffer_too_short) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); EXPECT_FALSE(parser.parse(bp, product)); } @@ -370,7 +370,7 @@ TEST(rtde_parser, test_buffer_too_long) comm::BinParser bp(raw_data, sizeof(raw_data)); std::unique_ptr product; - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); EXPECT_FALSE(parser.parse(bp, product)); } @@ -380,7 +380,7 @@ TEST(rtde_parser, test_deprecated_parse_method) 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" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); @@ -418,7 +418,7 @@ TEST(rtde_parser, text_message_protocol_v2) unsigned char raw_data[] = { 0x00, 0x0f, 0x4d, 0x05, 'h', 'e', 'l', 'l', 'o', 0x04, 'u', 'r', 'c', 'l', 0x01 }; comm::BinParser bp(raw_data, sizeof(raw_data)); - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); parser.setProtocolVersion(2); std::unique_ptr product; @@ -440,7 +440,7 @@ TEST(rtde_parser, already_typed_package_is_parsed_in_place_without_being_replace 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; std::vector recipe = { "timestamp", "target_speed_fraction" }; - test::TestableRTDEParser parser(recipe); + rtde_interface::RTDEParser parser(recipe); parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); @@ -475,7 +475,7 @@ TEST(rtde_parser, text_message_protocol_v1) unsigned char raw_data[] = { 0x00, 0x0a, 0x4d, 0x03, 'l', 'e', 'g', 'a', 'c', 'y' }; comm::BinParser bp(raw_data, sizeof(raw_data)); - test::TestableRTDEParser parser({ "" }); + rtde_interface::RTDEParser parser({ "" }); std::unique_ptr product; ASSERT_TRUE(parser.parse(bp, product)); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index 9f0895770..17ecb5d06 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -56,7 +56,7 @@ class RTDEWriterTest : public ::testing::Test stream_.reset(new comm::URStream("127.0.0.1", 60004)); stream_->connect(); - writer_.reset(new test::TestableRTDEWriter(stream_.get(), input_recipe_)); + writer_.reset(new rtde_interface::RTDEWriter(stream_.get(), input_recipe_)); writer_->setRecipeTypes(input_recipe_types_); writer_->init(1); } @@ -132,7 +132,7 @@ class RTDEWriterTest : public ::testing::Test std::vector input_recipe_types_ = { "UINT32", "DOUBLE", "UINT8", "UINT8", "UINT8", "UINT8", "UINT8", "UINT8", "UINT8", "UINT8", "DOUBLE", "DOUBLE", "BOOL", "INT32", "DOUBLE", "VECTOR6D" }; - std::unique_ptr writer_; + std::unique_ptr writer_; std::unique_ptr server_; std::unique_ptr> stream_; std::unordered_map parsed_data_; From a07bfaf6bf5d10f5de75a5389ce68ad6d2889393 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Thu, 3 Sep 2026 15:49:56 +0200 Subject: [PATCH 11/18] Accept a partly typed RTDE package on send without walking field names. Applications still construct a DataPackage from the input recipe and write only the fields they care about. That package does not have the robot's layout hash, so requiring a full match rejected the flow that already worked. copyFrom() now falls back to a recipe-hash compare and two positional loops: validate types, then take written values and send the rest as zeros. A package built from a different recipe is still rejected. --- include/ur_client_library/rtde/data_package.h | 25 ++++++---- src/rtde/data_package.cpp | 47 +++++++++++++++++-- tests/test_rtde_data_package.cpp | 15 +++++- tests/test_rtde_writer.cpp | 26 ++++++---- 4 files changed, 90 insertions(+), 23 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 652aaa411..212de6e86 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -370,20 +370,23 @@ class DataPackage : public RTDEPackage void setTypes(const std::vector& types); /*! - * \brief Overwrites every field of this package with the corresponding field of \p other. + * \brief Takes over the values of \p other, sending fields it has not written as zeros. * - * This package must already be typed. \p other has to carry the same field names and the same - * type on every field, which is what a package has after the robot's acknowledgement or after - * every subscribed field has been written. Recipe id and protocol version are left untouched. + * This package must already be typed. \p other has to be built from the same recipe, and every + * field it has a value for has to carry the type this package has for it. Recipe id and protocol + * version are left untouched. * - * The success path is a layout-hash compare and a memcpy of the value array. The hashes are a - * 64-bit identity of the field names and each field's variant index; a collision would skip a - * validation that should have failed, which is accepted for this path. + * When \p other has the same field names and the same type on every one of them, which is what a + * package has after the robot's acknowledgement, the copy is a layout-hash compare and a memcpy + * of the value array. The hashes are a 64-bit identity of the field names and each field's + * variant index; a collision would skip a validation that should have failed, which is accepted + * for this path. A package an application typed by writing only the fields it cares about is + * instead merged position by position, with unwritten fields sent as zeros. * * \param other The package to copy from * * \returns True on success, false if this package is untyped, if \p other was built from a - * different recipe or if a field in \p other has a different type + * different recipe, or if a field \p other has written has a different type */ bool copyFrom(const DataPackage& other); @@ -446,6 +449,11 @@ class DataPackage : public RTDEPackage static uint64_t layoutHashFor(const std::vector& recipe, const std::vector& types); private: + /*! + * \brief Logs once that a copy walked fields instead of memcpy'ing the value array. + */ + void reportSlowCopyOnce(); + /*! * \brief Allocates one slot per recipe field, with the type left undecided. */ @@ -482,6 +490,7 @@ class DataPackage : public RTDEPackage uint64_t recipe_hash_ = 0; uint64_t layout_hash_ = 0; bool fully_typed_ = false; + bool slow_copy_reported_ = false; }; } // namespace rtde_interface diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index e7522eaff..cba3a87a0 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -358,6 +358,20 @@ void rtde_interface::DataPackage::initEmpty() copyValues(values_, zeros_); } +void rtde_interface::DataPackage::reportSlowCopyOnce() +{ + if (slow_copy_reported_) + { + return; + } + slow_copy_reported_ = true; + URCL_LOG_WARN("Copying an RTDE data package that is not fully typed walks each field instead of " + "copying the value array in one step. That is the path a package takes when it is " + "constructed from a recipe and only some of its fields are written. A package that " + "already carries the same field names and types as this one can be copied in one " + "memcpy."); +} + bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) { if (!isTyped()) @@ -366,13 +380,40 @@ bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) "reported by the robot during the RTDE handshake."); return false; } - if (layout_hash_ != other.layout_hash_ || values_.size() != other.values_.size()) + + // Same field names and the same type on every field, so the whole value array can go across at + // once. This is the path a real-time loop takes. + if (layout_hash_ == other.layout_hash_ && values_.size() == other.values_.size()) + { + copyValues(values_, other.values_); + return true; + } + + if (recipe_hash_ != other.recipe_hash_ || values_.size() != other.values_.size()) { - URCL_LOG_ERROR("Cannot copy from an RTDE data package whose layout does not match this one."); + URCL_LOG_ERROR("Cannot copy from an RTDE data package built from a different recipe."); return false; } - copyValues(values_, other.values_); + // Same recipe, so field i here is field i there. Validate before writing anything, so a package + // that is rejected leaves the values already in here alone. + for (size_t i = 0; i < values_.size(); ++i) + { + if (!std::holds_alternative(other.values_[i]) && other.values_[i].index() != values_[i].index()) + { + URCL_LOG_ERROR("The value passed for the data field '%s' is of type %s, but the robot reports that field as " + "%s.", + recipe_[i].c_str(), typeNameOf(other.values_[i]).c_str(), typeNameOf(values_[i]).c_str()); + return false; + } + } + + for (size_t i = 0; i < values_.size(); ++i) + { + values_[i] = std::holds_alternative(other.values_[i]) ? zeros_[i] : other.values_[i]; + } + + reportSlowCopyOnce(); return true; } diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 5532f3517..154469b82 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -652,13 +652,24 @@ TEST(rtde_data_package, copy_from_rejects_a_source_whose_types_changed) EXPECT_DOUBLE_EQ(fraction, 0.5); } -TEST(rtde_data_package, copy_from_rejects_an_untyped_source_field) +// An application may write only the fields it cares about, which leaves the rest of its package +// untyped. Those fields are taken over as zeros rather than making the copy fail. +TEST(rtde_data_package, copy_from_zeros_the_fields_the_source_did_not_write) { auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(destination.setData("speed_slider_mask", static_cast(7))); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }); ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); - EXPECT_FALSE(destination.copyFrom(source)); + ASSERT_TRUE(destination.copyFrom(source)); + + double fraction = 0.0; + uint32_t mask = 0; + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + ASSERT_TRUE(destination.getData("speed_slider_mask", mask)); + EXPECT_DOUBLE_EQ(fraction, 0.5); + EXPECT_EQ(mask, 0u); } TEST(rtde_data_package, copy_from_rejects_when_the_destination_is_retyped) diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index 17ecb5d06..b32b3424b 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -519,7 +519,6 @@ TEST_F(RTDEWriterTest, send_data_package) const uint8_t standard_digital_output_mask = 0b00000001; // pin 1 rtde_interface::DataPackage data_package(input_recipe_); - data_package.setTypes(input_recipe_types_); 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( @@ -546,9 +545,9 @@ TEST_F(RTDEWriterTest, send_data_package) EXPECT_EQ(standard_digital_output_mask, received_standard_digital_output_mask); } -// A complete package overwrites the send buffer, so fields the application did not set this time -// go out as zeros rather than as whatever was sent before. -TEST_F(RTDEWriterTest, send_data_package_overwrites_every_field) +// The fields an application leaves alone are sent as zeros, so a package means the same thing no +// matter which values happened to be sent before it. +TEST_F(RTDEWriterTest, unset_fields_are_sent_as_zeros) { ASSERT_TRUE(writer_->sendSpeedSlider(0.7)); ASSERT_TRUE(waitForMessageCallback(1000)); @@ -556,7 +555,6 @@ TEST_F(RTDEWriterTest, send_data_package_overwrites_every_field) ASSERT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.7); rtde_interface::DataPackage data_package(input_recipe_); - data_package.setTypes(input_recipe_types_); ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); ASSERT_TRUE(writer_->sendPackage(data_package)); ASSERT_TRUE(waitForMessageCallback(1000)); @@ -567,17 +565,25 @@ TEST_F(RTDEWriterTest, send_data_package_overwrites_every_field) EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); } -// A package that still has untyped fields is not the same layout the robot acknowledged. -TEST_F(RTDEWriterTest, send_data_package_with_untyped_fields_fails) +// A package the robot's types have been applied to already has the send buffer's layout, which is +// the path a real-time loop takes. It has to put the same thing on the wire as the partial flow. +TEST_F(RTDEWriterTest, send_data_package_typed_by_the_robot) { rtde_interface::DataPackage data_package(input_recipe_); + data_package.setTypes(input_recipe_types_); ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); - EXPECT_FALSE(writer_->sendPackage(data_package)); + EXPECT_TRUE(writer_->sendPackage(data_package)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + ASSERT_TRUE(dataFieldExist("standard_analog_output_0")); + EXPECT_EQ(std::get(parsed_data_["standard_analog_output_0"]), 0.4); + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); } -// A package has to be built from the recipe that was registered, since that is what decides which -// field is which. +// A package has to be built from the recipe that was registered, since the fallback copies +// position by position and cannot map a subset onto a larger recipe. TEST_F(RTDEWriterTest, send_data_package_built_from_a_partial_recipe_fails) { rtde_interface::DataPackage data_package({ "speed_slider_mask", "speed_slider_fraction" }); From 436dfc16d3743bea055d4c9cb5f1c3706a1fac46 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 15:26:25 +0200 Subject: [PATCH 12/18] Type a pre-allocated RTDE data package in place instead of replacing it. A DataPackage built from a recipe carries no data types, since those only arrive with the robot's setup acknowledgement. The parser used to reject such a package, discard it and allocate a typed replacement with a warning. That made the first getDataPackageBlocking() call allocate for every application following the documented pattern, including our own rtde_client example. The types can simply be applied to the package that was handed in: its storage is already the right shape, and setTypes() only overwrites the variant alternatives, which allocates nothing. The parser now does that whenever the package's recipe hash matches the negotiated one, and keeps allocating a replacement only for a package built from a genuinely different recipe, where parsing on would silently write fields under the wrong names. RTDEParser holds the negotiated layout as a typed template package rather than a bare hash, so the recipe and layout identities and the blueprint for packages it has to allocate itself all come from one object that cannot go stale. Also adds an RTDE writer example covering sendPackage() with the typed input package, and corrects the documentation that this behaviour touches: - The claims that the read path never allocates were true for getDataPackage() but not for the blocking read; they hold again now, and the architecture doc explains which call types the package. - The writer example's sample output was not reproducible from its own code. It is replaced with a run captured against URSim 5.25.1, and the lag and comparison descriptions are corrected to match what the code checks. - Two parser tests asserting that a package "is replaced" now assert that it is typed in place, and an allocation test pins the first parse into an untyped package at zero allocations. --- doc/architecture/rtde_client.rst | 30 ++- doc/examples.rst | 1 + doc/examples/rtde_client.rst | 4 +- doc/examples/rtde_writer.rst | 183 ++++++++++++++ doc/migration_notes.rst | 5 +- examples/CMakeLists.txt | 4 + examples/rtde_writer.cpp | 238 ++++++++++++++++++ include/ur_client_library/rtde/data_package.h | 10 + include/ur_client_library/rtde/rtde_client.h | 22 +- include/ur_client_library/rtde/rtde_parser.h | 13 +- include/ur_client_library/rtde/rtde_writer.h | 15 ++ src/rtde/data_package.cpp | 11 + src/rtde/rtde_parser.cpp | 37 +-- src/rtde/rtde_writer.cpp | 12 + tests/test_rtde_allocations.cpp | 35 ++- tests/test_rtde_data_package.cpp | 38 +++ tests/test_rtde_parser.cpp | 14 +- tests/test_rtde_writer.cpp | 41 +++ 18 files changed, 672 insertions(+), 41 deletions(-) create mode 100644 doc/examples/rtde_writer.rst create mode 100644 examples/rtde_writer.cpp diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index 31022ca61..7928ef6fa 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -46,9 +46,9 @@ the :ref:`rtde_client_example` for an example of the blocking read method. allocate. A recipe only lists field names. The data types belonging to them are reported by the robot when - it acknowledges the recipe, and the first package received applies them to your ``DataPackage``, - which costs no memory. Until that has happened ``getData()`` on the package fails. See `Field - data types`_ for how to ask a package what type it gave a field. + it acknowledges the recipe, and the first read applies them to your ``DataPackage``, which costs + no memory. Until that has happened ``getData()`` on the package fails. See `Field data types`_ + for how to ask a package what type it gave a field. Upon construction, two recipe files have to be given, one for the RTDE inputs, one for the RTDE outputs. Please refer to the `RTDE @@ -79,9 +79,11 @@ After calling ``my_client.start()``, data can be read from the 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. -Both methods parse into a ``DataPackage`` that the caller owns, which is what keeps the read path -free of memory allocations. The deprecated ``getDataPackage(timeout)`` overload, which returns a new -package instead, allocates on every call by design and is therefore not suited for real-time use. +Both methods deliver their data into a ``DataPackage`` that the caller owns, which is what keeps the +read path free of memory allocations: ``getDataPackage()`` copies the background reader's latest +package into it, ``getDataPackageBlocking()`` parses the next package straight into it. The +deprecated ``getDataPackage(timeout)`` overload, which returns a new package instead, allocates on +every call by design and is therefore not suited for real-time use. Field data types ~~~~~~~~~~~~~~~~ @@ -182,18 +184,22 @@ The class offers specific methods for every RTDE input possible to write. Data is sent asynchronously to the RTDE interface. -To write several fields at once, construct a ``DataPackage`` from ``RTDEClient::getInputRecipe()``, -fill the fields you care about and pass it to ``sendPackage()``. Fields you leave alone are sent as -zeros. Since the robot decides what type each field has, ``sendPackage()`` is where a value written -with the wrong type is reported: +To write several fields at once, ask the client for a package that already carries the data types +the robot reported for the input recipe, fill the fields you care about and pass it to +``sendPackage()``. Fields you leave alone are sent as zeros. Because the package is already typed, +``setData()`` reports a value written with the wrong type immediately: .. code-block:: c++ - rtde_interface::DataPackage input_pkg(my_client.getInputRecipe()); - input_pkg.setData("speed_slider_mask", 1); + rtde_interface::DataPackage input_pkg = my_client.createInputDataPackage(); + input_pkg.setData("speed_slider_mask", uint32_t{ 1 }); input_pkg.setData("speed_slider_fraction", 0.5); my_client.getWriter().sendPackage(input_pkg); +A package constructed from ``getInputRecipe()`` still works. Its types are taken from the values +written to it and are checked when the package is sent. See the :ref:`rtde_writer_example` for a +complete example. + .. note:: The ``RTDEWriter`` will return ``false`` on any writing attempts for fields that have not been diff --git a/doc/examples.rst b/doc/examples.rst index fae5a85e9..afe955b80 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -23,6 +23,7 @@ may be running forever until manually stopped. examples/primary_pipeline examples/primary_pipeline_calibration examples/rtde_client + examples/rtde_writer examples/external_fts_through_rtde examples/script_command_interface examples/script_sender diff --git a/doc/examples/rtde_client.rst b/doc/examples/rtde_client.rst index 4e9779b28..9f4978f18 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -57,8 +57,8 @@ fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method. :end-before: // Change the speed slider Creating the package we read into is the last allocation the read path makes; the loop below reuses -the same package. The recipe only names the fields, so the first package received is also what tells -this one what type each of its fields has, which needs no further memory. +the same package. The recipe only names the fields, so the first read is also what tells this one +what type each of its fields has, which needs no further memory. 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 diff --git a/doc/examples/rtde_writer.rst b/doc/examples/rtde_writer.rst new file mode 100644 index 000000000..4aadd586f --- /dev/null +++ b/doc/examples/rtde_writer.rst @@ -0,0 +1,183 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_writer.rst + +.. _rtde_writer_example: + +RTDE writer example +=================== + +This example shows how to write several `Real-Time Data Exchange (RTDE) +`_ +inputs to the robot in a single package, at the robot's maximum frequency, and how to prove that +the robot processed them. + +The one-field ``send...()`` helpers on ``RTDEWriter`` each produce a package of their own. When +several general purpose registers have to change together, ``sendPackage()`` is the method that +puts them on the wire in one RTDE package. + +The example's source code can be found in `rtde_writer.cpp +`_. + +.. note:: The robot has to be powered on and, on an e-Series, in *remote control mode* for the + register-processing program to be accepted. + +Recipes as argument lists +------------------------- + +``RTDEClient`` takes the input and output recipes as two lists of field names. Recipe files work +as well; see :ref:`rtde_client_example`. ``timestamp`` is part of the output recipe either way, +because the client adds it if it is missing. + +The general purpose register ranges reserved for external RTDE clients are bit registers +``64..127`` and integer and double registers ``24..47``. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: const std::vector INPUT_RECIPE + :end-at: const std::string OUTPUT_DOUBLE_REGISTER + +.. note:: Register fields, unlike the digital and analog outputs and the speed slider, need no + companion ``_mask`` key in the input recipe. + +Processing the registers on the robot +------------------------------------- + +Input registers cannot be written from URScript, and output registers cannot be written through +RTDE. Getting values back therefore requires a program on the robot. + +The program does not copy the values. RTDE also exposes the input registers as outputs, so a +plain echo would be indistinguishable from that read-back. Instead the program inverts the bit, +adds one to the integer and negates the double. A value that satisfies those relations can only +have been produced by this program. ``sync()`` runs the loop once per control cycle. + +``sendScript()`` is used rather than ``sendScriptBlocking()``, because the latter would wait until +the program stops, and this one loops forever. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: const std::string MIRROR_PROGRAM + :end-at: end)"; + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // Start the robot program that processes the registers + :end-at: // The program keeps running until we stop it later. + +An input package with the robot's field types +--------------------------------------------- + +The data types of the input recipe belong to the robot and arrive with the handshake, so the +package has to be created after ``init()``. ``createInputDataPackage()`` returns a zeroed package +that already carries those types: ``setData()`` then rejects a wrong type immediately, and +copying the package into the send buffer is a single memcpy. + +A package constructed from ``getInputRecipe()`` still works. Its types are taken from the values +written to it and are only checked when the package is sent. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // RTDE client at the robot's maximum frequency + :end-at: my_client.start(false); + +``target_frequency = 0.0`` (the default) requests the robot's maximum: 125 Hz on CB3, 500 Hz on +e-Series. See :ref:`real time setup` and :ref:`rtde_client`. + +Both ``DataPackage`` objects are allocated before the loop, so the loop itself is allocation-free. +The output package is built from ``getOutputRecipe()`` and is therefore still untyped; the first +read applies the robot's types to it in place, which needs no memory. + +Letting the robot pace the loop +------------------------------- + +``start(false)`` leaves the background read thread off. ``getDataPackageBlocking()`` returns once +per RTDE cycle and is this loop's time base. The input package is produced immediately after the +read so it reaches the robot in time to be acted on in the next cycle. Printing is throttled to +about once per second, so it stays out of the hot path. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // The blocking read is this loop's clock + :end-at: URCL_LOG_ERROR("Could not get a fresh data package from the robot."); + +Writing several inputs in one package +------------------------------------- + +Unwritten fields of the package are sent as zeros. One ``sendPackage()`` produces exactly one +RTDE package; the ``send...()`` helpers would produce one package per field. The call only queues +the values for the writer thread, so the loop stays aligned to the robot. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // Writing several general purpose inputs in one package + :end-at: URCL_LOG_ERROR("Sending RTDE data failed."); + +Verifying that the robot processed the data +------------------------------------------- + +All three values sent in a cycle are derived from the cycle counter, so the integer the robot +returns identifies which cycle an answer belongs to. ``echoed_int - 1`` is that counter. The +expected bit is its inversion and the expected double is the negated sine. The robot's double +register is a 64-bit value, so the negated sine comes back bit for bit and is compared exactly. +Together with the inverted bit, that is what makes an answer attributable to this program rather +than to RTDE's own read-back of the input registers. + +Against URSim the lag is one cycle: the values written after the read of cycle N are processed by +the robot and observed in the read of cycle N+1. ``getData()`` needs a variable of the field's own +type; ``getDataType()`` reports that type if the recipe is not known in advance. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // Reading what the robot made of the previous package + :end-at: ++mismatches; + +Cleanup +------- + +The input registers are reset and the robot program is stopped. A failed stop is only logged, +because CI runs the example for one second and still requires exit code 0. + +.. literalinclude:: ../../examples/rtde_writer.cpp + :language: c++ + :caption: examples/rtde_writer.cpp + :linenos: + :lineno-match: + :start-at: // Reset the input registers before leaving + :end-at: return 0; + +Example output +-------------- + +The following shows a run against URSim 5.25.1 asking for 500 Hz. The echoed integer trails the +sent integer by one cycle, and ``verified=1`` means the bit and the double match the +transformations the robot program applies to that cycle. + +.. code:: + + [INFO] RTDE target frequency: 500.000000 Hz + sent: bit=1 int=484 double=-0.991869 | robot: bit=0 int=483 double=0.994216 | verified=1 lag_cycles=1 freq=483.063 Hz playing=1 + sent: bit=0 int=967 double=-0.242772 | robot: bit=1 int=966 double=0.223323 | verified=1 lag_cycles=1 freq=482.826 Hz playing=1 + sent: bit=1 int=1450 double=0.934895 | robot: bit=0 int=1449 double=-0.941806 | verified=1 lag_cycles=1 freq=482.669 Hz playing=1 + [INFO] Cycles: 1931, average frequency: 482.628400 Hz, verified: 1929, mismatches: 0, last lag: 1 cycles + +A simulator shares the host's CPU, so the measured frequency stays somewhat below the requested +one; on a real controller it tracks the target closely. diff --git a/doc/migration_notes.rst b/doc/migration_notes.rst index 51a29bddb..36d129378 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -23,7 +23,10 @@ Four consequences are worth knowing about: - **A wrongly typed input field is reported when the package is sent.** ``DataPackage::setData()`` decides a field's type from the value passed to it, so it can no longer tell on its own that the robot expects something else. ``RTDEWriter::sendPackage()`` checks the package against the robot's - answer and names the field and both types if they disagree. + answer and names the field and both types if they disagree. A typed input package is now also + available from ``RTDEClient::createInputDataPackage()`` after ``init()``, so filling several + input fields no longer depends on guessing the field types correctly; ``setData()`` then rejects + a mismatch immediately. - **Reading a field as the wrong type no longer throws.** ``DataPackage::getData()`` used to let a ``std::bad_variant_access`` escape when the passed variable didn't match the field's type. It now returns ``false`` and logs which type the robot reported for that field, matching what its diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 10007e75f..f870caac7 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -23,6 +23,10 @@ add_executable(rtde_client_example rtde_client.cpp) target_link_libraries(rtde_client_example ur_client_library::urcl) +add_executable(rtde_writer_example + rtde_writer.cpp) +target_link_libraries(rtde_writer_example ur_client_library::urcl) + add_executable(dashboard_example dashboard_example.cpp) target_link_libraries(dashboard_example ur_client_library::urcl) diff --git a/examples/rtde_writer.cpp b/examples/rtde_writer.cpp new file mode 100644 index 000000000..e2c4ed355 --- /dev/null +++ b/examples/rtde_writer.cpp @@ -0,0 +1,238 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace urcl; + +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; + +// RTDE recipes as argument lists, so this example needs no recipe files. RTDEClient also takes two +// filenames instead; see examples/rtde_client.cpp. The general purpose register ranges used here +// are the ones the RTDE guide reserves for external clients: bit registers 64..127, integer and +// double registers 24..47. Register fields need no companion "_mask" key. +const std::vector INPUT_RECIPE = { "input_bit_register_64", "input_int_register_24", + "input_double_register_24" }; +const std::vector OUTPUT_RECIPE = { "timestamp", "runtime_state", "output_bit_register_64", + "output_int_register_24", "output_double_register_24" }; + +// We write the inputs, the robot program below writes the outputs. +const std::string INPUT_BIT_REGISTER = "input_bit_register_64"; +const std::string INPUT_INT_REGISTER = "input_int_register_24"; +const std::string INPUT_DOUBLE_REGISTER = "input_double_register_24"; +const std::string OUTPUT_BIT_REGISTER = "output_bit_register_64"; +const std::string OUTPUT_INT_REGISTER = "output_int_register_24"; +const std::string OUTPUT_DOUBLE_REGISTER = "output_double_register_24"; + +// All three values we send are derived from the cycle counter, so the integer the robot returns +// identifies which cycle an answer belongs to. +const double SINE_INCREMENT = 0.01; // rad per cycle + +// Robot program processing the general purpose inputs and writing the results to the outputs. +// Input registers cannot be written from URScript and output registers cannot be written through +// RTDE, so getting values back requires a program on the robot. The program does not copy the +// values: it inverts the bit, adds one to the integer and negates the double. RTDE also offers the +// input registers as outputs, so a copy would be indistinguishable from that read-back, while a +// value satisfying this relation can only have been produced by this program. sync() runs the loop +// once per control cycle. +const std::string MIRROR_PROGRAM = R"(def rtde_register_mirror(): + while (True): + write_output_boolean_register(64, not read_input_boolean_register(64)) + write_output_integer_register(24, read_input_integer_register(24) + 1) + write_output_float_register(24, -1.0 * read_input_float_register(24)) + sync() + end +end)"; + +int main(int argc, char* argv[]) +{ + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + // Parse how may seconds to run + int second_to_run = -1; + if (argc > 2) + { + second_to_run = std::stoi(argv[2]); + } + + comm::INotifier notifier; + + // Start the robot program that processes the registers + primary_interface::PrimaryClient primary_client(robot_ip, notifier); + primary_client.start(); + try + { + primary_client.commandBrakeRelease(); + } + catch (const UrException& e) + { + URCL_LOG_WARN("Could not release the brakes: %s", e.what()); + } + if (!primary_client.sendScript(MIRROR_PROGRAM)) + { + URCL_LOG_WARN("Could not upload the register-processing program. Output registers will stay at " + "zero until a matching program is running on the robot."); + } + // The program keeps running until we stop it later. + + // RTDE client at the robot's maximum frequency + rtde_interface::RTDEClient my_client(robot_ip, notifier, OUTPUT_RECIPE, INPUT_RECIPE); + my_client.init(); + URCL_LOG_INFO("RTDE target frequency: %f Hz", my_client.getTargetFrequency()); + + // An input package carrying the data types the robot reported for the input recipe. Those types + // are only known once the RTDE handshake has run, which is why this is created after init(). + rtde_interface::DataPackage input_pkg = my_client.createInputDataPackage(); + // The output package is still untyped; the first read applies the robot's types to it in place. + auto output_pkg = std::make_unique(my_client.getOutputRecipe()); + + my_client.start(false); + + int32_t counter = 0; + size_t verified = 0; + size_t mismatches = 0; + int32_t last_lag_cycles = 0; + auto start_time = std::chrono::steady_clock::now(); + auto last_print = start_time; + + while (second_to_run <= 0 || + std::chrono::duration_cast(std::chrono::steady_clock::now() - start_time).count() < + second_to_run) + { + // The blocking read is this loop's clock + if (!my_client.getDataPackageBlocking(output_pkg)) + { + URCL_LOG_ERROR("Could not get a fresh data package from the robot."); + return 1; + } + const auto now = std::chrono::steady_clock::now(); + + // Reading what the robot made of the previous package + bool echoed_bit = false; + int32_t echoed_int = 0; + double echoed_double = 0.0; + uint32_t runtime_state = 0; + if (!output_pkg->getData(OUTPUT_BIT_REGISTER, echoed_bit) || + !output_pkg->getData(OUTPUT_INT_REGISTER, echoed_int) || + !output_pkg->getData(OUTPUT_DOUBLE_REGISTER, echoed_double) || + !output_pkg->getData("runtime_state", runtime_state)) + { + URCL_LOG_ERROR("Could not read the output registers from the received package."); + return 1; + } + + bool verified_this_cycle = false; + if (echoed_int > 1) + { + const int32_t origin = echoed_int - 1; // the counter value the robot processed + const bool expected_bit = !((origin % 2) == 0); + const double expected_double = -std::sin(origin * SINE_INCREMENT); + last_lag_cycles = counter - origin; + // The robot's double register is a 64-bit value, so the negated sine has to come back bit + // for bit. Together with the inverted bit that is the proof the robot processed this cycle. + if (echoed_bit == expected_bit && echoed_double == expected_double) + { + ++verified; + verified_this_cycle = true; + } + else + { + ++mismatches; + } + } + + // Writing several general purpose inputs in one package + ++counter; + const bool sent_bit = (counter % 2) == 0; + const double sent_double = std::sin(counter * SINE_INCREMENT); + bool write_ok = input_pkg.setData(INPUT_BIT_REGISTER, sent_bit); + write_ok = write_ok && input_pkg.setData(INPUT_INT_REGISTER, counter); + write_ok = write_ok && input_pkg.setData(INPUT_DOUBLE_REGISTER, sent_double); + if (!write_ok || !my_client.getWriter().sendPackage(input_pkg)) + { + URCL_LOG_ERROR("Sending RTDE data failed."); + return 1; + } + + if (now - last_print >= std::chrono::seconds(1)) + { + const double elapsed_s = std::chrono::duration(now - start_time).count(); + const double measured_hz = elapsed_s > 0.0 ? static_cast(counter) / elapsed_s : 0.0; + const bool program_playing = + static_cast(runtime_state) == rtde_interface::RUNTIME_STATE::PLAYING; + std::cout << "sent: bit=" << sent_bit << " int=" << counter << " double=" << sent_double + << " | robot: bit=" << echoed_bit << " int=" << echoed_int << " double=" << echoed_double + << " | verified=" << verified_this_cycle << " lag_cycles=" << last_lag_cycles << " freq=" << measured_hz + << " Hz playing=" << program_playing << std::endl; + if (echoed_int == 0) + { + std::cout << "No processed values yet. Is the register-processing program running on the robot?" << std::endl; + } + last_print = now; + } + } + + const double elapsed_s = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); + const double average_hz = elapsed_s > 0.0 ? static_cast(counter) / elapsed_s : 0.0; + URCL_LOG_INFO("Cycles: %d, average frequency: %f Hz, verified: %zu, mismatches: %zu, last lag: %d cycles", counter, + average_hz, verified, mismatches, last_lag_cycles); + + // Reset the input registers before leaving + input_pkg.setData(INPUT_BIT_REGISTER, false); + input_pkg.setData(INPUT_INT_REGISTER, static_cast(0)); + input_pkg.setData(INPUT_DOUBLE_REGISTER, 0.0); + my_client.getWriter().sendPackage(input_pkg); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + try + { + primary_client.commandStop(false); + } + catch (const UrException& e) + { + URCL_LOG_WARN("Could not stop the robot program: %s", e.what()); + } + + return 0; +} diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 212de6e86..9253bb713 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -198,6 +198,16 @@ class DataPackage : public RTDEPackage */ void initEmpty(); + /*! + * \brief A package with this one's recipe, protocol version and data types, all values zero. + * + * Since it carries the same field names and types, it has this package's layout hash and can be + * copied into it with a single memcpy; see copyFrom(). + * + * \returns A zeroed package with this package's layout + */ + DataPackage emptyCopy() const; + /*! * \brief Get the data type the robot reported for a field. * diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 92d3693a2..6adc25f7c 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -209,9 +209,11 @@ class RTDEClient * * \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. + * that was built from the same output recipe as used in this RTDEClient. Such a package needs no + * data types of its own: the first read applies the ones the robot reported, which allocates + * nothing. If the package was built from a different recipe, or none is passed at all, 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 */ @@ -282,6 +284,20 @@ class RTDEClient return input_recipe_; } + /*! + * \brief Creates a data package for the input recipe, carrying the data types the robot reported + * during the RTDE handshake. + * + * Fill it with DataPackage::setData() and hand it to RTDEWriter::sendPackage() to write several + * inputs in a single package. Has to be called after init(). + * + * \throws UrException if the robot hasn't acknowledged the input recipe yet + */ + DataPackage createInputDataPackage() + { + return writer_.createDataPackage(); + } + /// 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); diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index ed0ad2c85..71e18ba2e 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -19,6 +19,7 @@ */ #pragma once +#include #include #include "ur_client_library/comm/parser.h" #include "ur_client_library/comm/bin_parser.h" @@ -68,6 +69,8 @@ class RTDEParser : public comm::Parser * 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. + * A DataPackage built from the registered recipe is not replaced even when it carries no data types yet: those are + * applied to it in place, which allocates nothing. Only a DataPackage built from a different recipe is replaced. * * \returns True, if the byte stream could successfully be parsed as an RTDE package, false * otherwise @@ -109,17 +112,19 @@ class RTDEParser : public comm::Parser void setRecipeTypes(const std::vector& types) { recipe_types_ = types; - typed_layout_hash_ = DataPackage::layoutHashFor(recipe_, recipe_types_); + // A package carrying the negotiated layout. Its hashes are the reference a passed-in package is + // held against, and it is the blueprint for any package this parser has to allocate itself. + typed_template_.emplace(recipe_); + typed_template_->setTypes(recipe_types_); } private: - static std::unique_ptr makeTypedDataPackage(const std::vector& recipe, - const std::vector& types); + std::unique_ptr makeTypedDataPackage() const; bool parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const; std::vector recipe_; std::vector recipe_types_; - uint64_t typed_layout_hash_ = 0; + std::optional typed_template_; bool recipeTypesKnown() const; PackageType getPackageTypeFromHeader(comm::BinParser& bp) const; RTDEPackage* createNewPackageFromType(PackageType type) const; diff --git a/include/ur_client_library/rtde/rtde_writer.h b/include/ur_client_library/rtde/rtde_writer.h index 485752dac..52af8d8ee 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -105,6 +105,21 @@ class RTDEWriter */ bool sendPackage(const DataPackage& package); + /*! + * \brief Creates a data package for the input recipe, carrying the data types the robot reported + * for it. + * + * The returned package has all values at zero and is ready to be filled with + * DataPackage::setData(). Since it already carries the robot's types, a value written with a wrong + * type is reported by setData() itself rather than only when the package is sent, and copying the + * package into the send buffer is a single memcpy. + * + * \returns A package built from the input recipe with the acknowledged data types applied + * + * \throws UrException if the robot hasn't acknowledged the input recipe yet + */ + DataPackage createDataPackage(); + /*! * \brief Creates a package to request setting a new value for the speed slider. * diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index cba3a87a0..7752b1a9a 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -358,6 +358,17 @@ void rtde_interface::DataPackage::initEmpty() copyValues(values_, zeros_); } +rtde_interface::DataPackage rtde_interface::DataPackage::emptyCopy() const +{ + // The delegated constructor allocates the storage, builds the name-to-index map and computes the + // recipe hash; the field types and their zero values are what this package contributes. + DataPackage package(recipe_, protocol_version_); + package.values_ = zeros_; + package.zeros_ = zeros_; + package.updateLayoutHash(); + return package; +} + void rtde_interface::DataPackage::reportSlowCopyOnce() { if (slow_copy_reported_) diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 3b562a8ff..dc6d8a1dd 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -27,14 +27,11 @@ namespace urcl { namespace rtde_interface { -// A package allocates its storage from the recipe and learns its field types from the robot's -// acknowledgement afterwards, which costs no memory. -std::unique_ptr RTDEParser::makeTypedDataPackage(const std::vector& recipe, - const std::vector& types) +// Only reached when the caller didn't hand in a package we can use. Copying the template gives the +// negotiated recipe and data types without having to reapply them. +std::unique_ptr RTDEParser::makeTypedDataPackage() const { - auto package = std::make_unique(recipe); - package->setTypes(types); - return package; + return std::make_unique(*typed_template_); } bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const @@ -50,7 +47,7 @@ bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& packa bool RTDEParser::recipeTypesKnown() const { - if (recipe_types_.size() == recipe_.size()) + if (typed_template_.has_value()) { return true; } @@ -90,7 +87,7 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package = makeTypedDataPackage(recipe_, recipe_types_); + std::unique_ptr package = makeTypedDataPackage(); if (!parseDataPackagePayload(bp, *package)) { @@ -159,16 +156,26 @@ bool RTDEParser::parse(comm::BinParser& bp, std::unique_ptr& result "a DataPackage would be sent.", result->getType()); } - result = makeTypedDataPackage(recipe_, recipe_types_); + result = makeTypedDataPackage(); } DataPackage* data_package = dynamic_cast(result.get()); - if (data_package->layoutHash() != typed_layout_hash_) + if (data_package->layoutHash() != typed_template_->layoutHash()) { - URCL_LOG_WARN("The passed pre-allocated DataPackage does not have the negotiated output layout. A new " - "DataPackage will have to be allocated."); - result = makeTypedDataPackage(recipe_, recipe_types_); - data_package = dynamic_cast(result.get()); + if (data_package->recipeHash() == typed_template_->recipeHash()) + { + // Built from our recipe, so its storage is already the right shape and only the data + // types are missing or stale. Applying them writes into that storage without allocating, + // which is what lets an application hand in a package it built from the recipe alone. + data_package->setTypes(recipe_types_); + } + else + { + URCL_LOG_WARN("The passed pre-allocated DataPackage was built from a different recipe. A new DataPackage " + "will have to be allocated."); + result = makeTypedDataPackage(); + data_package = dynamic_cast(result.get()); + } } if (!parseDataPackagePayload(bp, *data_package)) diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index b125ee386..e5bf61bbb 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -28,6 +28,7 @@ #include "ur_client_library/rtde/rtde_writer.h" #include +#include "ur_client_library/exceptions.h" #include "ur_client_library/log.h" namespace urcl @@ -179,6 +180,17 @@ bool RTDEWriter::sendPackage(const DataPackage& package) return true; } +DataPackage RTDEWriter::createDataPackage() +{ + std::lock_guard guard(store_mutex_); + if (current_store_buffer_ == nullptr || !current_store_buffer_->isTyped()) + { + throw UrException("Cannot create an RTDE input data package before the robot has acknowledged the input recipe. " + "That happens during the RTDE handshake, so call this after RTDEClient::init()."); + } + return current_store_buffer_->emptyCopy(); +} + bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) { if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0) diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index 25d45c2f2..1c0b5c96f 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -237,6 +237,39 @@ TEST(DataPackageAllocationTest, parsing_a_preallocated_package_does_not_allocate EXPECT_DOUBLE_EQ(timestamp, 16412.206); } +// The very first parse into a package an application built from the recipe alone. Its types are +// still missing, and applying them has to happen in place, or a real-time loop would take an +// allocation on its first read. +TEST(DataPackageAllocationTest, parsing_into_an_untyped_package_does_not_allocate) +{ + 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.setRecipeTypes({ "DOUBLE", "DOUBLE" }); + parser.setProtocolVersion(2); + std::unique_ptr product = std::make_unique(recipe); + const rtde_interface::RTDEPackage* package_address = product.get(); + + std::size_t allocations = 0; + bool parsed = false; + { + AllocationCounter counter; + comm::BinParser bp(raw_data, sizeof(raw_data)); + parsed = parser.parse(bp, product); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(parsed); + EXPECT_EQ(product.get(), package_address); + rtde_interface::DataPackage* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 16412.206); +} + TEST(DataPackageAllocationTest, serializing_a_typed_package_does_not_allocate) { auto package = test::typedPackage({ "speed_slider_mask" }, { "UINT32" }); @@ -372,7 +405,7 @@ TEST_F(RTDEAllocationTest, background_receive_does_not_allocate) TEST_F(RTDEAllocationTest, sending_input_data_does_not_allocate) { ASSERT_TRUE(client_->start(true)); - rtde_interface::DataPackage input_pkg(client_->getInputRecipe()); + rtde_interface::DataPackage input_pkg = client_->createInputDataPackage(); ASSERT_TRUE(input_pkg.setData("speed_slider_mask", static_cast(1))); for (int i = 0; i < g_WARMUP_CYCLES; ++i) diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index 154469b82..a98372024 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -541,6 +541,44 @@ TEST(rtde_data_package, init_empty_keeps_types) EXPECT_DOUBLE_EQ(timestamp, 0.0); } +// emptyCopy() is the layout of this package with every value taken from zeros_, so writing here +// must not leak into the copy and the copy must keep the same hashes. +TEST(rtde_data_package, empty_copy_keeps_the_layout_and_zeroes_the_values) +{ + auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + const uint64_t recipe = package.recipeHash(); + const uint64_t layout = package.layoutHash(); + + const rtde_interface::DataPackage copy = package.emptyCopy(); + + EXPECT_TRUE(copy.isTyped()); + EXPECT_EQ(copy.recipeHash(), recipe); + EXPECT_EQ(copy.layoutHash(), layout); + EXPECT_EQ(copy.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + EXPECT_EQ(copy.getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + double timestamp = 1.0; + ASSERT_TRUE(copy.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 0.0); + vector6d_t actual_q{ 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + ASSERT_TRUE(copy.getData("actual_q", actual_q)); + EXPECT_EQ(actual_q, vector6d_t({ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 })); + + timestamp = 0.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +TEST(rtde_data_package, empty_copy_of_an_untyped_package_is_untyped) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + + const rtde_interface::DataPackage copy = package.emptyCopy(); + + EXPECT_FALSE(package.isTyped()); + EXPECT_FALSE(copy.isTyped()); +} + TEST(rtde_data_package, copy_keeps_types_and_values) { auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); diff --git a/tests/test_rtde_parser.cpp b/tests/test_rtde_parser.cpp index 3e8526b0d..4a357d710 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -237,7 +237,9 @@ TEST(rtde_parser, data_package_without_recipe_types_fails) EXPECT_FALSE(parser.parse(bp, product)); } -TEST(rtde_parser, untyped_pre_allocated_data_package_is_replaced) +// A package built from the recipe alone carries the right storage and only lacks its types, so the +// parser applies them to it rather than allocating a replacement. +TEST(rtde_parser, untyped_pre_allocated_data_package_is_typed_in_place) { unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; @@ -249,18 +251,22 @@ TEST(rtde_parser, untyped_pre_allocated_data_package_is_replaced) parser.setProtocolVersion(2); std::unique_ptr product = std::make_unique(recipe); + const rtde_interface::RTDEPackage* package_address = product.get(); ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); rtde_interface::DataPackage* data = dynamic_cast(product.get()); ASSERT_NE(data, nullptr); + EXPECT_EQ(data->getDataType("timestamp"), rtde_interface::DataType::DOUBLE); double timestamp = 0.0; ASSERT_TRUE(data->getData("timestamp", timestamp)); EXPECT_DOUBLE_EQ(timestamp, 16412.206); } -// setData() on every field makes isTyped() true, but those types did not come from the robot. -TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_replaced) +// setData() on every field makes isTyped() true, but those types did not come from the robot. The +// recipe still matches, so the robot's types overwrite them in place. +TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_retyped_in_place) { unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; @@ -276,8 +282,10 @@ TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_replaced) ASSERT_TRUE(package->setData("target_speed_fraction", static_cast(2))); std::unique_ptr product = std::move(package); + const rtde_interface::RTDEPackage* package_address = product.get(); ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); rtde_interface::DataPackage* data = dynamic_cast(product.get()); ASSERT_NE(data, nullptr); diff --git a/tests/test_rtde_writer.cpp b/tests/test_rtde_writer.cpp index b32b3424b..c3e8b169e 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -582,6 +582,47 @@ TEST_F(RTDEWriterTest, send_data_package_typed_by_the_robot) EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); } +// Values sitting in the store buffer must not leak into a newly created package; emptyCopy() +// builds from zeros_, not from the live values. +TEST_F(RTDEWriterTest, create_data_package_is_typed_and_zeroed) +{ + ASSERT_TRUE(writer_->sendSpeedSlider(0.7)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + rtde_interface::DataPackage data_package = writer_->createDataPackage(); + EXPECT_TRUE(data_package.isTyped()); + double speed_slider_fraction = 1.0; + ASSERT_TRUE(data_package.getData("speed_slider_fraction", speed_slider_fraction)); + EXPECT_DOUBLE_EQ(speed_slider_fraction, 0.0); +} + +// Once the package carries the robot's types, a mismatch is reported by setData() itself. +TEST_F(RTDEWriterTest, create_data_package_rejects_a_wrong_type_immediately) +{ + rtde_interface::DataPackage data_package = writer_->createDataPackage(); + EXPECT_FALSE(data_package.setData("speed_slider_mask", static_cast(1))); +} + +TEST_F(RTDEWriterTest, send_data_package_created_by_the_writer) +{ + rtde_interface::DataPackage data_package = writer_->createDataPackage(); + ASSERT_TRUE(data_package.setData("standard_analog_output_0", 0.4)); + + EXPECT_TRUE(writer_->sendPackage(data_package)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + ASSERT_TRUE(dataFieldExist("standard_analog_output_0")); + EXPECT_EQ(std::get(parsed_data_["standard_analog_output_0"]), 0.4); + ASSERT_TRUE(dataFieldExist("speed_slider_fraction")); + EXPECT_EQ(std::get(parsed_data_["speed_slider_fraction"]), 0.0); +} + +TEST_F(RTDEWriterTest, create_data_package_before_types_are_known_throws) +{ + rtde_interface::RTDEWriter writer(stream_.get(), input_recipe_); + EXPECT_THROW(writer.createDataPackage(), UrException); +} + // A package has to be built from the recipe that was registered, since the fallback copies // position by position and cannot map a subset onto a larger recipe. TEST_F(RTDEWriterTest, send_data_package_built_from_a_partial_recipe_fails) From b873a4a4d14ec70aa70a4479794aa4a2efbc1035 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 17:28:11 +0200 Subject: [PATCH 13/18] Remove unused DataPackage::layoutHashFor(). The parser now takes the layout identity from its typed template package, so this static helper had no production caller left. --- include/ur_client_library/rtde/data_package.h | 12 ------------ src/rtde/data_package.cpp | 19 ------------------- tests/test_rtde_data_package.cpp | 2 -- 3 files changed, 33 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 9253bb713..df8bc47d4 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -446,18 +446,6 @@ class DataPackage : public RTDEPackage return layout_hash_; } - /*! - * \brief The layout hash a package would have after applying \p types to \p recipe. - * - * Used by the parser to recognise a package that already carries the negotiated output layout. - * - * \param recipe Field names, in order - * \param types Data type names as reported by the robot, in the same order as \p recipe - * - * \throws UrException if the number of types doesn't match the recipe or if a type is unknown - */ - static uint64_t layoutHashFor(const std::vector& recipe, const std::vector& types); - private: /*! * \brief Logs once that a copy walked fields instead of memcpy'ing the value array. diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 7752b1a9a..6bb104c3e 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -316,25 +316,6 @@ void rtde_interface::DataPackage::updateLayoutHash() }); } -uint64_t rtde_interface::DataPackage::layoutHashFor(const std::vector& recipe, - const std::vector& types) -{ - if (types.size() != recipe.size()) - { - std::stringstream ss; - ss << "Cannot compute the layout hash of an RTDE data package: got " << types.size() - << " data types for a recipe with " << recipe.size() << " fields."; - throw UrException(ss.str()); - } - - uint64_t hash = hashRecipe(recipe); - for (const auto& type_name : types) - { - hash = fnv1aByte(hash, static_cast(variantFromTypeName(type_name).index())); - } - return hash; -} - void rtde_interface::DataPackage::setTypes(const std::vector& types) { if (types.size() != recipe_.size()) diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index a98372024..dacdd1701 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -613,8 +613,6 @@ TEST(rtde_data_package, layout_hash_changes_when_types_are_set) EXPECT_EQ(package.recipeHash(), recipe); EXPECT_NE(package.layoutHash(), untyped); - EXPECT_EQ(package.layoutHash(), - rtde_interface::DataPackage::layoutHashFor({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" })); } TEST(rtde_data_package, layout_hash_changes_on_first_set_data_to_an_untyped_field) From 029a87177e01b24f098a8c96bd1632cc34c4c22d Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 17:55:13 +0200 Subject: [PATCH 14/18] Join the fake RTDE server's worker before destroying the mutexes it locks. ~TCPServer ran after those members were gone, which aborted the handshake-retry test on macOS arm64 with mutex lock failed: Invalid argument. --- tests/fake_rtde_server.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 8eb4faf3a..91a70a50a 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -505,7 +505,11 @@ RTDEServer::RTDEServer(const int port) : server_(port) RTDEServer::~RTDEServer() { + // The TCP worker calls handlePackage() and the disconnect callback, both of which lock + // mutexes declared after server_. Join that thread here so those mutexes are still alive. + // ~TCPServer would otherwise do it too late, after the mutexes have already been destroyed. stopSendingDataPackages(); + server_.shutdown(); } void RTDEServer::queueTextMessageBeforeVersionReply(const std::string& message) From ea69eb998330c45469da374d77afcb8f02793074 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 19:59:28 +0200 Subject: [PATCH 15/18] Restore parseWith's protocol-aware payload and keep setTypes transactional. A v2 caller passing the same bytes as before would otherwise read the recipe-id as field data. Type names are validated before any field is written, and the slow-copy warning waits until destruction so a partial send does not allocate. --- include/ur_client_library/rtde/data_package.h | 19 +++--- include/ur_client_library/rtde/rtde_parser.h | 5 ++ src/rtde/data_package.cpp | 51 ++++++++------ src/rtde/rtde_parser.cpp | 10 ++- tests/fake_rtde_server.cpp | 5 -- tests/test_rtde_allocations.cpp | 23 +++++++ tests/test_rtde_data_package.cpp | 67 +++++++++++-------- 7 files changed, 114 insertions(+), 66 deletions(-) diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index df8bc47d4..bee17d5cf 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -189,7 +189,7 @@ class DataPackage : public RTDEPackage initStorage(); } - virtual ~DataPackage() = default; + virtual ~DataPackage(); /*! * \brief Resets every data field to a default-constructed value of its own type. @@ -226,6 +226,10 @@ class DataPackage : public RTDEPackage * \brief Sets the attributes of the package by parsing a serialized representation of the * package. * + * The payload is the bytes after the package header. Version 2 data packages start with a + * recipe-id byte; version 1 packages do not. That is the same layout serializePackage() writes + * after the header. + * * \param bp A parser containing a serialized version of the package * * \returns True, if the package was parsed successfully, false otherwise @@ -375,7 +379,8 @@ class DataPackage : public RTDEPackage * * \param types The data types of the recipe's fields, in the same order as the recipe * - * \throws UrException if the number of types doesn't match the recipe or if a type is unknown + * \throws UrException if the number of types doesn't match the recipe or if a type is unknown. + * Every name is checked before any field is written, so a failure leaves the package unchanged. */ void setTypes(const std::vector& types); @@ -391,7 +396,8 @@ class DataPackage : public RTDEPackage * of the value array. The hashes are a 64-bit identity of the field names and each field's * variant index; a collision would skip a validation that should have failed, which is accepted * for this path. A package an application typed by writing only the fields it cares about is - * instead merged position by position, with unwritten fields sent as zeros. + * instead merged position by position, with unwritten fields sent as zeros. That slower path is + * noted when this package is destroyed, so the real-time copy itself does not log. * * \param other The package to copy from * @@ -447,11 +453,6 @@ class DataPackage : public RTDEPackage } private: - /*! - * \brief Logs once that a copy walked fields instead of memcpy'ing the value array. - */ - void reportSlowCopyOnce(); - /*! * \brief Allocates one slot per recipe field, with the type left undecided. */ @@ -488,7 +489,7 @@ class DataPackage : public RTDEPackage uint64_t recipe_hash_ = 0; uint64_t layout_hash_ = 0; bool fully_typed_ = false; - bool slow_copy_reported_ = false; + bool used_slow_copy_ = false; }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 71e18ba2e..a15ad8bc4 100644 --- a/include/ur_client_library/rtde/rtde_parser.h +++ b/include/ur_client_library/rtde/rtde_parser.h @@ -93,6 +93,10 @@ class RTDEParser : public comm::Parser void setProtocolVersion(uint16_t protocol_version) { protocol_version_ = protocol_version; + if (typed_template_.has_value()) + { + typed_template_->setProtocolVersion(protocol_version); + } } uint16_t getProtocolVersion() const @@ -116,6 +120,7 @@ class RTDEParser : public comm::Parser // held against, and it is the blueprint for any package this parser has to allocate itself. typed_template_.emplace(recipe_); typed_template_->setTypes(recipe_types_); + typed_template_->setProtocolVersion(protocol_version_); } private: diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 6bb104c3e..333b2a715 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -207,20 +207,20 @@ DataPackage::_rtde_type_variant variantFor(const DataType type) } /*! - * \brief Creates an empty value of the RTDE data type with the given name. + * \brief The protocol data type with the given name. * * \param type_name One of the RTDE data type names as reported by the robot in a setup * acknowledgement * * \throws UrException if the name is not a known RTDE data type */ -DataPackage::_rtde_type_variant variantFromTypeName(const std::string_view type_name) +DataType typeFromName(const std::string_view type_name) { for (const auto& entry : g_type_names) { if (entry.name == type_name) { - return variantFor(entry.type); + return entry.type; } } @@ -326,9 +326,17 @@ void rtde_interface::DataPackage::setTypes(const std::vector& types throw UrException(ss.str()); } + // Confirm every name before writing any field. variantFor cannot fail once the name is known, so + // a later unknown type cannot leave earlier fields retyped while layout_hash_ still describes + // the old layout. + for (const auto& type_name : types) + { + typeFromName(type_name); + } + for (size_t i = 0; i < recipe_.size(); ++i) { - values_[i] = variantFromTypeName(types[i]); + values_[i] = variantFor(typeFromName(types[i])); zeros_[i] = values_[i]; } updateLayoutHash(); @@ -350,20 +358,6 @@ rtde_interface::DataPackage rtde_interface::DataPackage::emptyCopy() const return package; } -void rtde_interface::DataPackage::reportSlowCopyOnce() -{ - if (slow_copy_reported_) - { - return; - } - slow_copy_reported_ = true; - URCL_LOG_WARN("Copying an RTDE data package that is not fully typed walks each field instead of " - "copying the value array in one step. That is the path a package takes when it is " - "constructed from a recipe and only some of its fields are written. A package that " - "already carries the same field names and types as this one can be copied in one " - "memcpy."); -} - bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) { if (!isTyped()) @@ -405,10 +399,22 @@ bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) values_[i] = std::holds_alternative(other.values_[i]) ? zeros_[i] : other.values_[i]; } - reportSlowCopyOnce(); + used_slow_copy_ = true; return true; } +rtde_interface::DataPackage::~DataPackage() +{ + if (used_slow_copy_) + { + URCL_LOG_WARN("Copied an RTDE data package that was not fully typed by walking each field " + "instead of copying the value array in one step. That is the path a package takes " + "when it is constructed from a recipe and only some of its fields are written. A " + "package that already carries the same field names and types as this one can be " + "copied in one memcpy."); + } +} + bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) { if (!isTyped()) @@ -418,6 +424,13 @@ bool rtde_interface::DataPackage::parseWith(comm::BinParser& bp) return false; } + // Same contract as serializePackage(): the bytes after the package header, so a version 2 + // payload starts with the recipe-id byte. + if (protocol_version_ == 2) + { + bp.parse(recipe_id_); + } + for (size_t i = 0; i < recipe_.size(); ++i) { std::visit( diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index dc6d8a1dd..23c01e753 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -36,12 +36,10 @@ std::unique_ptr RTDEParser::makeTypedDataPackage() const bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const { - if (protocol_version_ == 2) - { - uint8_t recipe_id = 0; - bp.parse(recipe_id); - package.setRecipeID(recipe_id); - } + // A package an application built from the recipe alone defaults to protocol version 2. The + // negotiated version lives on the parser, so apply it before parseWith() decides whether the + // payload starts with a recipe-id byte. + package.setProtocolVersion(protocol_version_); return package.parseWith(bp); } diff --git a/tests/fake_rtde_server.cpp b/tests/fake_rtde_server.cpp index 91a70a50a..3f17f6e25 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -825,11 +825,6 @@ void RTDEServer::handlePackage(const socket_t filedescriptor, rtde_interface::Pa throw std::runtime_error("Fake RTDE Server received a data package before input recipe was setup. This should " "not happen."); } - if (negotiated_protocol_version_ == 2) - { - uint8_t recipe_id = 0; - bp.parse(recipe_id); - } input_data_package_->parseWith(bp); actOnInput(); break; diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp index 1c0b5c96f..d00c50526 100644 --- a/tests/test_rtde_allocations.cpp +++ b/tests/test_rtde_allocations.cpp @@ -270,6 +270,29 @@ TEST(DataPackageAllocationTest, parsing_into_an_untyped_package_does_not_allocat EXPECT_DOUBLE_EQ(timestamp, 16412.206); } +// The existing pattern of constructing an input package from the recipe and setting only the +// fields that change. The copy itself must not log; the warning is deferred until the destination +// is destroyed. +TEST(DataPackageAllocationTest, copying_a_partial_package_does_not_allocate) +{ + auto destination = test::typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + + setLogLevel(LogLevel::INFO); + std::size_t allocations = 0; + bool copied = false; + { + AllocationCounter counter; + copied = destination.copyFrom(source); + allocations = counter.count(); + } + setLogLevel(LogLevel::ERROR); + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(copied); +} + TEST(DataPackageAllocationTest, serializing_a_typed_package_does_not_allocate) { auto package = test::typedPackage({ "speed_slider_mask" }, { "UINT32" }); diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index dacdd1701..ca4af39ec 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -27,11 +27,8 @@ //---------------------------------------------------------------------- #include -#include -#include #include -#include #include "rtde_test_helpers.h" @@ -67,11 +64,11 @@ TEST(rtde_data_package, parse_pkg_protocolv2) std::vector types{ "DOUBLE", "VECTOR6D" }; auto package = typedPackage(recipe, types); - // Field payload only. The parser consumes the v2 recipe-id byte before parseWith(). - 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, - 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, - 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; + // Payload after the package header: recipe-id byte, then the fields. + 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, + 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, 0x85, 0x87, + 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; comm::BinParser bp(data_package, sizeof(data_package)); @@ -100,27 +97,21 @@ TEST(rtde_data_package, parse_pkg_protocolv1) { std::vector recipe{ "timestamp", "actual_q" }; std::vector types{ "DOUBLE", "VECTOR6D" }; + auto package = typedPackage(recipe, types); + package.setProtocolVersion(1); - // Full v1 package: header then fields, no recipe-id. The parser owns that distinction. - uint8_t data_package[] = { 0x00, 0x3b, 0x55, 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, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, + // Payload after the package header: fields only, no recipe-id. + 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, + 0x68, 0x88, 0x5a, 0x30, 0xbf, 0xe9, 0xdb, 0x22, 0xa2, 0x21, 0x68, 0xc0, 0x3f, 0xf9, 0x85, 0x87, 0xa0, 0x00, 0x00, 0x00, 0xbf, 0x9f, 0xbe, 0x74, 0x44, 0x2d, 0x18, 0x00 }; comm::BinParser bp(data_package, sizeof(data_package)); - rtde_interface::RTDEParser parser(recipe); - parser.setRecipeTypes(types); - parser.setProtocolVersion(1); - - std::unique_ptr product = std::make_unique(recipe); - ASSERT_TRUE(parser.parse(bp, product)); - - rtde_interface::DataPackage* package = dynamic_cast(product.get()); - ASSERT_NE(package, nullptr); + EXPECT_TRUE(package.parseWith(bp)); vector6d_t expected_q = { -1.6007, -1.7271, -2.203, -0.808, 1.5951, -0.031 }; vector6d_t actual_q; - package->getData("actual_q", actual_q); + package.getData("actual_q", actual_q); double abs = 1e-4; EXPECT_NEAR(expected_q[0], actual_q[0], abs); @@ -132,7 +123,7 @@ TEST(rtde_data_package, parse_pkg_protocolv1) double expected_timestamp = 16854.1919; double actual_timestamp; - package->getData("timestamp", actual_timestamp); + package.getData("timestamp", actual_timestamp); EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); } @@ -345,9 +336,8 @@ TEST(rtde_data_package, every_rtde_data_type_survives_a_serialize_parse_round_tr EXPECT_EQ(buffer[header_size + i], expected_integers[i]) << "at payload byte " << i; } - // serializePackage() writes the v2 recipe-id after the header; parseWith() starts at the fields. - const size_t recipe_id_size = sizeof(uint8_t); - comm::BinParser bp(buffer + header_size + recipe_id_size, size - header_size - recipe_id_size); + // serializePackage() writes the v2 recipe-id after the header; parseWith() consumes it too. + comm::BinParser bp(buffer + header_size, size - header_size); auto received = typedPackage(recipe, types); ASSERT_TRUE(received.parseWith(bp)); EXPECT_TRUE(bp.empty()) << "the parser did not consume exactly what was serialized"; @@ -398,6 +388,28 @@ TEST(rtde_data_package, unknown_data_types_are_rejected) EXPECT_THROW(package.setTypes({ "double" }), UrException); } +TEST(rtde_data_package, failed_set_types_leaves_the_package_unchanged) +{ + auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + const uint64_t layout = package.layoutHash(); + + EXPECT_THROW(package.setTypes({ "UINT64", "NOT_A_TYPE" }), UrException); + + EXPECT_EQ(package.layoutHash(), layout); + EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + EXPECT_EQ(package.getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + double timestamp = 0.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + + auto other = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(other.setData("timestamp", 1.0)); + ASSERT_TRUE(package.copyFrom(other)); + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); +} + TEST(rtde_data_package, type_count_has_to_match_recipe) { std::vector recipe{ "timestamp", "actual_q" }; @@ -640,7 +652,8 @@ TEST(rtde_data_package, layout_hash_does_not_change_on_reset_init_empty_or_parse package.initEmpty(); EXPECT_EQ(package.layoutHash(), hash); - uint8_t data[] = { 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + uint8_t data[] = { 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, 0x9f, 0xbe, + 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; comm::BinParser bp(data, sizeof(data)); ASSERT_TRUE(package.parseWith(bp)); EXPECT_EQ(package.layoutHash(), hash); From de0b6cfdb66f978f5ad0ccae5ef0417d7dba9523 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rune=20S=C3=B8e-Knudsen?= <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 21:54:55 +0200 Subject: [PATCH 16/18] Apply batched suggestions from code review Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/rtde/rtde_writer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rtde/rtde_writer.cpp b/src/rtde/rtde_writer.cpp index e5bf61bbb..09b3ebf72 100644 --- a/src/rtde/rtde_writer.cpp +++ b/src/rtde/rtde_writer.cpp @@ -183,7 +183,7 @@ bool RTDEWriter::sendPackage(const DataPackage& package) DataPackage RTDEWriter::createDataPackage() { std::lock_guard guard(store_mutex_); - if (current_store_buffer_ == nullptr || !current_store_buffer_->isTyped()) + if (current_store_buffer_ == nullptr || !running_ || !current_store_buffer_->isTyped()) { throw UrException("Cannot create an RTDE input data package before the robot has acknowledged the input recipe. " "That happens during the RTDE handshake, so call this after RTDEClient::init()."); From 49846f49ac205be6232cd24004e4fca589b8805f Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 22:09:51 +0200 Subject: [PATCH 17/18] Make RTDE client state atomic and document getDataType as the stored type. Reconnect writes that state from another thread, so polling it was a data race. getDataType already reports a type setData() established, which the docs now match. --- doc/architecture/rtde_client.rst | 9 +++++---- examples/rtde_writer.cpp | 2 +- include/ur_client_library/rtde/data_package.h | 9 +++++---- include/ur_client_library/rtde/rtde_client.h | 6 ++++-- tests/test_rtde_client_reconnect.cpp | 10 +++++++--- 5 files changed, 22 insertions(+), 14 deletions(-) diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index 7928ef6fa..09605aa0a 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -90,16 +90,17 @@ Field data types ``getData()`` has to be given a variable of the field's own type, and returns ``false`` if it isn't. Rather than hardcoding which type a field has, ask the package: ``getDataType()`` reports the -``DataType`` the robot gave a field, or nothing at all if the recipe hasn't been acknowledged yet. -This is useful for code that has to handle whatever recipe it is configured with, such as a bridge -to another middleware: +``DataType`` a field currently holds. After acknowledgement that is the type the robot reported; +on an input package written with ``setData()`` before then, it is the type of that write. An +untouched field has no type. This is useful for code that has to handle whatever recipe it is +configured with, such as a bridge to another middleware: .. code-block:: c++ const std::optional type = data_pkg.getDataType(field_name); if (!type) { - // Not part of the recipe, or the recipe hasn't been acknowledged yet + // Not part of the recipe, or the field has no type yet return; } diff --git a/examples/rtde_writer.cpp b/examples/rtde_writer.cpp index e2c4ed355..92e91e5af 100644 --- a/examples/rtde_writer.cpp +++ b/examples/rtde_writer.cpp @@ -90,7 +90,7 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } - // Parse how may seconds to run + // Parse how many seconds to run int second_to_run = -1; if (argc > 2) { diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index bee17d5cf..f0a0ca326 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -209,11 +209,12 @@ class DataPackage : public RTDEPackage DataPackage emptyCopy() const; /*! - * \brief Get the data type the robot reported for a field. + * \brief Get the data type a field currently holds. * - * Which type a field holds is decided by the robot when it acknowledges the recipe, so this is - * the way to find out what to pass to getData() without hardcoding it. A package that hasn't - * been acknowledged yet has no answer to give. + * After the robot acknowledges the recipe this is the type it reported, which is how to find + * out what to pass to getData() without hardcoding it. On an input package, setData() can + * establish a type before that acknowledgement; this then reports that stored type, which + * sendPackage() still checks against the robot. An untouched field has no type yet. * * \param name The string identifier for the data field as used in the documentation. * diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 6adc25f7c..2524c475f 100644 --- a/include/ur_client_library/rtde/rtde_client.h +++ b/include/ur_client_library/rtde/rtde_client.h @@ -29,6 +29,7 @@ #ifndef UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED #define UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED +#include #include #include "ur_client_library/comm/producer.h" @@ -304,7 +305,7 @@ class RTDEClient ClientState getClientState() const { - return client_state_; + return client_state_.load(); } /*! \brief Starts a background thread to read data packages from the robot. @@ -354,7 +355,8 @@ class RTDEClient DataPackage preallocated_data_pkg_; - ClientState client_state_; + // Written by reconnect() on its own thread and read by getClientState() / start / pause. + std::atomic client_state_; uint16_t protocol_version_; diff --git a/tests/test_rtde_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp index e598a0929..8f034ea1b 100644 --- a/tests/test_rtde_client_reconnect.cpp +++ b/tests/test_rtde_client_reconnect.cpp @@ -316,8 +316,12 @@ TEST_F(RTDEClientReconnectTest, reconnect_gives_up_when_the_handshake_keeps_fail data_consumer.join(); EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + // During a retry the client is UNINITIALIZED between attempts, so a later state check alone + // cannot prove it stopped. Another protocol-version request would mean it is still trying. + const auto requests_after_give_up = server_->requestedProtocolVersions().size(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); - EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED) << "the client kept retrying after " - "exhausting its initialization " - "attempts"; + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_EQ(server_->requestedProtocolVersions().size(), requests_after_give_up) << "the client kept retrying after " + "exhausting its initialization " + "attempts"; } From f1505c354881768702cbe46a7ac244bfa0164430 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rune=20S=C3=B8e-Knudsen?= <41109954+urrsk@users.noreply.github.com> Date: Fri, 4 Sep 2026 23:17:45 +0200 Subject: [PATCH 18/18] Apply batched suggestions from code review Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- src/rtde/data_package.cpp | 4 ++++ tests/test_rtde_client_reconnect.cpp | 2 +- 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index 333b2a715..6463a4291 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -369,6 +369,10 @@ bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) // Same field names and the same type on every field, so the whole value array can go across at // once. This is the path a real-time loop takes. + if (this == &other) + { + return true; + } if (layout_hash_ == other.layout_hash_ && values_.size() == other.values_.size()) { copyValues(values_, other.values_); diff --git a/tests/test_rtde_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp index 8f034ea1b..4552cbd49 100644 --- a/tests/test_rtde_client_reconnect.cpp +++ b/tests/test_rtde_client_reconnect.cpp @@ -263,7 +263,7 @@ TEST_F(RTDEClientReconnectTest, destructor_not_blocked_by_stuck_reconnect_thread // completes in well under 2 s. Without the fix this would block for >= large_reconnect_timeout // (5 s), or forever with unlimited attempts. Run it on a worker with a watchdog so a regression // fails fast with a clear message instead of hanging the test binary. - std::packaged_task teardown([this]() { client_.reset(); }); + std::packaged_task teardown([client = std::move(client_)]() mutable { client.reset(); }); auto teardown_future = teardown.get_future(); std::thread teardown_thread(std::move(teardown));