diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index b45823974..09605aa0a 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 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 guide `_ @@ -69,6 +79,56 @@ 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 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 +~~~~~~~~~~~~~~~~ + +``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`` 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 field has no type 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 +165,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 +185,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, 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.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 5339e7198..9f4978f18 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 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 ``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/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 bb271b5f0..36d129378 100644 --- a/doc/migration_notes.rst +++ b/doc/migration_notes.rst @@ -5,6 +5,41 @@ 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. + +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``. ``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 + 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 + 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 +``getDataType()`` reports that the field has no type yet. + Migrating from 1.x.x to 2.x.x ----------------------------- 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_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/examples/rtde_writer.cpp b/examples/rtde_writer.cpp new file mode 100644 index 000000000..92e91e5af --- /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 many 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 3a5d69b1b..f0a0ca326 100644 --- a/include/ur_client_library/rtde/data_package.h +++ b/include/ur_client_library/rtde/data_package.h @@ -29,11 +29,18 @@ #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 +#include "ur_client_library/log.h" #include "ur_client_library/types.h" #include "ur_client_library/rtde/rtde_package.h" @@ -54,64 +61,176 @@ 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. + * + * 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. + */ + using _rtde_type_variant = std::variant; - DataPackage() = delete; + // 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(const DataPackage& other) : DataPackage(other.recipe_) - { - this->data_ = other.data_; - this->protocol_version_ = other.protocol_version_; - } + DataPackage() = delete; - DataPackage& operator=(DataPackage& other) + DataPackage(const DataPackage& other) + : RTDEPackage(PackageType::RTDE_DATA_PACKAGE) + , recipe_id_(other.recipe_id_) + , 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_) { - this->data_ = other.data_; - this->recipe_ = other.recipe_; - this->protocol_version_ = other.protocol_version_; - return *this; + rebuildFieldIndex(); } - 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_; + // 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; } /*! - * \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; + virtual ~DataPackage(); /*! - * \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 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 a field currently holds. + * + * 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. + * + * \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. * + * 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 @@ -127,6 +246,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 @@ -141,23 +263,24 @@ 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()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { - val = std::get(it->second); + return false; } - else + const T* value = std::get_if(&values_[*index]); + if (value == nullptr) { + reportReadFailure(recipe_[*index], values_[*index]); return false; } + val = *value; return true; } @@ -169,25 +292,19 @@ 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"); - - const auto it = - std::find_if(data_.begin(), data_.end(), [&name](const std::pair& element) { - return element.first == name; - }); - if (it != data_.end()) - { - val = std::bitset(std::get(it->second)); - } - else + T recipe_type; + if (!getData(name, recipe_type)) { return false; } + val = std::bitset(recipe_type); return true; } @@ -196,33 +313,40 @@ 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()) + const std::optional index = fieldIndex(name); + if (!index.has_value()) { - 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 + _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. + 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; } + const bool type_changed = std::holds_alternative(field); + field = val; + if (type_changed) + { + zeros_[*index] = T(); + updateLayoutHash(); + } return true; } @@ -236,13 +360,137 @@ class DataPackage : public RTDEPackage recipe_id_ = recipe_id; } + /*! + * \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. + * + * 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. + * Every name is checked before any field is written, so a failure leaves the package unchanged. + */ + void setTypes(const std::vector& types); + + /*! + * \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 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. + * + * 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. 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 + * + * \returns True on success, false if this package is untyped, if \p other was built from a + * different recipe, or if a field \p other has written has a different type + */ + 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); + + /*! + * \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. + * + * \returns True if the package carries type information for all of its fields + */ + bool isTyped() const + { + return fully_typed_; + } + + /*! + * \brief FNV-1a identity of this package's field names, in order. + * + * Not sent on the wire. Used together with layoutHash() to identify a recipe without comparing + * field-name strings. + */ + uint64_t recipeHash() const + { + return recipe_hash_; + } + + /*! + * \brief FNV-1a identity of this package's field names and each field's current variant index. + * + * 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. + */ + uint64_t layoutHash() const + { + return layout_hash_; + } + private: - // Const would be better here - static std::unordered_map g_type_list; - uint8_t recipe_id_; - std::vector> data_; + /*! + * \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_. + * + * The keys are string_views into recipe_, so this must run after recipe_ is in its final place. + */ + void rebuildFieldIndex(); + + /*! + * \brief The recipe index of \p name, or empty if the name is not in this package. + */ + std::optional fieldIndex(const std::string_view name) const; + + /*! + * \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 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; + bool used_slow_copy_ = false; }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 92d3693a2..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" @@ -209,9 +210,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,13 +285,27 @@ 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); ClientState getClientState() const { - return client_state_; + return client_state_.load(); } /*! \brief Starts a background thread to read data packages from the robot. @@ -338,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/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 9c97827b0..a15ad8bc4 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" @@ -49,6 +50,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) @@ -65,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 @@ -87,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 @@ -94,8 +104,33 @@ class RTDEParser : public comm::Parser return protocol_version_; } + /*! + * \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; + // 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_); + typed_template_->setProtocolVersion(protocol_version_); + } + private: + std::unique_ptr makeTypedDataPackage() const; + bool parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const; + std::vector recipe_; + std::vector recipe_types_; + 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 8b482454f..52af8d8ee 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -95,12 +95,31 @@ 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 + * 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 * * \returns Success of the package creation */ 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. * @@ -192,6 +211,26 @@ class RTDEWriter */ bool sendExternalForceTorque(const vector6d_t& external_force_torque); + /*! + * \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); + + /*! + * \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); void markStorageToBeSent(); @@ -200,6 +239,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/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..6463a4291 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -28,464 +28,423 @@ #include "ur_client_library/rtde/data_package.h" -#include +#include +#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() }, - - // 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() }, - - // 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() }, - - // NOT IN OFFICIAL DOCS - { "tool_digital_output_mask", uint8_t() }, - { "tool_digital_output", uint8_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" }, }; +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. + */ +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"; +} + +/*! + * \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."); +} + +/*! + * \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 + */ +DataType typeFromName(const std::string_view type_name) +{ + for (const auto& entry : g_type_names) + { + if (entry.name == type_name) + { + return entry.type; + } + } + + 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()); +} + +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) +{ + for (const auto& entry : g_type_names) + { + if (entry.type == type) + { + return std::string(entry.name); + } + } + 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 std::optional index = fieldIndex(name); + if (!index.has_value()) + { + return std::nullopt; + } + return typeOf(values_[*index]); +} + +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() +{ + 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); + }); +} + +void rtde_interface::DataPackage::setTypes(const std::vector& types) +{ + if (types.size() != recipe_.size()) + { + std::stringstream ss; + 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()); + } + + // 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] = variantFor(typeFromName(types[i])); + zeros_[i] = values_[i]; + } + updateLayoutHash(); +} + void rtde_interface::DataPackage::initEmpty() { - data_.clear(); - data_.reserve(recipe_.size()); - for (auto& item : recipe_) + 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; +} + +bool rtde_interface::DataPackage::copyFrom(const DataPackage& other) +{ + if (!isTyped()) + { + 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; + } + + // 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_); + return true; + } + + if (recipe_hash_ != other.recipe_hash_ || values_.size() != other.values_.size()) + { + URCL_LOG_ERROR("Cannot copy from an RTDE data package built from a different recipe."); + return false; + } + + // 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 (g_type_list.find(item) == g_type_list.end()) + if (!std::holds_alternative(other.values_[i]) && other.values_[i].index() != values_[i].index()) { - throw RTDEInvalidKeyException("Unknown item in recipe: " + item); + 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; } - _rtde_type_variant entry = g_type_list[item]; - data_.push_back({ item, entry }); + } + + for (size_t i = 0; i < values_.size(); ++i) + { + values_[i] = std::holds_alternative(other.values_[i]) ? zeros_[i] : other.values_[i]; + } + + 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()) + { + 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; + } + + // 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([&bp](auto&& arg) { bp.parse(arg); }, data_[i].second); + std::visit( + [&bp](auto&& arg) { + if constexpr (!is_untyped_v) + { + bp.parse(arg); + } + }, + values_[i]); } return true; } @@ -493,16 +452,27 @@ 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 { - std::visit([&ss](auto&& arg) { ss << arg; }, item.second); + std::visit( + [&ss](auto&& arg) { + if constexpr (is_untyped_v) + { + ss << ""; + } + else + { + ss << arg; + } + }, + values_[i]); } ss << std::endl; } @@ -511,23 +481,69 @@ std::string rtde_interface::DataPackage::toString() const size_t rtde_interface::DataPackage::serializePackage(uint8_t* buffer) { - uint16_t payload_size = sizeof(recipe_id_); + 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 = 0; + if (protocol_version_ == 2) + { + payload_size += sizeof(recipe_id_); + } - for (auto& item : data_) + for (const auto& value : values_) { - 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); + } + }, + value); } size_t size = 0; size += PackageHeader::serializeHeader(buffer, PackageType::RTDE_DATA_PACKAGE, payload_size); - size += comm::PackageSerializer::serialize(buffer + size, recipe_id_); - for (size_t i = 0; i < data_.size(); ++i) + if (protocol_version_ == 2) + { + size += comm::PackageSerializer::serialize(buffer + size, recipe_id_); + } + for (size_t i = 0; i < values_.size(); ++i) { size += std::visit( - [&buffer, &size](auto&& arg) -> size_t { return comm::PackageSerializer::serialize(buffer + size, arg); }, - data_[i].second); + [&buffer, &size](auto&& arg) -> size_t { + if constexpr (is_untyped_v) + { + return 0; + } + else + { + return comm::PackageSerializer::serialize(buffer + size, arg); + } + }, + values_[i]); } return size; } + +bool rtde_interface::DataPackage::resetData(const std::string_view name) +{ + const std::optional index = fieldIndex(name); + if (!index.has_value()) + { + return false; + } + values_[*index] = zeros_[*index]; + return true; +} + } // namespace rtde_interface } // namespace urcl diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index 3000db6ef..5d6f3dfc4 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) @@ -118,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 @@ -220,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; @@ -296,10 +310,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; } @@ -328,9 +341,12 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); - preallocated_data_pkg_ = DataPackage(output_recipe_, protocol_version_); + // 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_); parser_ = RTDEParser(output_recipe_); + parser_.setProtocolVersion(protocol_version_); prod_ = std::make_unique>(stream_, parser_); } @@ -380,7 +396,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 +446,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_.setTypes(variable_types); return true; } } @@ -471,7 +498,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 +519,7 @@ bool RTDEClient::setupInputs() throw RTDEInputConflictException(input_recipe_[i]); } } + writer_.setRecipeTypes(variable_types); writer_.init(tmp_input->input_recipe_id_); return true; @@ -524,7 +558,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 +654,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 +704,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 +978,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..23c01e753 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -27,6 +27,33 @@ namespace urcl { namespace rtde_interface { +// 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 +{ + return std::make_unique(*typed_template_); +} + +bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const +{ + // 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); +} + +bool RTDEParser::recipeTypesKnown() const +{ + if (typed_template_.has_value()) + { + 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,9 +81,13 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package(new DataPackage(recipe_, protocol_version_)); + if (!recipeTypesKnown()) + { + return false; + } + std::unique_ptr package = makeTypedDataPackage(); - if (!package->parseWith(bp)) + if (!parseDataPackagePayload(bp, *package)) { URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; @@ -104,6 +135,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 +154,29 @@ 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(); + } + + DataPackage* data_package = dynamic_cast(result.get()); + if (data_package->layoutHash() != typed_template_->layoutHash()) + { + 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 (!dynamic_cast(result.get())->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 c0c48ccdb..09b3ebf72 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 @@ -69,18 +70,44 @@ 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); } } + // 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_); + data_buffer0_->setProtocolVersion(protocol_version_); + data_buffer1_->setProtocolVersion(protocol_version_); current_store_buffer_ = data_buffer0_; current_send_buffer_ = data_buffer1_; } +void RTDEWriter::setProtocolVersion(uint16_t protocol_version) +{ + std::lock_guard lock_guard(store_mutex_); + 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) { if (running_) @@ -92,6 +119,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,11 +172,25 @@ void RTDEWriter::stop() bool RTDEWriter::sendPackage(const DataPackage& package) { std::lock_guard guard(store_mutex_); - *current_store_buffer_ = package; + if (!current_store_buffer_->copyFrom(package)) + { + return false; + } markStorageToBeSent(); return true; } +DataPackage RTDEWriter::createDataPackage() +{ + std::lock_guard guard(store_mutex_); + 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()."); + } + return current_store_buffer_->emptyCopy(); +} + bool RTDEWriter::sendSpeedSlider(double speed_slider_fraction) { if (speed_slider_fraction > 1.0 || speed_slider_fraction < 0.0) @@ -404,19 +447,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..e84068399 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) @@ -212,6 +209,37 @@ 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} +) + +# 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 + RTDEClientReconnectTest.reconnect_gives_up_when_the_handshake_keeps_failing + 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 c0219029c..3f17f6e25 100644 --- a/tests/fake_rtde_server.cpp +++ b/tests/fake_rtde_server.cpp @@ -1,10 +1,496 @@ #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, + const uint16_t protocol_version = 2) +{ + auto package = std::make_unique(recipe); + package->setTypes(types); + package->setProtocolVersion(protocol_version); + return package; +} +} // namespace RTDEServer::RTDEServer(const int port) : server_(port) { @@ -19,32 +505,142 @@ 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) +{ + 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::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"; + 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; + 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: { - 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_; + if (accepted) + { + negotiated_protocol_version_ = requested_version; + } + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; @@ -52,12 +648,23 @@ 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; } 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; @@ -69,61 +676,99 @@ 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; } 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; 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, negotiated_protocol_version_); + } + } 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; } 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); + 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()); @@ -131,32 +776,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; + 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; + size_t written = 0; server_.writeUnchecked(filedescriptor, send_buffer, send_size, written); - stopSendingDataPackages(); + if (accepted) + { + stopSendingDataPackages(); + } break; } case rtde_interface::PackageType::RTDE_DATA_PACKAGE: @@ -212,7 +871,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 +886,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/fake_rtde_server.h b/tests/fake_rtde_server.h index b34eec510..d18b7bfd3 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -1,8 +1,10 @@ #pragma once +#include #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" @@ -16,13 +18,59 @@ class RTDEServer RTDEServer() = delete; explicit RTDEServer(const int port); - ~RTDEServer(); + virtual ~RTDEServer(); void startSendingDataPackages(); void stopSendingDataPackages(); 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(); + + /*! + * \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_; @@ -39,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_; @@ -47,11 +97,24 @@ class RTDEServer std::chrono::steady_clock::time_point start_time_; socket_t client_socket_; + std::vector receive_buffer_; 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_; + 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; }; } // namespace urcl diff --git a/tests/resources/generate_rtde_outputs.py b/tests/resources/generate_rtde_outputs.py index cf4f527b9..15e520590 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 @@ -60,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"] @@ -75,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/rtde_test_helpers.h b/tests/rtde_test_helpers.h new file mode 100644 index 000000000..5794f68b5 --- /dev/null +++ b/tests/rtde_test_helpers.h @@ -0,0 +1,54 @@ +// -- 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 + +#include +#include + +#include + +namespace urcl +{ +namespace test +{ +/*! + * \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 rtde_interface::DataPackage typedPackage(const std::vector& recipe, + const std::vector& types) +{ + rtde_interface::DataPackage package(recipe); + package.setTypes(types); + return package; +} +} // namespace test +} // namespace urcl diff --git a/tests/test_pipeline.cpp b/tests/test_pipeline.cpp index c7f692234..ede09f0b5 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 @@ -54,6 +56,7 @@ class PipelineTest : public ::testing::Test stream_.reset(new comm::URStream("127.0.0.1", 60002)); std::vector recipe = { "timestamp" }; parser_.reset(new rtde_interface::RTDEParser(recipe)); + parser_->setRecipeTypes({ "DOUBLE" }); parser_->setProtocolVersion(2); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); 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_producer.cpp b/tests/test_producer.cpp index 32069be32..a256a388d 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 @@ -63,6 +65,7 @@ TEST_F(ProducerTest, get_data_package) comm::URStream stream("127.0.0.1", 60002); std::vector recipe = { "timestamp" }; rtde_interface::RTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE" }); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); @@ -98,6 +101,7 @@ TEST_F(ProducerTest, connect_non_connected_robot) comm::URStream stream("127.0.0.1", 12321); std::vector recipe = { "timestamp" }; 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 new file mode 100644 index 000000000..d00c50526 --- /dev/null +++ b/tests/test_rtde_allocations.cpp @@ -0,0 +1,469 @@ +// -- 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 + +#include "fake_rtde_server.h" +#include "rtde_test_helpers.h" + +using namespace urcl; + +namespace +{ +// 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; +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); +} + +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. +// 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; + g_allocation_sink = ::operator new(sizeof(int)); + allocations = counter.count(); + ::operator delete(g_allocation_sink); + g_allocation_sink = nullptr; + } + EXPECT_GT(allocations, 0); +} + +TEST(DataPackageAllocationTest, applying_types_does_not_allocate) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + const std::vector types{ "DOUBLE", "VECTOR6D" }; + + std::size_t allocations = 0; + { + AllocationCounter counter; + package.setTypes(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" }; + const std::vector types = { "DOUBLE", "DOUBLE" }; + 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 + // allocate a replacement. + std::unique_ptr product = + std::make_unique(test::typedPackage(recipe, types)); + + 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); +} + +// 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); +} + +// 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" }); + 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: + 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)); + } + + // 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; + 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_->createInputDataPackage(); + 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)); + } + + // 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; + { + 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..eb36e57d9 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include #include #include #include "ur_client_library/comm/tcp_server.h" @@ -290,17 +290,20 @@ 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"))); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_THAT([&]() { client_->init(); }, testing::ThrowsMessage(testing::HasSubstr("i_do_not_" + "exist"))); } TEST_F(RTDEClientTest, output_recipe_with_invalid_key) @@ -308,12 +311,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); @@ -364,10 +365,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)); @@ -445,7 +446,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; @@ -459,158 +460,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 }; - auto data_pkg = rtde_interface::DataPackage(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(); @@ -721,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) - { - diff[name] -= 1; - } - for (auto elem : diff) + // 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) { - 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 @@ -771,30 +622,37 @@ 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); + // 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_TRUE(client->init()); + EXPECT_THAT(client->getOutputRecipe(), testing::Not(testing::Contains("unknown_rtde_variable"))); } TEST_F(RTDEClientTest, empty_input_recipe) @@ -852,91 +710,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 new file mode 100644 index 000000000..97f21c24e --- /dev/null +++ b/tests/test_rtde_client_fake_server.cpp @@ -0,0 +1,565 @@ +// -- 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 +#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); +} + +// 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"; +} + +// 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) +{ + 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"; +} + +// 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); + 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); + 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) +{ + 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); + EXPECT_EQ(client->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + 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); + 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 +// 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_client_reconnect.cpp b/tests/test_rtde_client_reconnect.cpp new file mode 100644 index 000000000..4552cbd49 --- /dev/null +++ b/tests/test_rtde_client_reconnect.cpp @@ -0,0 +1,327 @@ +// -- 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([client = std::move(client_)]() mutable { 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()"; +} + +// 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); + // 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); + EXPECT_EQ(server_->requestedProtocolVersions().size(), requests_after_give_up) << "the client kept retrying after " + "exhausting its initialization " + "attempts"; +} diff --git a/tests/test_rtde_data_package.cpp b/tests/test_rtde_data_package.cpp index f641fcf21..ca4af39ec 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -30,12 +30,16 @@ #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,8 +61,10 @@ 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); + // 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, @@ -90,8 +96,11 @@ 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); + package.setProtocolVersion(1); + // 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, @@ -119,10 +128,34 @@ TEST(rtde_data_package, parse_pkg_protocolv1) EXPECT_NEAR(expected_timestamp, actual_timestamp, abs); } +TEST(rtde_data_package, serialize_pkg_protocolv1) +{ + std::vector recipe{ "speed_slider_mask" }; + std::vector types{ "UINT32" }; + 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); + + 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" }; - 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 +164,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 +174,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 +192,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 +207,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 +226,570 @@ 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)); +} + +// 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; + } + + // 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"; + + 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" }; + 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. + EXPECT_THROW(package.setTypes({ "NOT_FOUND" }), UrException); + EXPECT_THROW(package.setTypes({ "IN_USE" }), UrException); + 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" }; + rtde_interface::DataPackage package(recipe); + 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) +{ + 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_makes_the_package_usable) +{ + std::vector recipe{ "timestamp", "actual_q" }; + rtde_interface::DataPackage package(recipe); + + double timestamp = 0.0; + ASSERT_FALSE(package.getData("timestamp", timestamp)); + + package.setTypes({ "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)); +} + +// 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 +// 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); +} + +// 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" }); + 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)); +} + +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); +} + +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[] = { 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); +} + +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); +} + +// 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)); + + 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) +{ + 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 20a2a0eff..4a357d710 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -31,6 +31,8 @@ #include #include +#include "rtde_test_helpers.h" + using namespace urcl; TEST(rtde_parser, request_protocol_version) @@ -201,6 +203,7 @@ TEST(rtde_parser, data_package) std::unique_ptr product; std::vector recipe = { "timestamp", "target_speed_fraction" }; rtde_interface::RTDEParser parser(recipe); + parser.setRecipeTypes({ "DOUBLE", "DOUBLE" }); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -220,6 +223,126 @@ 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; + rtde_interface::RTDEParser parser({ "timestamp", "target_speed_fraction" }); + parser.setProtocolVersion(2); + + EXPECT_FALSE(parser.parse(bp, product)); +} + +// 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 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + 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(); + + 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. 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 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser 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); +} + +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" }; + rtde_interface::RTDEParser 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. + 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" }; + rtde_interface::RTDEParser 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 @@ -266,6 +389,7 @@ TEST(rtde_parser, test_deprecated_parse_method) 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::vector> products; @@ -294,6 +418,82 @@ 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)); + + rtde_interface::RTDEParser 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"); +} + +// 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" }; + rtde_interface::RTDEParser 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) +{ + // 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)); + + rtde_interface::RTDEParser 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 b95e9f088..c3e8b169e 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; @@ -54,6 +57,7 @@ class RTDEWriterTest : public ::testing::Test stream_->connect(); writer_.reset(new rtde_interface::RTDEWriter(stream_.get(), input_recipe_)); + writer_->setRecipeTypes(input_recipe_types_); writer_->init(1); } @@ -124,6 +128,10 @@ class RTDEWriterTest : public ::testing::Test "input_int_register_25", "input_double_register_25", "external_force_torque" }; + // 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_; @@ -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,141 @@ 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); +} + +// 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_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); +} + +// 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) +{ + 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) +{ + 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)); +} + +// 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" }); + 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);