diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8051c4b8d..52a344bf8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -46,6 +46,40 @@ jobs: retention-days: 5 archive: false + unit_coverage: + name: unit_coverage + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v7 + - name: Install build-tools + run: sudo apt-get update && sudo apt-get install -y build-essential cmake gcovr + - name: configure + run: > + mkdir build && + cd build && + cmake .. + -DBUILDING_TESTS=1 + -DINTEGRATION_TESTS=0 + -DCMAKE_COMPILE_WARNING_AS_ERROR=ON + env: + CXXFLAGS: -g -O2 -fprofile-arcs -ftest-coverage + CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage + LDFLAGS: -fprofile-arcs -ftest-coverage + - name: build + run: cmake --build build --config Debug + - name: test + run: cd build && ctest --output-on-failure --output-junit junit.xml + - name: gcovr + run: cd build && gcovr -r .. --xml coverage.xml --gcov-ignore-parse-errors negative_hits.warn_once_per_file --exclude "../3rdparty" + - name: Upload coverage reports to Codecov with GitHub Action + uses: codecov/codecov-action@v7 + with: + fail_ci_if_error: true + files: build/coverage.xml + flags: unit + env: + CODECOV_TOKEN: ${{ secrets.CODECOV_TOKEN }} + run_tests: timeout-minutes: 60 runs-on: ubuntu-latest diff --git a/doc/architecture/rtde_client.rst b/doc/architecture/rtde_client.rst index b45823974..a67a9ace3 100644 --- a/doc/architecture/rtde_client.rst +++ b/doc/architecture/rtde_client.rst @@ -36,10 +36,32 @@ 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:: + + **Recommended:** Construct a ``DataPackage`` from ``getOutputRecipe()`` after ``init()`` and + reuse it in your control loop. With a matching recipe, the normal data receive path of + ``getDataPackage()`` and ``getDataPackageBlocking()`` does not allocate. + + **Still supported, but not recommended:** The older flow that lets the client allocate a + package remains available for compatibility. The deprecated ``getDataPackage(timeout)`` + overload allocates a new package on each call, and passing a null unique pointer to either + read method also allocates a package. Passing a package with a foreign recipe is supported + through automatic repair, which may allocate. The null-pointer and foreign-recipe paths log + warnings; these warn about allocation, not unsupported usage. Prefer a reusable, matching-recipe + package for new code, especially in real-time loops. + + The allocation-free guarantee applies only to the normal data receive path with that reused + package. It does not cover error handling, non-data messages or reconnection. + + 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`` without + allocation. Until that has happened ``getData()`` throws ``std::bad_variant_access``. 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 +91,94 @@ 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: +``getDataPackage()`` copies the background reader's latest package into it, while +``getDataPackageBlocking()`` parses the next package straight into it. Reusing a package with the +negotiated recipe keeps the normal data receive path free of memory allocations. The +older ``getDataPackage(timeout)`` overload, which returns a new package instead, is deprecated but +still supported. It allocates on every call by design and is not recommended for new code or +real-time use; prefer an overload that fills an existing, reusable package. + +Always check the return value before using received data. A background read returns ``false`` on +timeout or when stopping or reconnecting cancels the pending read; restarting the reader does not +make a cancelled read succeed with stale data. Both unique-pointer overloads retain caller +ownership on failure and assign a previously null pointer only on success. A failed blocking read +can still partially update an existing package's values if the incoming data is malformed. + +Pacing a loop with the robot +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Both read modes can pace an application loop at the robot's negotiated RTDE output frequency, +without a separate fixed-period sleep. Choose the mode according to how directly the loop should +follow incoming packages: + +- With ``start(false)``, ``getDataPackageBlocking()`` waits for the next package when no data is + already buffered. Calling it at the start of each iteration lets packet arrival pace the loop, + coupling it directly to the RTDE stream without a background-reader handoff. This is useful + when each iteration should read data and then compute and submit a response. See the + :ref:`rtde_roundtrip_example`. +- With ``start(true)``, ``getDataPackage(package, timeout)`` can also pace the loop: after the + latest sample has been consumed, it waits for the background reader to publish another one, + up to the timeout. If a newer sample is already available, it returns immediately. This mode + decouples socket reading from application work and favors the latest sample; intermediate + samples can be overwritten when the application is slower than the stream. It suits loops + that need less direct synchronization and do not need to process every received sample. + +For background reads used as a loop clock, allow enough timeout for the expected RTDE period +and scheduling jitter, and handle a ``false`` return instead of processing old data. In either +mode, the loop must keep up with the negotiated frequency for steady pacing. Buffered data can +make synchronous reads return immediately, and network or scheduling delays can make arrivals +irregular. Neither mode guarantees phase synchronization with the robot's internal control cycle +or receipt of a command in the next cycle. Use the output ``timestamp`` to track robot time and +detect gaps between samples. + +Field data types +~~~~~~~~~~~~~~~~ + +``getData()`` has to be given a variable of the field's own type. A missing name returns +``false``; a type mismatch throws ``std::bad_variant_access``. +Rather than hardcoding which type a field has, ask the package: ``getDataType()`` reports the +``DataType`` a field currently holds. A successful client read applies the robot's negotiated +types to an output package; ``init()`` alone does not type application-owned packages. +``createInputDataPackage()`` returns an input package with the negotiated types already applied. +On a recipe-only package, ``setData()`` establishes an untyped field's type from the value written; +subsequent writes must match that type. 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 +215,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; } @@ -121,12 +231,58 @@ RTDEWriter The ``RTDEWriter`` class provides an interface to write data to the RTDE interface. Data fields that should be written have to be defined inside the ``INPUT_RECIPE`` as noted above. -The class offers specific methods for every RTDE input possible to write. +.. important:: + + **Use RTDEClient to initialize and access RTDEWriter (recommended).** Create an ``RTDEClient`` + with a non-empty input recipe, call ``init()``, then use its writer through ``getWriter()``. + The client handles the connection, protocol negotiation, input recipe setup and writer + initialization, including the field types and recipe ID reported by the robot. + + Constructing and using ``RTDEWriter`` directly is still supported, but is a lower-level option + for applications that manage the RTDE connection and handshake themselves. It is not the + recommended approach for normal application code. + +The class offers convenience methods for common inputs and ``sendPackage()`` for a complete input +recipe. + +Data is sent asynchronously to the RTDE interface. A successful ``sendPackage()`` or ``send...()`` +call updates the pending send buffer and notifies the writer thread; it does not confirm delivery +or processing by the robot. This is not a FIFO queue of calls: multiple updates before the writer +consumes the pending buffer can be coalesced, and a later ``sendPackage()`` can replace an earlier +pending package. Separate helper calls may be transmitted separately or coalesced, depending on +when the writer runs. Use ``sendPackage()`` to submit related fields together in one buffer update, +not to guarantee a distinct transmission for every call. + +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. Call ``createInputDataPackage()`` after a successful +``init()`` with a non-empty input recipe, fill the fields you care about and pass it to +``sendPackage()``. The new package starts with zero values; when reusing it, fields retain their +previous values unless explicitly changed or reset. 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 submitted to ``sendPackage()``. The field names +and order must match the negotiated input recipe. Fields that remain untyped are sent as typed +zeros, while incompatible types cause ``sendPackage()`` to return ``false``. See the +:ref:`rtde_roundtrip_example` for a complete example. -Data is sent asynchronously to the RTDE interface. +If direct ``RTDEWriter`` use is required instead of the recommended ``RTDEClient`` flow, perform the RTDE handshake +and configure the stopped writer with ``setProtocolVersion(negotiated_version)`` and +``setRecipeTypes(acknowledged_types)`` before calling ``init(recipe_id)`` with the acknowledged +input recipe ID. Constructing the writer or calling ``init(recipe_id)`` alone does not establish +the field types. ``RTDEClient::init()`` handles these steps automatically. .. note:: The ``RTDEWriter`` will return ``false`` on any writing attempts for fields that have not been setup in the ``INPUT_RECIPE``. When no input recipe was provided, all write operations will - return ``false``. + return ``false``. No writer thread is started in that case, and ``createInputDataPackage()`` + throws ``UrException`` even after successful client initialization. The factory also throws + before input recipe negotiation or while the writer is stopped. diff --git a/doc/examples.rst b/doc/examples.rst index fae5a85e9..c401fbc4d 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_roundtrip 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..dd1586974 100644 --- a/doc/examples/rtde_client.rst +++ b/doc/examples/rtde_client.rst @@ -56,15 +56,20 @@ 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 +The loop reuses a package built from the negotiated output recipe, keeping the normal data receive +path allocation-free. Null pointers allocate a package, and foreign-recipe repair may allocate; +error handling, non-data messages and reconnection are outside this guarantee. The recipe only +names the fields, so the first read applies their negotiated types in place without allocation. + 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. If the type of the passed + variable doesn't match the type the robot reported for that field, it throws + ``std::bad_variant_access``. Writing Data to the RTDE client ------------------------------- @@ -91,7 +96,9 @@ initialize the RTDE client has to contain the keys necessary to send that specif `_ for more information. -.. note:: Every ``send...`` call to the RTDEWriter triggers a package sent to the robot. If you - want to modify more than one input at a time, it is recommended to use the ``sendPackage()`` - method. That allows setting up the complete data package with its input recipe and sending that - to the robot at once. +.. note:: Every successful ``send...`` call updates the pending buffer and notifies the writer + thread. Calls may be coalesced before transmission; they are not queued as separate packages. + To submit several inputs together, use ``createInputDataPackage()`` after ``init()``, fill the + fields and pass the package to ``sendPackage()``. Separate helper calls can otherwise be + transmitted between updates. Neither API confirms delivery to the robot; see the + :ref:`rtde_roundtrip_example` for verification using robot outputs. diff --git a/doc/examples/rtde_roundtrip.rst b/doc/examples/rtde_roundtrip.rst new file mode 100644 index 000000000..e095a4622 --- /dev/null +++ b/doc/examples/rtde_roundtrip.rst @@ -0,0 +1,193 @@ +:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_roundtrip.rst + +.. _rtde_roundtrip_example: + +RTDE register round-trip 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 ``send...()`` helpers on ``RTDEWriter`` update individual inputs and notify the asynchronous +writer separately. When several general purpose registers have to change together, +``sendPackage()`` submits them together in one pending-buffer update, avoiding a transmission +between separate helper calls. It does not guarantee a separate transmission for every call. + +The example's source code can be found in `rtde_roundtrip.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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.cpp + :linenos: + :lineno-match: + :start-at: // We write the inputs, the robot program below writes the outputs. + :end-at: 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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.cpp + :linenos: + :lineno-match: + :start-at: const std::string MIRROR_PROGRAM + :end-at: end)"; + +.. literalinclude:: ../../examples/rtde_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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 normal RTDE data receive and +submission paths reuse their storage without allocation. This does not extend to logging, error +handling or reconnection. The output package is built from ``getOutputRecipe()`` and is therefore +still untyped; the first read applies the robot's types to it in place, without allocation. + +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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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 +------------------------------------- + +The newly created input package contains zeros. Reusing it preserves previously written values +unless they are changed or reset. ``sendPackage()`` copies all fields into the pending buffer and +notifies the writer thread without waiting for transmission. Multiple calls before the writer +consumes that buffer can be coalesced, with a later package replacing an earlier pending one. +Separate ``send...()`` calls may be transmitted separately or coalesced; they are not an atomic +update of several fields. A successful return means the buffer update was accepted, not that the +robot received or processed it. + +Without a run duration the loop runs until it is interrupted, so the counter wraps at one million +rather than growing past what an integer register can hold. Every answer carries the counter value +it belongs to, so verification is unaffected, and the lag is measured modulo the same period. + +.. literalinclude:: ../../examples/rtde_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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_roundtrip.cpp + :language: c++ + :caption: examples/rtde_roundtrip.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/examples/CMakeLists.txt b/examples/CMakeLists.txt index 10007e75f..fe8bb0b2b 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_roundtrip_example + rtde_roundtrip.cpp) +target_link_libraries(rtde_roundtrip_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_roundtrip.cpp b/examples/rtde_roundtrip.cpp new file mode 100644 index 000000000..d919122d8 --- /dev/null +++ b/examples/rtde_roundtrip.cpp @@ -0,0 +1,247 @@ +// -- 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"; + +// 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"; + +// 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, INPUT_INT_REGISTER, INPUT_DOUBLE_REGISTER }; +const std::vector OUTPUT_RECIPE = { "timestamp", "runtime_state", OUTPUT_BIT_REGISTER, OUTPUT_INT_REGISTER, + OUTPUT_DOUBLE_REGISTER }; + +// 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 + +// Without a run duration the loop below never ends, so the counter wraps instead of growing until +// it overflows. One is the lowest value the "has the robot processed anything yet" check accepts, +// and the robot program adds one to whatever it receives, so the sent value stays in +// [1, COUNTER_WRAP] and the echoed one in [2, COUNTER_WRAP + 1]. +const int32_t COUNTER_WRAP = 1000000; + +// 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; + // The counter wraps, so the frequency report needs its own count of the cycles that ran. + size_t cycles = 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); + // Taken modulo the wrap period, so a lag measured across a wrap is still a small number. + last_lag_cycles = (counter - origin + COUNTER_WRAP) % COUNTER_WRAP; + // 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 + ++cycles; + counter = counter % COUNTER_WRAP + 1; + 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(cycles) / 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(cycles) / elapsed_s : 0.0; + URCL_LOG_INFO("Cycles: %zu, average frequency: %f Hz, verified: %zu, mismatches: %zu, last lag: %d cycles", cycles, + 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/comm/producer.h b/include/ur_client_library/comm/producer.h index 5843f57f7..644050579 100644 --- a/include/ur_client_library/comm/producer.h +++ b/include/ur_client_library/comm/producer.h @@ -47,8 +47,8 @@ class URProducer : public IProducer bool running_; - template - bool tryGetImpl(ProductT& product) + template + bool tryGetImpl(ParseFrame&& parse_frame) { // TODO This function has become really ugly! That should be refactored! @@ -63,11 +63,14 @@ class URProducer : public IProducer // reset sleep amount timeout_ = std::chrono::seconds(1); BinParser bp(buf, read); - return parser_.parse(bp, product); + return parse_frame(bp); } if (!running_) + { + URCL_LOG_DEBUG("Cannot receive a package: producer is stopped."); return false; + } const SocketState state = stream_.getState(); @@ -96,7 +99,10 @@ class URProducer : public IProducer } if (stream_.closed() || stream_.stopRequested()) + { + URCL_LOG_DEBUG("Cannot receive a package: stream is closing or stopped."); return false; + } if (on_reconnect_cb_) { @@ -120,7 +126,10 @@ class URProducer : public IProducer } if (!running_ || stream_.closed() || stream_.stopRequested()) + { + URCL_LOG_DEBUG("Package receive cancelled during reconnect backoff."); return false; + } if (stream_.reconnect()) continue; @@ -129,8 +138,6 @@ class URProducer : public IProducer if (next <= std::chrono::seconds(120)) timeout_ = next; } - - return false; } public: @@ -192,7 +199,7 @@ class URProducer : public IProducer */ bool tryGet(std::vector>& products) override { - return tryGetImpl(products); + return tryGetImpl([this, &products](BinParser& bp) { return parser_.parse(bp, products); }); } /*! @@ -207,7 +214,18 @@ class URProducer : public IProducer */ bool tryGet(std::unique_ptr& product) override { - return tryGetImpl(product); + return tryGetImpl([this, &product](BinParser& bp) { return parser_.parse(bp, product); }); + } + + /*! + * \brief Receives one frame using the existing read/reconnect loop and a custom parser. + * The callable is invoked synchronously and is never stored. It must return whether parsing + * succeeded, and diagnose failures. This allows parsing into borrowed, caller-owned storage. + */ + template + bool tryGetWithParser(ParseFrame&& parse_frame) + { + return tryGetImpl(parse_frame); } /*! diff --git a/include/ur_client_library/primary/package_header.h b/include/ur_client_library/primary/package_header.h index 459d13580..bbca5cc6f 100644 --- a/include/ur_client_library/primary/package_header.h +++ b/include/ur_client_library/primary/package_header.h @@ -32,6 +32,7 @@ #include #include +#include #include #include "ur_client_library/types.h" @@ -76,7 +77,11 @@ class PackageHeader */ static size_t getPackageLength(uint8_t* buf) { - return be32toh(*(reinterpret_cast<_package_size_type*>(buf))); + // Copy the bytes out instead of casting: buf can point anywhere inside a byte stream, and an + // unaligned _package_size_type access is undefined behavior. + _package_size_type package_size; + std::memcpy(&package_size, buf, sizeof(package_size)); + return be32toh(package_size); } }; } // namespace primary_interface diff --git a/include/ur_client_library/primary/primary_client.h b/include/ur_client_library/primary/primary_client.h index 8440a9ff8..4d9d63098 100644 --- a/include/ur_client_library/primary/primary_client.h +++ b/include/ur_client_library/primary/primary_client.h @@ -66,7 +66,14 @@ class PrimaryClient { public: PrimaryClient() = delete; - PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier); + /*! + * \brief Creates a new PrimaryClient object. + * + * \param robot_ip The IP of the robot + * \param notifier The notifier to notify of start and stop events + * \param port Optionally specify a different port + */ + PrimaryClient(const std::string& robot_ip, comm::INotifier& notifier, const int port = UR_PRIMARY_PORT); ~PrimaryClient(); /*! diff --git a/include/ur_client_library/rtde/data_package.h b/include/ur_client_library/rtde/data_package.h index 3a5d69b1b..3046ec091 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,179 @@ 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() throws std::bad_variant_access. */ 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. Reusing it for normal client reads with the same recipe + * needs no further allocation; foreign-recipe assignment or repair may allocate. Construction + * needs no connection, but using getOutputRecipe() after init() accounts for unavailable fields + * removed during negotiation. + * + * \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 a client read or setTypes() applies the negotiated types, this is the type the robot + * reported. createInputDataPackage() also returns a package with those types already applied. + * The handshake alone does not type application-owned packages. On a recipe-only package, + * setData() can establish a field's type; this 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 +249,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 @@ -142,22 +267,19 @@ class DataPackage : public RTDEPackage * \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. + * + * \throws std::bad_variant_access if the field is present but does not hold T, including when + * the field has not been typed yet. */ 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()) - { - val = std::get(it->second); - } - else + const std::optional index = fieldIndex(name); + if (!index.has_value()) { return false; } + val = std::get(values_[*index]); return true; } @@ -170,24 +292,20 @@ class DataPackage : public RTDEPackage * \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. + * + * \throws std::bad_variant_access if the field is present but the underlying type is not T, + * including when the field has not been typed yet. */ 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 +314,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 +361,125 @@ 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. The layout hash is always recomputed, including when the + * package is still untyped or only partially typed. + */ + void setProtocolVersion(const uint16_t protocol_version) + { + protocol_version_ = protocol_version; + updateLayoutHash(); + } + + /*! + * \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 Copies same-recipe values without allocation; unset source fields become typed zeros. + * \param other Source package; its typed fields must match this typed destination. + * \returns False on recipe/type mismatch or an untyped destination, without changing values, recipe id or version. + */ + bool copyFrom(const DataPackage& other); + + /*! + * \brief Resets a data field to a default-constructed value of its own type. + * + * \param name The string identifier for the data field as used in the documentation. + * + * \returns True on success, false if the field cannot be found inside the package. + */ + bool resetData(const std::string_view name); + + /*! + * \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 + * incompletely typed package cannot be parsed into or serialized. getData() throws + * std::bad_variant_access for an untyped field, but typed fields can already be read. + * + * \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_; + } + + /// Exact setup-time comparison of recipe field names and order; does not allocate. + bool hasRecipe(const std::vector& recipe) const + { + return recipe_ == recipe; + } + + /*! + * \brief FNV-1a identity of this package's protocol version, field names and current variant indices. + * + * Not sent on the wire. Combined from the recipe hash, protocol version and type of every field, + * so it changes when the protocol version or a field's type changes, 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; + + 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; }; } // namespace rtde_interface diff --git a/include/ur_client_library/rtde/package_header.h b/include/ur_client_library/rtde/package_header.h index e111eab30..7baed9028 100644 --- a/include/ur_client_library/rtde/package_header.h +++ b/include/ur_client_library/rtde/package_header.h @@ -31,6 +31,7 @@ #define UR_CLIENT_LIBRARY_RTDE__HEADER_H_INCLUDED #include +#include #include #include "ur_client_library/types.h" #include "ur_client_library/comm/package_serializer.h" @@ -73,7 +74,11 @@ class PackageHeader */ static size_t getPackageLength(uint8_t* buf) { - return be16toh(*(reinterpret_cast<_package_size_type*>(buf))); + // Copy the bytes out instead of casting: buf can point anywhere inside a byte stream, and an + // unaligned _package_size_type access is undefined behavior. + _package_size_type package_size; + std::memcpy(&package_size, buf, sizeof(package_size)); + return be16toh(package_size); } /*! diff --git a/include/ur_client_library/rtde/rtde_client.h b/include/ur_client_library/rtde/rtde_client.h index 92d3693a2..7f0bebc21 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" @@ -190,11 +191,20 @@ class RTDEClient * received from the robot can be fetched with this. When no new data has been received since the last call to this * function, it will wait for the time specified in the \p timeout parameter. * + * This wait can pace an application loop at the negotiated RTDE output frequency. If a newer + * sample is already available, the call returns immediately. The background reader keeps only + * the latest sample, so intermediate samples may be skipped when the application is slower. + * Allow a timeout covering the expected period and scheduling jitter, and check the return value. + * For more direct packet-arrival pacing without a background-reader handoff, use start(false) + * and getDataPackageBlocking(). Neither mode guarantees phase synchronization with robot control. + * * When packages are not read from the background thread, this function will return false and * print an error message. * * \param data_package Reference to a DataPackage where the received data package will be stored * if a package was fetched successfully. + * Foreign recipes are repaired with a warning; null pointers allocate a package with a warning. + * A null pointer is assigned only after a successful read. Stop/reconnect cancels pending reads. * \param timeout Time to wait if no data package is currently in the queue * * \returns Whether a data package was received successfully @@ -207,11 +217,22 @@ class RTDEClient * * This function will block until a new data package is received from the robot and return it. * + * With start(false), calling this at the start of each loop iteration can pace application work + * at the negotiated RTDE output frequency: when no data is buffered, packet arrival releases + * the wait. Already buffered packages can return immediately, so the application must keep up + * with the stream to avoid lag. Network and scheduling jitter still apply; this does not guarantee + * phase synchronization with the robot's internal control cycle or next-cycle command delivery. + * getDataPackage() with background reading can also pace a loop, but favors the latest sample + * and decouples socket reading from application work. + * * \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. Use getOutputRecipe() for the recipe; foreign recipes are repaired with a warning and may allocate. + * Null pointers allocate a package with a warning. + * The caller retains ownership on failure; null pointers are assigned only on success. + * Malformed data may partially update an existing package's values before failure. * * \returns Whether a data package was received successfully */ @@ -282,13 +303,29 @@ 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 successful init() with a non-empty input + * recipe. The new package contains zeros; reusing it retains previously written values. + * + * \throws UrException if the writer is stopped or input types have not been negotiated, including + * when the client has an empty input recipe. + */ + 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. @@ -335,10 +372,13 @@ class RTDEClient std::atomic background_read_running_ = false; std::thread background_read_thread_; std::condition_variable background_read_cv_; + // Protected by read_mutex_; prevents a waiter from crossing a stop/restart or reconnect. + uint64_t background_read_session_id_ = 0; 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_; @@ -380,6 +420,15 @@ class RTDEClient bool sendStart(); bool sendPause(); + /*! + * \brief Repairs foreign or untyped packages by assigning the negotiated output template. + */ + void ensureOutputLayout(DataPackage& data_package, const DataPackage& output_template) const; + /*! + * \brief Allocates a package with the negotiated output layout if null, or delegates to reference repair. + */ + void ensureOutputLayout(std::unique_ptr& data_package) const; + /*! * \brief Reconnects to the RTDE interface and set the input and output recipes again. */ diff --git a/include/ur_client_library/rtde/rtde_parser.h b/include/ur_client_library/rtde/rtde_parser.h index 9c97827b0..5ded0b103 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,8 @@ class RTDEParser : public comm::Parser /*! * \brief Creates a new RTDEParser object, registering the used recipe. * + * Register robot-acknowledged types with setExpectedDataPackage() or setExpectedLayoutHash() before parsing data. + * * \param recipe The recipe used in RTDE data communication */ RTDEParser(const std::vector& recipe) : recipe_(recipe), protocol_version_(1) @@ -63,14 +66,22 @@ class RTDEParser : public comm::Parser * \param bp A BinParser holding a serialized RTDE package * \param result A pointer to the created RTDE package object. Ideally, the passed \p result is a pre-allocated * package of the type expected to be read. For example, when RTDE communication has been setup it enters the data - * communication phase, where the expected package is a DataPackage. If the package content inside the \p bp object - * being doesn't match the result package's type or if the \p result is a nullptr, a new package will be allocated. + * communication phase, where the expected package is a DataPackage. A DataPackage passed for RTDE data must have + * the registered layout hash; a mismatch returns false. Null/non-data pointers require setExpectedDataPackage(). * * \returns True, if the byte stream could successfully be parsed as an RTDE package, false * otherwise */ bool parse(comm::BinParser& bp, std::unique_ptr& result) override; + /*! + * \brief Consumes one frame, borrowing the destination without replacing or deleting it. + * Returns false for non-data frames or parse failures. Malformed data may partially update + * field values; ownership is always retained by the caller. The destination must have the + * registered layout. Non-data frames use separate temporary storage. + */ + bool parseDataPackage(comm::BinParser& bp, DataPackage& destination); + /*! * \brief Uses the given BinParser to create package objects from the contained serialization. * @@ -84,9 +95,31 @@ class RTDEParser : public comm::Parser "a pre-allocated package. This function will be removed in May 2027.")]] bool parse(comm::BinParser& bp, std::vector>& results) override; + /*! + * \brief Records the RTDE protocol version used for parsing. + * + * A typed template registered with setExpectedDataPackage() is updated in place. A hash-only + * registration from setExpectedLayoutHash() is bound to the protocol version at registration + * time: changing the version clears it, and a new hash must be registered before data can be + * parsed. Setting the same version is a no-op. + */ void setProtocolVersion(uint16_t protocol_version) { + if (protocol_version_ == protocol_version) + { + return; + } protocol_version_ = protocol_version; + if (expected_data_package_.has_value()) + { + expected_data_package_->setProtocolVersion(protocol_version); + layout_hash_ = expected_data_package_->layoutHash(); + } + else if (expected_layout_known_) + { + expected_layout_known_ = false; + layout_hash_ = 0; + } } uint16_t getProtocolVersion() const @@ -94,8 +127,50 @@ class RTDEParser : public comm::Parser return protocol_version_; } + /*! + * \brief Registers the expected data-package layout 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. The hash is bound to the parser's current protocol + * version; changing the version with setProtocolVersion() clears this registration. + * + * \param layout_hash The layout hash of the acknowledged output recipe + */ + void setExpectedLayoutHash(uint64_t layout_hash) + { + // Clear any previous template when registering only a hash to enforce strict non-allocating mode. + expected_data_package_.reset(); + layout_hash_ = layout_hash; + expected_layout_known_ = true; + } + + /// Registers a typed template, enabling allocation for null/non-data pointers and the deprecated vector overload. + void setExpectedDataPackage(const DataPackage& data_package) + { + if (!data_package.isTyped()) + { + throw UrException("The expected RTDE data package must be typed."); + } + if (!data_package.hasRecipe(recipe_)) + { + throw UrException("The expected RTDE data package must use the parser's recipe."); + } + expected_data_package_.emplace(data_package); + expected_data_package_->setProtocolVersion(protocol_version_); + layout_hash_ = expected_data_package_->layoutHash(); + expected_layout_known_ = true; + } + private: + bool parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const; + std::vector recipe_; + // Optional typed template restoring legacy allocation for null pointers and deprecated vector parse. + std::optional expected_data_package_; + uint64_t layout_hash_ = 0; + bool expected_layout_known_ = false; + 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..9bd0d615f 100644 --- a/include/ur_client_library/rtde/rtde_writer.h +++ b/include/ur_client_library/rtde/rtde_writer.h @@ -69,12 +69,17 @@ class RTDEWriter * needed. * * \param recipe The new recipe to use + * + * \throws UrException if the writer is already running */ void setInputRecipe(const std::vector& recipe); /*! - * \brief Starts the writer thread, which periodically clears the queue to write packages to the - * robot. + * \brief Starts the writer thread, which sends pending buffer updates to the robot. + * + * Apply the negotiated protocol version and input field types with setProtocolVersion() and + * setRecipeTypes() while stopped, before calling this method. This method does not negotiate + * or establish field types. RTDEClient::init() handles that setup for its writer. * * \param recipe_id The recipe id to use, so the robot correctly identifies the used recipe */ @@ -92,15 +97,37 @@ class RTDEWriter /*! * \brief Sends a complete RTDEPackage to the robot. * - * Use this if multiple values need to be sent at once. When using the other provided functions, - * an RTDE data package will be sent each time. + * Use this to submit multiple values together in one pending-buffer update. Separate helper + * calls may be transmitted separately or coalesced, depending on when the writer thread runs. + * Calls are not queued individually: a later update can replace an earlier pending package. * - * \param package The package to send + * Every field of \p package is copied into the pending buffer. Field names and order must match + * the input recipe the robot acknowledged. Typed fields must match the negotiated types; + * fields that are still untyped are copied as typed zeros. A mismatch returns false. * - * \returns Success of the package creation + * \param package The package to send, constructed from the client's input recipe + * + * \returns Whether the pending buffer update was accepted, not confirmation of transmission or + * processing by the robot. */ 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 writer is stopped or its input buffers are not typed. An RTDEClient + * with an empty input recipe does not start its writer, so this also throws for read-only clients. + */ + DataPackage createDataPackage(); + /*! * \brief Creates a package to request setting a new value for the speed slider. * @@ -192,6 +219,29 @@ 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, if a type is unknown, or + * if the writer is already running + */ + 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. + * + * \throws UrException if the writer is already running + */ + void setProtocolVersion(uint16_t protocol_version); + private: void resetMasks(const std::shared_ptr& buffer); void markStorageToBeSent(); @@ -200,6 +250,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/primary/primary_client.cpp b/src/primary/primary_client.cpp index 68959ecd9..d2a163f28 100644 --- a/src/primary/primary_client.cpp +++ b/src/primary/primary_client.cpp @@ -42,8 +42,8 @@ namespace urcl { namespace primary_interface { -PrimaryClient::PrimaryClient(const std::string& robot_ip, [[maybe_unused]] comm::INotifier& notifier) - : stream_(robot_ip, UR_PRIMARY_PORT) +PrimaryClient::PrimaryClient(const std::string& robot_ip, [[maybe_unused]] comm::INotifier& notifier, const int port) + : stream_(robot_ip, port) { parser_.setStrictMode(COMPILE_OPTIONS.PRIMARY_CLIENT_STRICT_PARSING); prod_.reset(new comm::URProducer(stream_, parser_)); diff --git a/src/rtde/data_package.cpp b/src/rtde/data_package.cpp index b2abff24d..ea7025a2f 100644 --- a/src/rtde/data_package.cpp +++ b/src/rtde/data_package.cpp @@ -28,464 +28,387 @@ #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 uint16_t protocol_version, + const std::vector& values) +{ + uint64_t hash = recipe_hash; + hash = fnv1aByte(hash, static_cast(protocol_version)); + hash = fnv1aByte(hash, static_cast(protocol_version >> 8)); + 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 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::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_, protocol_version_, 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; + } + + // copyValues() uses memcpy. Overlapping source and destination are undefined, so a package + // copied onto itself has to return before that path. + if (this == &other) + { + return true; + } + + // Same field names and the same type on every field, so the whole value array can go across at + // once. This is the path a real-time loop takes. + if (layout_hash_ == other.layout_hash_ && values_.size() == other.values_.size()) + { + copyValues(values_, other.values_); + return true; + } + + // Backwards compatibility: accept partial input packages where unset fields default to typed zeros. + if (recipe_hash_ != other.recipe_hash_ || values_.size() != other.values_.size()) { - if (g_type_list.find(item) == g_type_list.end()) + return false; + } + // Validate every set field against destination types before writing to ensure atomic rejection. + for (size_t i = 0; i < values_.size(); ++i) + { + if (!std::holds_alternative(other.values_[i]) && values_[i].index() != other.values_[i].index()) { - throw RTDEInvalidKeyException("Unknown item in recipe: " + item); + 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]; + } + return true; } +rtde_interface::DataPackage::~DataPackage() = default; + 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 +416,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 +445,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; + } - for (auto& item : data_) + uint16_t payload_size = 0; + if (protocol_version_ == 2) { - payload_size += std::visit([](auto&& arg) -> uint16_t { return sizeof(arg); }, item.second); + payload_size += sizeof(recipe_id_); + } + + for (const auto& value : values_) + { + 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..945f08265 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,8 @@ uint16_t RTDEClient::negotiateProtocolVersion() { URCL_LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version); parser_.setProtocolVersion(protocol_version); + preallocated_data_pkg_.setProtocolVersion(protocol_version); + writer_.setProtocolVersion(protocol_version); return protocol_version; } break; @@ -296,10 +311,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 +342,13 @@ void RTDEClient::resetOutputRecipe(const std::vector new_recipe) disconnect(); output_recipe_.assign(new_recipe.begin(), new_recipe.end()); - preallocated_data_pkg_ = DataPackage(output_recipe_, 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_); + preallocated_data_pkg_.setProtocolVersion(protocol_version_); parser_ = RTDEParser(output_recipe_); + parser_.setProtocolVersion(protocol_version_); prod_ = std::make_unique>(stream_, parser_); } @@ -380,7 +398,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 +448,9 @@ bool RTDEClient::setupOutputs() } else { - // All variables are accounted for in the RTDE package + preallocated_data_pkg_.setTypes(variable_types); + // Register typed template so parser can allocate for null pointers or deprecated vector calls. + parser_.setExpectedDataPackage(preallocated_data_pkg_); return true; } } @@ -471,7 +497,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 +518,7 @@ bool RTDEClient::setupInputs() throw RTDEInputConflictException(input_recipe_[i]); } } + writer_.setRecipeTypes(variable_types); writer_.init(tmp_input->input_recipe_id_); return true; @@ -524,7 +557,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 +653,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 +703,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)) @@ -743,14 +777,78 @@ std::unique_ptr RTDEClient::getDataPackage(std::chr return std::unique_ptr(nullptr); } +void RTDEClient::ensureOutputLayout(DataPackage& data_package, const DataPackage& output_template) const +{ + if (data_package.layoutHash() == output_template.layoutHash()) + { + return; + } + // Backwards compatibility: master repaired foreign recipes by assignment; warn because repair allocates. + if (data_package.recipeHash() != output_template.recipeHash()) + { + URCL_LOG_WARN("Replacing a DataPackage with a different output recipe; this may allocate. " + "Construct it from RTDEClient::getOutputRecipe() to avoid this repair."); + } + data_package = output_template; +} + +void RTDEClient::ensureOutputLayout(std::unique_ptr& data_package) const +{ + // Backwards compatibility: allocate a typed package if caller passed null. + if (data_package == nullptr) + { + URCL_LOG_WARN("No DataPackage supplied; allocating one with the negotiated output layout."); + data_package = std::make_unique(preallocated_data_pkg_); + return; + } + ensureOutputLayout(*data_package, preallocated_data_pkg_); +} + bool RTDEClient::getDataPackage(std::unique_ptr& data_package, std::chrono::milliseconds timeout) { - return getDataPackage(*data_package, timeout); + if (data_package) + { + return getDataPackage(*data_package, timeout); + } + std::unique_ptr candidate; + { + // Hold reconnect lock while allocating from preallocated_data_pkg_ to prevent race with reconnect. + std::unique_lock lock(reconnect_mutex_, std::try_to_lock); + if (!lock.owns_lock()) + { + URCL_LOG_DEBUG("Cannot prepare RTDE output: communication setup is locked."); + return false; + } + if (reconnecting_) + { + URCL_LOG_DEBUG("Cannot prepare RTDE output while reconnecting."); + return false; + } + if (!background_read_running_) + { + URCL_LOG_ERROR("Cannot get RTDE output: background reading is not running."); + return false; + } + if (!preallocated_data_pkg_.isTyped()) + { + URCL_LOG_ERROR("Cannot get RTDE output before recipe types are negotiated."); + return false; + } + ensureOutputLayout(candidate); + } + if (!getDataPackage(*candidate, timeout)) + { + URCL_LOG_DEBUG("Failed to get RTDE data package within the specified timeout."); + return false; // The reference overload diagnoses the failure. + } + data_package = std::move(candidate); + return true; } bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::milliseconds timeout) { + std::unique_lock lock(read_mutex_); if (reconnecting_) { URCL_LOG_WARN("Currently reconnecting to the RTDE interface, unable to get data package"); @@ -762,27 +860,39 @@ bool RTDEClient::getDataPackage(DataPackage& data_package, std::chrono::millisec "reading or use getDataPackageBlocking(...)."); return false; } - - if (new_data_.load()) + const auto initial_session_id = background_read_session_id_; + if (!background_read_cv_.wait_for(lock, timeout, [this, initial_session_id] { + return new_data_.load() || !background_read_running_ || reconnecting_ || + background_read_session_id_ != initial_session_id; + })) { - std::lock_guard guard(read_mutex_); - data_package = *dynamic_cast(data_buffer0_.get()); - new_data_.store(false); + URCL_LOG_DEBUG("Timed out waiting for new RTDE data."); + return false; } - else + if (background_read_session_id_ != initial_session_id) { - std::unique_lock lock(read_mutex_); - auto wait_result = background_read_cv_.wait_for(lock, timeout); - if (wait_result == std::cv_status::timeout) - { - return false; - } - if (new_data_.load()) - { - data_package = *dynamic_cast(data_buffer0_.get()); - new_data_.store(false); - } + URCL_LOG_DEBUG("RTDE read cancelled by a reader lifecycle change."); + return false; + } + if (reconnecting_) + { + URCL_LOG_DEBUG("RTDE read cancelled by reconnect."); + return false; } + if (!background_read_running_) + { + URCL_LOG_DEBUG("RTDE read cancelled because background reading stopped."); + return false; + } + auto* received = dynamic_cast(data_buffer0_.get()); + if (!new_data_ || received == nullptr) + { + URCL_LOG_ERROR("RTDE reader signalled data without a received data package."); + return false; + } + ensureOutputLayout(data_package, *received); + data_package = *received; + new_data_ = false; return true; } @@ -794,25 +904,38 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa "background reading or use getDataPackage(...)."); return false; } - // Cannot get data packages while reconnecting as we could end up getting some of the configuration packages - std::unique_ptr base_package(data_package.release()); std::unique_lock lock(reconnect_mutex_, std::defer_lock); if (lock.try_lock()) { - if (prod_->tryGet(base_package)) + if (!preallocated_data_pkg_.isTyped()) { - lock.unlock(); - auto package_type = base_package->getType(); - if (package_type != PackageType::RTDE_DATA_PACKAGE) - { - URCL_LOG_ERROR("Received package from RTDE interface is not a data package, but of type %d", package_type); - return false; - } - data_package.reset(dynamic_cast(base_package.release())); - return true; + URCL_LOG_ERROR("Cannot read RTDE data before recipe types are negotiated."); + return false; + } + // Recheck after acquiring the setup lock; never compete with a background socket reader. + if (background_read_running_ || reconnecting_) + { + URCL_LOG_DEBUG("Blocking RTDE read cancelled: background reading or reconnect is active."); + return false; + } + const auto read_into = [this](DataPackage& destination) { + return prod_->tryGetWithParser( + [this, &destination](comm::BinParser& bp) { return parser_.parseDataPackage(bp, destination); }); + }; + if (data_package) + { + ensureOutputLayout(data_package); + return read_into(*data_package); } - lock.unlock(); + std::unique_ptr candidate; + ensureOutputLayout(candidate); + if (!read_into(*candidate)) + { + return false; // The producer/parser diagnoses the failure. + } + data_package = std::move(candidate); + return true; } else { @@ -821,7 +944,6 @@ bool RTDEClient::getDataPackageBlocking(std::unique_ptr& data_packa std::this_thread::sleep_for(period); } - data_package.reset(dynamic_cast(base_package.release())); return false; } @@ -932,7 +1054,12 @@ void RTDEClient::reconnectCallback() { reconnecting_thread_.join(); } - reconnecting_ = true; + { + std::lock_guard lock(read_mutex_); + reconnecting_ = true; + ++background_read_session_id_; + } + background_read_cv_.notify_all(); reconnecting_thread_ = std::thread(&RTDEClient::reconnect, this); } @@ -943,17 +1070,45 @@ 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; + } + // 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(); + std::lock_guard lock(read_mutex_); + data_buffer0_ = std::move(buffer0); + data_buffer1_ = std::move(buffer1); + new_data_ = false; + ++background_read_session_id_; background_read_running_ = true; - data_buffer0_ = std::make_unique(output_recipe_, protocol_version_); - data_buffer1_ = std::make_unique(output_recipe_, protocol_version_); - - background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); + try + { + background_read_thread_ = std::thread(&RTDEClient::backgroundReadThreadFunc, this); + } + catch (...) + { + background_read_running_ = false; + background_read_cv_.notify_all(); + throw; + } } void RTDEClient::stopBackgroundRead() { - background_read_running_ = false; - background_read_cv_.notify_one(); + { + std::lock_guard lock(read_mutex_); + background_read_running_ = false; + new_data_ = false; + ++background_read_session_id_; + } + background_read_cv_.notify_all(); if (background_read_thread_.joinable()) { background_read_thread_.join(); @@ -979,15 +1134,19 @@ void RTDEClient::backgroundReadThreadFunc() { { std::scoped_lock rw_lock(read_mutex_, write_mutex_); + if (!background_read_running_ || reconnecting_) + { + continue; + } std::swap(data_buffer0_, data_buffer1_); + new_data_.store(true); } - new_data_.store(true); background_read_cv_.notify_one(); } else if (data_buffer1_->getType() == PackageType::RTDE_TEXT_MESSAGE) { - URCL_LOG_INFO(data_buffer1_->toString().c_str()); + URCL_LOG_INFO("%s", data_buffer1_->toString().c_str()); } } else @@ -1004,7 +1163,10 @@ void RTDEClient::backgroundReadThreadFunc() std::this_thread::sleep_for(period); } } - new_data_.store(false); + { + std::lock_guard lock(read_mutex_); + new_data_.store(false); + } URCL_LOG_INFO("RTDE background read thread stopped"); } diff --git a/src/rtde/rtde_parser.cpp b/src/rtde/rtde_parser.cpp index 48bd60c25..c40eecb4c 100644 --- a/src/rtde/rtde_parser.cpp +++ b/src/rtde/rtde_parser.cpp @@ -27,6 +27,65 @@ namespace urcl { namespace rtde_interface { +bool RTDEParser::parseDataPackagePayload(comm::BinParser& bp, DataPackage& package) const +{ + return package.parseWith(bp); +} + +bool RTDEParser::recipeTypesKnown() const +{ + if (expected_layout_known_) + { + 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::parseDataPackage(comm::BinParser& bp, DataPackage& destination) +{ + try + { + const auto type = getPackageTypeFromHeader(bp); + if (type != PackageType::RTDE_DATA_PACKAGE) + { + std::unique_ptr message(createNewPackageFromType(type)); + if (!message->parseWith(bp) || !bp.empty()) + { + URCL_LOG_ERROR("Malformed non-data RTDE frame, type %d.", static_cast(type)); + return false; + } + URCL_LOG_WARN("Expected RTDE data but received type %d: %s", static_cast(type), message->toString().c_str()); + return false; + } + if (!recipeTypesKnown()) + { + return false; + } + if (destination.layoutHash() != layout_hash_) + { + destination.setProtocolVersion(protocol_version_); + } + if (destination.layoutHash() != layout_hash_) + { + URCL_LOG_DEBUG("Cannot parse RTDE data: destination layout does not match the registered layout."); + return false; + } + if (!parseDataPackagePayload(bp, destination) || !bp.empty()) + { + URCL_LOG_ERROR("RTDE data payload was not parsed completely."); + return false; + } + return true; + } + catch (const UrException& error) + { + URCL_LOG_ERROR("RTDE data parsing failed: %s", error.what()); + return false; + } +} bool RTDEParser::parse(comm::BinParser& bp, std::vector>& results) { @@ -54,14 +113,44 @@ bool RTDEParser::parse(comm::BinParser& bp, std::vector package(new DataPackage(recipe_, protocol_version_)); - - if (!package->parseWith(bp)) + if (!recipeTypesKnown()) + { + return false; + } + if (expected_data_package_.has_value()) + { + // Backwards compatibility: deprecated vector overload allocates a fresh package per cycle from template. + auto package = std::make_unique(*expected_data_package_); + if (!parseDataPackagePayload(bp, *package) || !bp.empty()) + { + URCL_LOG_ERROR("RTDE data payload was not parsed completely."); + return false; + } + results.push_back(std::move(package)); + break; + } + if (results.empty() || results.back() == nullptr) + { + URCL_LOG_ERROR("Cannot parse an RTDE data package without a pre-allocated DataPackage with the expected " + "layout."); + return false; + } + DataPackage* package = dynamic_cast(results.back().get()); + if (package != nullptr && package->layoutHash() != layout_hash_) + { + // Re-sync negotiated protocol version in case parser version changed after package creation. + package->setProtocolVersion(protocol_version_); + } + if (package == nullptr || package->layoutHash() != layout_hash_) + { + URCL_LOG_DEBUG("Cannot parse RTDE data: destination layout does not match the registered layout."); + return false; + } + if (!parseDataPackagePayload(bp, *package)) { URCL_LOG_ERROR("Package parsing of type %d failed!", static_cast(type)); return false; } - results.push_back(std::move(package)); break; } default: @@ -104,25 +193,35 @@ 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) - { - URCL_LOG_WARN("The passed result pointer is empty. A new DataPackage will " - "have to be allocated. Please pass a pre-allocated DataPackage if you expect a DataPackage " - "would be sent."); - } - else + if (!expected_data_package_.has_value()) { - URCL_LOG_WARN("Passed a pre-allocated RTDE package of type %u while a DataPackage was received. A new " - "DataPackage will have to be allocated. Please pass a pre-allocated DataPackage if you expect " - "a DataPackage would be sent.", - result->getType()); + URCL_LOG_DEBUG("Cannot allocate RTDE data: no typed template is registered."); + return false; } - result = std::make_unique(recipe_, protocol_version_); + // Backwards compatibility: allocate from template if caller supplied null or non-data package. + URCL_LOG_WARN("Allocating an RTDE DataPackage; pass a matching pre-allocated package to avoid allocation."); + result = std::make_unique(*expected_data_package_); + } + + DataPackage* data_package = dynamic_cast(result.get()); + if (data_package != nullptr && data_package->layoutHash() != layout_hash_) + { + // Re-sync negotiated protocol version in case parser version changed after package creation. + data_package->setProtocolVersion(protocol_version_); + } + if (data_package == nullptr || data_package->layoutHash() != layout_hash_) + { + URCL_LOG_DEBUG("Cannot parse RTDE data: destination layout does not match the registered layout."); + return false; } - 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..bab4d03d9 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 @@ -59,28 +60,62 @@ RTDEWriter::RTDEWriter(comm::URStream* stream, const std::vector& recipe) { + std::lock_guard lock_guard(store_mutex_); if (running_) { throw UrException("Requesting to change the input recipe while the RTDEWriter is running. The writer has to be " "stopped before setting the recipe."); } - std::lock_guard lock_guard(store_mutex_); recipe_ = recipe; 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_); + if (running_) + { + throw UrException("Cannot change the RTDE protocol version while the writer is running."); + } + 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_); + if (running_) + { + throw UrException("Cannot apply RTDE recipe types while the writer is running."); + } + data_buffer0_->setTypes(types); + data_buffer1_->setTypes(types); +} + void RTDEWriter::init(uint8_t recipe_id) { if (running_) @@ -90,12 +125,19 @@ void RTDEWriter::init(uint8_t recipe_id) } { std::lock_guard lock_guard(store_mutex_); + if (running_) + { + throw UrException("Requesting to init a RTDEWriter while it is running. The writer has to be " + "stopped before initializing it."); + } data_buffer0_->setRecipeID(recipe_id); data_buffer1_->setRecipeID(recipe_id); + current_store_buffer_ = data_buffer0_; + current_send_buffer_ = data_buffer1_; + running_ = true; } recipe_id_ = recipe_id; new_data_available_ = false; - running_ = true; writer_thread_ = std::thread(&RTDEWriter::run, this); } @@ -143,11 +185,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 +460,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 5479ca8b7..797831465 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) @@ -217,6 +214,53 @@ 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} +) +set_tests_properties(RTDEClientFakeServerTest.server_sender_start_and_stop_are_idempotent + RTDEClientFakeServerTest.blocking_failures_preserve_caller_ownership + RTDEBackgroundReadTest.notifications_without_data_do_not_succeed + RTDEBackgroundReadTest.stop_and_reconnect_cancel_pending_reads + RTDEClientFakeServerTest.server_sender_start_and_stop_are_serialized + RTDEClientFakeServerTest.too_few_output_types_fail_then_recover + RTDEClientFakeServerTest.too_many_output_types_fail_then_recover + RTDEClientFakeServerTest.too_few_input_types_fail_then_recover + RTDEClientFakeServerTest.too_many_input_types_fail_then_recover + RTDEClientFakeServerTest.unknown_output_type_fails_then_recovers + RTDEClientFakeServerTest.unknown_input_type_fails_then_recovers + RTDEClientFakeServerTest.disconnection_wait_tracks_each_connection + RTDEClientFakeServerTest.repeated_output_negotiation_failures_recover + RTDEClientFakeServerTest.input_in_use_exhausts_retries_then_recovers + PROPERTIES TIMEOUT 60) + +# 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.destroying_the_server_immediately_after_start + 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..1be3b50b1 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,197 @@ 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. + // Finish callbacks before stopping the sender: an in-flight START may have acknowledged the + // request but not yet created send_thread_. Stopping it first would leave that new thread + // joinable when its destructor runs, causing std::terminate(). + server_.shutdown(); stopSendingDataPackages(); } +void RTDEServer::queueTextMessageBeforeVersionReply(const std::string& message) +{ + std::lock_guard lock(negotiation_mutex_); + pending_text_messages_.push_back(message); +} + +void RTDEServer::setHighestAcceptedProtocolVersion(const uint16_t highest_accepted) +{ + std::lock_guard lock(negotiation_mutex_); + highest_accepted_protocol_version_ = highest_accepted; +} + +std::vector RTDEServer::requestedProtocolVersions() +{ + std::lock_guard lock(negotiation_mutex_); + return requested_protocol_versions_; +} + +void RTDEServer::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::setOutputTypeReply(const std::optional>& types) +{ + std::lock_guard lock(negotiation_mutex_); + output_type_reply_ = types; +} + +void RTDEServer::setInputTypeReply(const std::optional>& types) +{ + std::lock_guard lock(negotiation_mutex_); + input_type_reply_ = types; +} + +void RTDEServer::sendTextMessage(const socket_t filedescriptor, const std::string& message) +{ + comm::PackageSerializer serializer; + uint8_t send_buffer[4096]; + size_t send_size = 0; + uint16_t protocol_version = 2; + { + std::lock_guard lock(negotiation_mutex_); + protocol_version = negotiated_protocol_version_; + } + + if (protocol_version == 1) + { + const uint8_t message_type = 0; + const uint16_t payload_size = static_cast(sizeof(message_type) + message.size()); + send_size += rtde_interface::PackageHeader::serializeHeader( + send_buffer, rtde_interface::PackageType::RTDE_TEXT_MESSAGE, payload_size); + send_size += serializer.serialize(send_buffer + send_size, message_type); + send_size += serializer.serialize(send_buffer + send_size, message); + } + else + { + const std::string source = "fake_rtde_server"; + const uint8_t warning_level = 1; + const uint16_t payload_size = + static_cast(2 * sizeof(uint8_t) + message.size() + source.size() + sizeof(warning_level)); + send_size += rtde_interface::PackageHeader::serializeHeader( + send_buffer, rtde_interface::PackageType::RTDE_TEXT_MESSAGE, 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(); + { + std::lock_guard lock(connection_mutex_); + client_connected_ = true; + } URCL_LOG_INFO("Client connected to RTDE server on FD %d", filedescriptor); } + +bool RTDEServer::waitForDisconnection(const std::chrono::milliseconds timeout) +{ + std::unique_lock lock(connection_mutex_); + return connection_cv_.wait_for(lock, timeout, [this]() { return !client_connected_; }); +} + +bool RTDEServer::sendTestFrame(const std::vector& frame) +{ + size_t written = 0; + return server_.write(client_socket_, frame.data(), frame.size(), written) && written == frame.size(); +} void RTDEServer::disconnectionCallback(const socket_t filedescriptor) { URCL_LOG_INFO("Client disconnected from RTDE server on FD %d", filedescriptor); + receive_buffer_.clear(); stopSendingDataPackages(); + { + std::lock_guard lock(connection_mutex_); + client_connected_ = false; + } + connection_cv_.notify_all(); } -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 +703,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 +731,133 @@ 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: { - bp.parse(output_frequency_); + // The client re-sends the output recipe on every retry, so one queued unexpected reply is + // consumed per request. + 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; + } + + uint16_t protocol_version = 2; + { + std::lock_guard lock(negotiation_mutex_); + protocol_version = negotiated_protocol_version_; + } + if (protocol_version == 2) + { + bp.parse(output_frequency_); + } + else + { + output_frequency_ = 125.0; + } 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_); + std::string variable_types_str; + { + std::lock_guard lock(negotiation_mutex_); + variable_types_str = joinStrings(output_type_reply_.value_or(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, protocol_version); + } + } comm::PackageSerializer serializer; uint8_t send_buffer[4096]; size_t send_size = 0; + uint16_t payload_size = static_cast(variable_types_str.length()); + if (protocol_version == 2) + { + payload_size = static_cast(variable_types_str.length() + sizeof(uint8_t)); + } 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))); - 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_buffer, rtde_interface::PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS, payload_size); + if (protocol_version == 2) + { + uint8_t recipe_id = 1; + send_size += serializer.serialize(send_buffer + send_size, recipe_id); + } + 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_); + std::string variable_types_str; + { + std::lock_guard lock(negotiation_mutex_); + variable_types_str = joinStrings(input_type_reply_.value_or(variable_types)); + } - input_data_package_ = std::make_unique(input_recipe_); + uint16_t protocol_version = 2; + { + std::lock_guard lock(negotiation_mutex_); + protocol_version = negotiated_protocol_version_; + } + input_data_package_.reset(); + if (allVariablesFound(variable_types)) + { + input_data_package_ = makeTypedDataPackage(input_recipe_, variable_types, 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_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 +865,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: @@ -185,6 +933,11 @@ void RTDEServer::messageCallback([[maybe_unused]] const socket_t filedescriptor, void RTDEServer::startSendingDataPackages() { + std::lock_guard thread_lock(thread_control_mutex_); + if (send_thread_.joinable()) + { + return; + } URCL_LOG_INFO("Start sending data."); send_loop_running_ = true; send_thread_ = std::thread(&RTDEServer::sendDataLoop, this); @@ -212,7 +965,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 +980,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..badeb9ecb 100644 --- a/tests/fake_rtde_server.h +++ b/tests/fake_rtde_server.h @@ -1,8 +1,12 @@ #pragma once +#include +#include +#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 +20,71 @@ 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); + + // Override only the wire acknowledgement, not the server's internal typed data. Overrides + // persist across reconnects so retries see the same fault; nullopt restores normal replies. + void setOutputTypeReply(const std::optional>& types); + void setInputTypeReply(const std::optional>& types); + + // Wait until the single-client slot and disconnect cleanup are finished before reconnecting. + // Call after a handshake has established the connection and the client has disconnected. + bool waitForDisconnection(const std::chrono::milliseconds timeout); + + // Inject one frame after init(), while streaming is paused and the connection is stable. + bool sendTestFrame(const std::vector& frame); + private: std::vector input_recipe_; std::vector output_recipe_; @@ -39,6 +101,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 +111,30 @@ 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 connection_mutex_; + std::condition_variable connection_cv_; + bool client_connected_ = false; + + std::mutex negotiation_mutex_; + std::deque pending_text_messages_; + std::deque pending_setup_outputs_text_messages_; + std::deque pending_setup_inputs_text_messages_; + std::optional> output_type_reply_; + std::optional> input_type_reply_; + 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..bca9a2562 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_->setExpectedDataPackage(test::typedPackage(recipe, { "DOUBLE" })); parser_->setProtocolVersion(2); producer_.reset(new comm::URProducer(*stream_.get(), *parser_.get())); diff --git a/tests/test_primary_client.cpp b/tests/test_primary_client.cpp index 0c870bb6e..523f6afe0 100644 --- a/tests/test_primary_client.cpp +++ b/tests/test_primary_client.cpp @@ -45,6 +45,10 @@ using namespace urcl; std::string g_ROBOT_IP = "192.168.56.101"; +// The fake-server fixture below binds a test port rather than the real primary one, which another +// process on the host may already be using. +constexpr int g_FAKE_PRIMARY_PORT = 60016; + class RobotMessageConsumer : public comm::IConsumer { public: @@ -102,8 +106,8 @@ class PrimaryClientFakeTest : public ::testing::Test protected: void SetUp() override { - server_ = std::make_unique(30001); - client_ = std::make_unique("127.0.0.1", notifier_); + server_ = std::make_unique(g_FAKE_PRIMARY_PORT); + client_ = std::make_unique("127.0.0.1", notifier_, g_FAKE_PRIMARY_PORT); EXPECT_NO_THROW(client_->start()); EXPECT_TRUE(server_->waitForClient()); } diff --git a/tests/test_primary_client_reconnect.cpp b/tests/test_primary_client_reconnect.cpp index 1032bc19b..686d0fee0 100644 --- a/tests/test_primary_client_reconnect.cpp +++ b/tests/test_primary_client_reconnect.cpp @@ -41,11 +41,18 @@ using namespace urcl; +// The real primary port is a poor choice for a test: another process on the host may already be +// listening on it, and TCPServer retries binding indefinitely, which would hang these tests +// instead of failing them. The tests below take the port away and give it back, so it has to be a +// fixed one rather than an ephemeral one. +constexpr int g_FAKE_PRIMARY_PORT = 60015; + // Regression test for ~PrimaryClient() blocking indefinitely when the pipeline's // 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 @@ -74,8 +81,8 @@ TEST(PrimaryClientReconnectTest, destructor_not_blocked_by_stuck_reconnect_threa { comm::INotifier notifier; - auto server = std::make_unique(primary_interface::UR_PRIMARY_PORT); - auto client = std::make_unique("127.0.0.1", notifier); + auto server = std::make_unique(g_FAKE_PRIMARY_PORT); + auto client = std::make_unique("127.0.0.1", notifier, g_FAKE_PRIMARY_PORT); // Unlimited reconnect attempts with a large reconnection time: if the fix is // absent, the producer's reconnect path keeps the destructor blocked. @@ -131,8 +138,8 @@ TEST(PrimaryClientReconnectTest, stop_not_blocked_by_stuck_reconnect_thread) { comm::INotifier notifier; - auto server = std::make_unique(primary_interface::UR_PRIMARY_PORT); - auto client = std::make_unique("127.0.0.1", notifier); + auto server = std::make_unique(g_FAKE_PRIMARY_PORT); + auto client = std::make_unique("127.0.0.1", notifier, g_FAKE_PRIMARY_PORT); // Unlimited reconnect attempts with a large reconnection time: if the fix is // absent, the producer's reconnect path keeps stop()'s pipeline join blocked. @@ -173,7 +180,7 @@ TEST(PrimaryClientReconnectTest, stop_not_blocked_by_stuck_reconnect_thread) // Restart-reuse check: bring up a fresh server and start() again. This must reconnect, // proving that the deliberate-stop state set by stop() was cleared by connect() on restart. - auto server2 = std::make_unique(primary_interface::UR_PRIMARY_PORT); + auto server2 = std::make_unique(g_FAKE_PRIMARY_PORT); ASSERT_NO_THROW(client->start(/*max_num_tries=*/0, large_reconnect_timeout)); EXPECT_TRUE(server2->waitForClient(std::chrono::seconds(3))) << "PrimaryClient did not reconnect after " "stop()/start() — the deliberate-stop state set by " diff --git a/tests/test_producer.cpp b/tests/test_producer.cpp index 32069be32..c0a3f2489 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.setExpectedDataPackage(test::typedPackage(recipe, { "DOUBLE" })); parser.setProtocolVersion(2); comm::URProducer producer(stream, parser); @@ -76,7 +79,8 @@ TEST_F(ProducerTest, get_data_package) server_->write(data_package, sizeof(data_package), written); std::vector> products; - EXPECT_EQ(producer.tryGet(products), true); + ASSERT_TRUE(producer.tryGet(products)); + ASSERT_EQ(products.size(), 1u); if (rtde_interface::DataPackage* data = dynamic_cast(products[0].get())) { @@ -99,6 +103,7 @@ TEST_F(ProducerTest, connect_non_connected_robot) std::vector recipe = { "timestamp" }; rtde_interface::RTDEParser parser(recipe); parser.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE" }).layoutHash()); comm::URProducer producer(stream, parser); auto start = std::chrono::system_clock::now(); diff --git a/tests/test_rtde_allocations.cpp b/tests/test_rtde_allocations.cpp new file mode 100644 index 000000000..068cd764f --- /dev/null +++ b/tests/test_rtde_allocations.cpp @@ -0,0 +1,678 @@ +// -- 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 ------------------------------------------------ + +// These tests count calls to the replaced allocation functions on the calling thread. +// Package construction, recipe negotiation and test assertions stay outside measured sections. +// Unit tests isolate typing, parsing, copying and serialization; fake-server tests exercise +// steady-state client reads and input submission after warm-up. Separate first-read tests measure +// applying negotiated types to untyped and incorrectly typed application packages. +// Background-thread allocations, direct malloc calls and aligned allocation overloads are not +// counted. Error handling and logging are not generally allocation-free. + +#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 parsing on this thread. Background receive and input submission measure +// the calling thread (copying the latest sample / copying into the store buffer); parsing and +// serialization themselves are covered 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 + +// Positive control: require a nonzero count for an explicit allocation, so a broken counter cannot +// make the allocation-free tests pass without observing any allocations. +// 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); +} + +// Construct the recipe storage and type vector before counting, then measure only setTypes(). +// Applying valid types must reuse that storage; check a resulting type after measurement. +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); +} + +// Serialize a valid frame before counting, then parse it 100 times into the same typed package. +// The borrowed parser must succeed on every iteration without allocating temporary packages. +TEST(DataPackageAllocationTest, borrowed_parser_reuses_storage) +{ + auto output = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(output); + uint8_t bytes[64]; + const auto size = output.serializePackage(bytes); + bool success = true; + size_t allocations = 0; + { + AllocationCounter counter; + for (size_t i = 0; i < 100; ++i) + { + comm::BinParser bp(bytes, size); + success = parser.parseDataPackage(bp, output) && success; + } + allocations = counter.count(); + } + EXPECT_TRUE(success); + EXPECT_EQ(allocations, 0u); +} + +// Deliberately pass a foreign layout with DEBUG logging enabled. Parsing must fail and emit one +// diagnostic. Even a non-allocating handler cannot avoid the logger's formatting allocation, +// demonstrating why enabled failure diagnostics are outside the allocation-free guarantee. +TEST(DataPackageAllocationTest, enabled_failure_diagnostics_are_not_allocation_free) +{ + struct DiagnosticHandler : LogHandler + { + explicit DiagnosticHandler(size_t& calls) : calls_(calls) + { + } + void log(const char*, int, LogLevel, const char*) override + { + ++calls_; + } + size_t& calls_; + }; + struct RestoreLogger + { + ~RestoreLogger() + { + unregisterLogHandler(); + setLogLevel(LogLevel::INFO); + } + } restore; + size_t messages = 0; + registerLogHandler(std::make_unique(messages)); + setLogLevel(LogLevel::DEBUG); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + auto foreign = test::typedPackage({ "other" }, { "DOUBLE" }); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(expected); + uint8_t bytes[64]; + const auto size = expected.serializePackage(bytes); + size_t allocations = 0; + bool parsed = true; + { + AllocationCounter counter; + comm::BinParser bp(bytes, size); + parsed = parser.parseDataPackage(bp, foreign); + allocations = counter.count(); + } + EXPECT_FALSE(parsed); + EXPECT_EQ(messages, 1u); + // urcl::log allocates its formatting buffer even with a non-allocating handler. + EXPECT_GT(allocations, 0u); +} + +// Parse a known wire frame through the unique-pointer overload into an already typed package. +// Require zero allocations and verify the decoded timestamp, so simply rejecting the frame +// cannot satisfy the allocation check. +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.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, types).layoutHash()); + // 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)); + try + { + parsed = parser.parse(bp, product); + } + catch (const urcl::UrException&) + { + parsed = false; + } + 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); +} + +// Unlike RTDEClient, the parser does not apply negotiated types to an untyped destination. +// Register only the expected layout hash, then require rejection without allocation or pointer +// replacement. The layout-mismatch DEBUG diagnostic is disabled for this measurement. +TEST(DataPackageAllocationTest, parsing_into_an_untyped_package_is_rejected) +{ + unsigned char raw_data[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(recipe); + parser.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }).layoutHash()); + 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_FALSE(parsed); + EXPECT_EQ(product.get(), package_address); +} + +// Copy a partially initialized source into a typed destination with a previously nonzero mask. +// Measure copyFrom() only, then verify the supplied fraction is copied and the unset mask becomes +// a typed zero. INFO logging remains enabled to catch unexpected allocating chatter on this path. +TEST(DataPackageAllocationTest, copying_a_partial_package_does_not_allocate) +{ + auto destination = test::typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(destination.setData("speed_slider_mask", uint32_t{ 1 })); + 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); + uint32_t mask = 1; + double fraction = 0.0; + ASSERT_TRUE(destination.getData("speed_slider_mask", mask)); + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + EXPECT_EQ(mask, 0u); + EXPECT_DOUBLE_EQ(fraction, 0.5); +} + +// Serialize an already typed UINT32 field into caller-owned storage while counting allocations. +// Require zero allocations and the expected eight-byte frame size: header, recipe ID and payload. +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(); + } + + // Representative output recipe covering scalar, vector and digital-bit fields. + 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_; +}; + +// After warm-up, measure synchronous socket reads, parsing and selected scalar/vector/bitset +// accessors on the calling thread. Require zero allocations, successful field reads and at least +// one received package. First-read typing and wrong-type repair have separate allocation tests. +TEST_F(RTDEAllocationTest, blocking_receive_does_not_allocate) +{ + ASSERT_TRUE(client_->start(false)); + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + + // The first warm-up read applies the negotiated types to this recipe-only package. + // The remaining 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); +} + +// Warm up the connection using a different package, leaving the measured destination untyped. +// Count its first read, including application of negotiated types, then require successful +// receipt, a typed destination and a readable timestamp without allocation. +TEST_F(RTDEAllocationTest, typing_a_package_on_the_first_read_does_not_allocate) +{ + ASSERT_TRUE(client_->start(false)); + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_FALSE(data_pkg->isTyped()); + + // Warm up on a package of its own so the measured read is the first one for data_pkg + auto warmup_pkg = std::make_unique(client_->getOutputRecipe()); + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getDataPackageBlocking(warmup_pkg)); + } + + bool received = false; + std::size_t allocations = 0; + { + AllocationCounter counter; + received = client_->getDataPackageBlocking(data_pkg); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(received); + EXPECT_TRUE(data_pkg->isTyped()); + + double timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); +} + +// A same-recipe package may already carry incorrect types. The client must replace them with +// the negotiated layout before parsing, without allocating or replacing the package object. +// Warm up using a separate package so the measured read includes the incorrect layout's repair. +TEST_F(RTDEAllocationTest, repairing_wrong_types_on_the_first_read_does_not_allocate) +{ + ASSERT_TRUE(client_->start(false)); + + auto warmup_pkg = std::make_unique(client_->getOutputRecipe()); + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getDataPackageBlocking(warmup_pkg)); + } + ASSERT_EQ(warmup_pkg->getDataType("joint_mode"), rtde_interface::DataType::VECTOR6INT32); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + // Deliberately wrong: joint_mode is a six-element integer vector, not an INT32 scalar. + // Do not correct this entry: it is the mismatch whose repair this test measures. + data_pkg->setTypes( + { "DOUBLE", "VECTOR6D", "VECTOR6D", "UINT32", "UINT32", "UINT64", "INT32", "VECTOR3D", "UINT32", "INT32" }); + + ASSERT_TRUE(data_pkg->isTyped()); + ASSERT_EQ(data_pkg->getDataType("joint_mode"), rtde_interface::DataType::INT32); + ASSERT_NE(data_pkg->layoutHash(), warmup_pkg->layoutHash()); + const auto* package_address = data_pkg.get(); + + bool received = false; + std::size_t allocations = 0; + { + AllocationCounter counter; + received = client_->getDataPackageBlocking(data_pkg); + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0u); + ASSERT_TRUE(received); + ASSERT_EQ(data_pkg.get(), package_address); + EXPECT_EQ(data_pkg->layoutHash(), warmup_pkg->layoutHash()); + ASSERT_EQ(data_pkg->getDataType("joint_mode"), rtde_interface::DataType::VECTOR6INT32); + + // Verify that the repaired layout supports reading the vector and that data was received. + vector6int32_t joint_mode{}; + EXPECT_TRUE(data_pkg->getData("joint_mode", joint_mode)); + double timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); +} + +// Warm up a reusable destination, then measure copying the latest background sample and reading +// its timestamp on the calling thread. Require successful receipt without allocation; parsing +// on the background reader thread is outside this counter and is tested separately. +TEST_F(RTDEAllocationTest, copying_the_latest_background_package_does_not_allocate) +{ + ASSERT_TRUE(client_->start(true)); + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + const std::chrono::milliseconds read_timeout{ 100 }; + + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); + } + + int received = 0; + bool all_data_read = true; + double timestamp = 0.0; + std::size_t allocations = 0; + { + AllocationCounter counter; + for (int i = 0; i < g_MEASURED_CYCLES; ++i) + { + if (!client_->getDataPackage(data_pkg, read_timeout)) + { + continue; + } + ++received; + all_data_read &= data_pkg.getData("timestamp", timestamp); + } + allocations = counter.count(); + } + + EXPECT_EQ(allocations, 0); + EXPECT_TRUE(all_data_read); + EXPECT_GT(received, 0); + EXPECT_GT(timestamp, 0.0); + + client_->pause(); +} + +// Create a negotiated input package before counting. Repeatedly update its fraction and submit +// it to the writer, requiring every operation to succeed without calling-thread allocations. +// This measures store-buffer updates, not background serialization or delivery to the server. +TEST_F(RTDEAllocationTest, copying_input_data_into_the_store_buffer_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)); + } + + 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(); +} + +// Leave the input mask untyped and set only the fraction, then repeatedly submit that partial +// package after warm-up. Submission must accept its compatible fields and fill unset fields +// with typed zeros without calling-thread allocations. Wire delivery is not checked here. +TEST_F(RTDEAllocationTest, sending_a_partial_package_does_not_allocate) +{ + ASSERT_TRUE(client_->start(true)); + rtde_interface::DataPackage input_pkg(client_->getInputRecipe()); + ASSERT_TRUE(input_pkg.setData("speed_slider_fraction", 0.5)); + + for (int i = 0; i < g_WARMUP_CYCLES; ++i) + { + ASSERT_TRUE(client_->getWriter().sendPackage(input_pkg)); + } + + bool all_sent = true; + std::size_t allocations = 0; + { + AllocationCounter counter; + for (int i = 0; i < g_MEASURED_CYCLES; ++i) + { + 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..e72ae142f 100644 --- a/tests/test_rtde_client.cpp +++ b/tests/test_rtde_client.cpp @@ -32,7 +32,8 @@ #include #include #include -#include +#include +#include #include #include #include "ur_client_library/comm/tcp_server.h" @@ -290,17 +291,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 +312,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 +366,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 throws. rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); double timestamp; - EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_THROW(data_pkg.getData("timestamp", timestamp), std::bad_variant_access); ASSERT_TRUE(data_pkg.setData("timestamp", 0.0)); ASSERT_TRUE(client_->getDataPackage(data_pkg, read_timeout)); @@ -445,7 +447,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 +461,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 +571,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 +623,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 +711,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..e6204c0bd --- /dev/null +++ b/tests/test_rtde_client_fake_server.cpp @@ -0,0 +1,1090 @@ +// -- 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 +#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("127.0.0.1", notifier_, output_recipe, input_recipe, + target_frequency, ignore_unavailable_outputs, g_FAKE_RTDE_PORT); + } + + void expectFailedNegotiationThenRecovery() + { + // Count failures as bounded initialization attempts, not elapsed time. An IN_USE response is + // caught internally and retried; malformed counts also return false from setupCommunication. + EXPECT_THROW(client_->init(1, std::chrono::milliseconds(10), 2, std::chrono::milliseconds(10)), UrException); + ASSERT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_THROW(client_->createInputDataPackage(), UrException); + + // init() closes the client socket on failure, but the server processes that close asynchronously. + // Wait for its single-client slot to be released so recovery tests negotiation, not connection rejection. + ASSERT_TRUE(server_->waitForDisconnection(std::chrono::seconds(1))); + server_->setOutputTypeReply(std::nullopt); + server_->setInputTypeReply(std::nullopt); + ASSERT_TRUE(client_->init(1, std::chrono::milliseconds(10), 1, std::chrono::milliseconds(10))); + ASSERT_EQ(client_->getClientState(), rtde_interface::ClientState::INITIALIZED); + EXPECT_EQ(client_->getOutputRecipe(), g_OUTPUT_RECIPE); + EXPECT_EQ(client_->getInputRecipe(), g_INPUT_RECIPE); + + auto input = client_->createInputDataPackage(); + EXPECT_TRUE(input.isTyped()); + EXPECT_EQ(input.getDataType("speed_slider_mask"), rtde_interface::DataType::UINT32); + EXPECT_EQ(input.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); + ASSERT_TRUE(input.setData("speed_slider_fraction", 0.5)); + EXPECT_FALSE(input.setData("speed_slider_mask", uint8_t(1))); + + ASSERT_TRUE(client_->start(true)); + rtde_interface::DataPackage output(g_OUTPUT_RECIPE); + ASSERT_TRUE(client_->getDataPackage(output, std::chrono::seconds(1))); + EXPECT_TRUE(output.isTyped()); + EXPECT_EQ(output.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + EXPECT_EQ(output.getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + EXPECT_EQ(output.getDataType("target_speed_fraction"), rtde_interface::DataType::DOUBLE); + EXPECT_EQ(output.getDataType("runtime_state"), rtde_interface::DataType::UINT32); + double timestamp = 0; + ASSERT_TRUE(output.getData("timestamp", timestamp)); + EXPECT_GE(timestamp, 40.0); + EXPECT_TRUE(client_->pause()); + } + + 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, server_sender_start_and_stop_are_idempotent) +{ + ASSERT_TRUE(client_->init()); + + server_->startSendingDataPackages(); + server_->startSendingDataPackages(); + server_->stopSendingDataPackages(); + server_->stopSendingDataPackages(); + + // Restart directly: the manual sender calls can leave data queued on the socket. + // A protocol START could mistake that data for its reply and leave the START ack unread. + server_->startSendingDataPackages(); + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + EXPECT_TRUE(client_->getDataPackageBlocking(data_pkg)); +} + +TEST_F(RTDEClientFakeServerTest, server_sender_start_and_stop_are_serialized) +{ + ASSERT_TRUE(client_->init()); + + auto toggle_sender = [this]() { + for (int i = 0; i < 25; ++i) + { + server_->startSendingDataPackages(); + server_->stopSendingDataPackages(); + } + }; + std::thread first(toggle_sender); + std::thread second(toggle_sender); + first.join(); + second.join(); + + // As above, keep the sender lifecycle check separate from the protocol START handshake. + server_->startSendingDataPackages(); + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + EXPECT_TRUE(client_->getDataPackageBlocking(data_pkg)); +} + +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"; +} + +TEST_F(RTDEClientFakeServerTest, init_succeeds_after_protocol_v1_fallback) +{ + server_->setHighestAcceptedProtocolVersion(1); + + ASSERT_TRUE(client_->init()); + const std::vector requested = server_->requestedProtocolVersions(); + ASSERT_GE(requested.size(), 2u); + EXPECT_EQ(requested[0], 2); + EXPECT_EQ(requested[1], 1); + + 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; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + EXPECT_GT(timestamp, 0.0); + + rtde_interface::DataPackage input_pkg = client_->createInputDataPackage(); + ASSERT_TRUE(input_pkg.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(input_pkg.setData("speed_slider_fraction", 0.25)); + EXPECT_TRUE(client_->getWriter().sendPackage(input_pkg)); + + client_->pause(); +} + +// 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); + // A thrown init() must leave the client usable for another attempt, not stuck INITIALIZING. + 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, unknown_field_is_accepted_by_the_constructor) +{ + EXPECT_NO_THROW(makeClient({ "timestamp", "not_a_field_the_robot_knows" }, g_INPUT_RECIPE, g_RTDE_FREQUENCY)); +} + +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(); +} + +// A package built from the output recipe alone carries no data types. The robot reports those +// during the handshake, and the first read is what puts them on the application's package. +TEST_F(RTDEClientFakeServerTest, the_first_read_types_the_package) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(false)); + + auto data_pkg = std::make_unique(client_->getOutputRecipe()); + ASSERT_FALSE(data_pkg->isTyped()); + + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + + EXPECT_TRUE(data_pkg->isTyped()); + EXPECT_EQ(data_pkg->getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + EXPECT_EQ(data_pkg->getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + + // Reading into the same package again keeps it usable + double first_timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", first_timestamp)); + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + double second_timestamp = 0.0; + ASSERT_TRUE(data_pkg->getData("timestamp", second_timestamp)); + EXPECT_GE(second_timestamp, first_timestamp); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, the_first_background_read_types_the_package) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage data_pkg(client_->getOutputRecipe()); + ASSERT_FALSE(data_pkg.isTyped()); + + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + + EXPECT_TRUE(data_pkg.isTyped()); + EXPECT_EQ(data_pkg.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, receiving_into_a_foreign_recipe_is_repaired) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(false)); + + auto foreign_pkg = std::make_unique(std::vector{ "timestamp" }); + auto* original = foreign_pkg.get(); + ASSERT_TRUE(client_->getDataPackageBlocking(foreign_pkg)); + EXPECT_EQ(foreign_pkg.get(), original); + EXPECT_EQ(foreign_pkg->getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + ASSERT_TRUE(client_->getDataPackageBlocking(foreign_pkg)); + EXPECT_EQ(foreign_pkg.get(), original); + + client_->pause(); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage other_foreign_pkg({ "timestamp" }); + ASSERT_TRUE(client_->getDataPackage(other_foreign_pkg, g_READ_TIMEOUT)); + EXPECT_EQ(other_foreign_pkg.getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + ASSERT_TRUE(client_->getDataPackage(other_foreign_pkg, g_READ_TIMEOUT)); + + foreign_pkg = std::make_unique(std::vector{ "timestamp" }); + original = foreign_pkg.get(); + ASSERT_TRUE(client_->getDataPackage(foreign_pkg, g_READ_TIMEOUT)); + EXPECT_EQ(foreign_pkg.get(), original); + EXPECT_EQ(foreign_pkg->getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, receiving_into_an_empty_pointer_allocates_one) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(false)); + + std::unique_ptr data_pkg; + ASSERT_TRUE(client_->getDataPackageBlocking(data_pkg)); + ASSERT_NE(data_pkg, nullptr); + EXPECT_TRUE(data_pkg->isTyped()); + EXPECT_EQ(data_pkg->getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + + client_->pause(); + ASSERT_TRUE(client_->start(true)); + data_pkg.reset(); + ASSERT_TRUE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + ASSERT_NE(data_pkg, nullptr); + EXPECT_TRUE(data_pkg->isTyped()); + EXPECT_EQ(data_pkg->getDataType("actual_q"), rtde_interface::DataType::VECTOR6D); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, blocking_failures_preserve_caller_ownership) +{ + ASSERT_TRUE(client_->init()); // init leaves streaming paused, so no DATA races with the injected frames. + auto output = std::make_unique(client_->getOutputRecipe()); + auto* original = output.get(); + const std::vector> frames{ + { 0x00, 0x04, 0x53, 0x01 }, + { 0x00, 0x07, 0x4d, 0x01, 'x', 0x00, 0x01 }, + { 0x00, 0x04, 0x4d, 0xff }, + { 0x00, 0x04, 0x55, 0x01 }, + }; + for (const auto& frame : frames) + { + ASSERT_TRUE(server_->sendTestFrame(frame)); + EXPECT_FALSE(client_->getDataPackageBlocking(output)); + ASSERT_EQ(output.get(), original); + std::unique_ptr empty; + ASSERT_TRUE(server_->sendTestFrame(frame)); + EXPECT_FALSE(client_->getDataPackageBlocking(empty)); + EXPECT_EQ(empty, nullptr); + } + ASSERT_TRUE(client_->start(false)); + EXPECT_TRUE(client_->getDataPackageBlocking(output)); + EXPECT_EQ(output.get(), original); + EXPECT_TRUE(client_->pause()); +} + +namespace +{ +// Exercise consumer synchronization without a socket reader racing to deliver extra data. +// Uses the same protected publication state as the real worker, but never starts a worker thread. +class BackgroundReadHarness : public rtde_interface::RTDEClient +{ +public: + explicit BackgroundReadHarness(comm::INotifier& notifier) + : RTDEClient("127.0.0.1", notifier, std::vector{ "timestamp" }, std::vector{}) + { + preallocated_data_pkg_.setTypes({ "DOUBLE" }); + prepareReader(); + } + + void prepareReader() + { + std::lock_guard lock(read_mutex_); + data_buffer0_ = std::make_unique(preallocated_data_pkg_); + new_data_ = false; + background_read_running_ = true; + ++background_read_session_id_; + } + + void notifyWithoutData() + { + background_read_cv_.notify_all(); + } + + void cancelForReconnect() + { + { + std::lock_guard lock(read_mutex_); + reconnecting_ = true; + ++background_read_session_id_; + } + background_read_cv_.notify_all(); + } + + void publish(double timestamp) + { + { + std::lock_guard lock(read_mutex_); + dynamic_cast(*data_buffer0_).setData("timestamp", timestamp); + new_data_ = true; + } + background_read_cv_.notify_one(); + } +}; +} // namespace + +TEST(RTDEBackgroundReadTest, timeout_does_not_publish_an_empty_pointer) +{ + comm::INotifier notifier; + BackgroundReadHarness client(notifier); + std::unique_ptr output; + EXPECT_FALSE(client.getDataPackage(output, std::chrono::milliseconds(1))); + EXPECT_EQ(output, nullptr); + client.publish(42.0); + ASSERT_TRUE(client.getDataPackage(output, std::chrono::milliseconds(1))); + double timestamp = 0; + ASSERT_TRUE(output->getData("timestamp", timestamp)); + EXPECT_EQ(timestamp, 42.0); +} + +TEST(RTDEBackgroundReadTest, notifications_without_data_do_not_succeed) +{ + comm::INotifier notifier; + BackgroundReadHarness client(notifier); + std::unique_ptr output; + auto read = + std::async(std::launch::async, [&] { return client.getDataPackage(output, std::chrono::milliseconds(50)); }); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(1); + while (read.wait_for(std::chrono::milliseconds(0)) != std::future_status::ready && + std::chrono::steady_clock::now() < deadline) + { + client.notifyWithoutData(); + std::this_thread::yield(); + } + EXPECT_FALSE(read.get()); + EXPECT_EQ(output, nullptr); +} + +TEST(RTDEBackgroundReadTest, stop_and_reconnect_cancel_pending_reads) +{ + for (const bool reconnect : { false, true }) + { + comm::INotifier notifier; + BackgroundReadHarness client(notifier); + std::unique_ptr output; + std::promise entered; + auto read = std::async(std::launch::async, [&] { + entered.set_value(); + return client.getDataPackage(output, std::chrono::seconds(2)); + }); + entered.get_future().wait(); + EXPECT_EQ(read.wait_for(std::chrono::milliseconds(20)), std::future_status::timeout); + if (reconnect) + client.cancelForReconnect(); + else + client.stopBackgroundRead(); + EXPECT_EQ(read.wait_for(std::chrono::milliseconds(500)), std::future_status::ready); + EXPECT_FALSE(read.get()); + EXPECT_EQ(output, nullptr); + } +} + +TEST(RTDEBackgroundReadTest, restart_does_not_publish_stale_data) +{ + comm::INotifier notifier; + BackgroundReadHarness client(notifier); + client.publish(42.0); + client.stopBackgroundRead(); + client.prepareReader(); + std::unique_ptr output; + EXPECT_FALSE(client.getDataPackage(output, std::chrono::milliseconds(1))); + EXPECT_EQ(output, nullptr); + client.publish(43.0); + ASSERT_TRUE(client.getDataPackage(output, std::chrono::milliseconds(1))); + double timestamp = 0; + ASSERT_TRUE(output->getData("timestamp", timestamp)); + EXPECT_EQ(timestamp, 43.0); +} + +TEST_F(RTDEClientFakeServerTest, unavailable_reads_leave_an_empty_pointer_untouched) +{ + std::unique_ptr data_pkg; + EXPECT_FALSE(client_->getDataPackageBlocking(data_pkg)); + EXPECT_EQ(data_pkg, nullptr); + EXPECT_FALSE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + EXPECT_EQ(data_pkg, nullptr); + + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(false)); + EXPECT_FALSE(client_->getDataPackage(data_pkg, g_READ_TIMEOUT)); + EXPECT_EQ(data_pkg, nullptr); + 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, received_package_rejects_a_wrong_get_data_type) +{ + 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)); + + uint32_t timestamp_as_int = 0; + EXPECT_THROW(data_pkg.getData("timestamp", timestamp_as_int), std::bad_variant_access); + double timestamp = 0.0; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + + client_->pause(); +} + +TEST_F(RTDEClientFakeServerTest, create_input_data_package_matches_the_robot_types) +{ + ASSERT_TRUE(client_->init()); + ASSERT_TRUE(client_->start(true)); + + rtde_interface::DataPackage typed = client_->createInputDataPackage(); + EXPECT_EQ(typed.getDataType("speed_slider_mask"), rtde_interface::DataType::UINT32); + EXPECT_EQ(typed.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); + ASSERT_TRUE(typed.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(typed.setData("speed_slider_fraction", 0.3)); + EXPECT_TRUE(client_->getWriter().sendPackage(typed)); + + rtde_interface::DataPackage recipe_built(client_->getInputRecipe()); + ASSERT_TRUE(recipe_built.setData("speed_slider_mask", static_cast(1))); + ASSERT_TRUE(recipe_built.setData("speed_slider_fraction", 0.4)); + EXPECT_TRUE(client_->getWriter().sendPackage(recipe_built)); + + rtde_interface::DataPackage wrong(client_->getInputRecipe()); + ASSERT_TRUE(wrong.setData("speed_slider_mask", static_cast(1))); + EXPECT_FALSE(client_->getWriter().sendPackage(wrong)); + + client_->pause(); +} + +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, too_few_output_types_fail_then_recover) +{ + server_->setOutputTypeReply(std::vector{ "DOUBLE", "VECTOR6D", "DOUBLE" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, too_many_output_types_fail_then_recover) +{ + server_->setOutputTypeReply(std::vector{ "DOUBLE", "VECTOR6D", "DOUBLE", "UINT32", "DOUBLE" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, too_few_input_types_fail_then_recover) +{ + server_->setInputTypeReply(std::vector{ "UINT32" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, too_many_input_types_fail_then_recover) +{ + server_->setInputTypeReply(std::vector{ "UINT32", "DOUBLE", "BOOL" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, unknown_output_type_fails_then_recovers) +{ + server_->setOutputTypeReply(std::vector{ "DOUBLE", "VECTOR6D", "DOUBLE", "UNKNOWN_TYPE" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, unknown_input_type_fails_then_recovers) +{ + server_->setInputTypeReply(std::vector{ "UINT32", "UNKNOWN_TYPE" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); +} + +TEST_F(RTDEClientFakeServerTest, disconnection_wait_tracks_each_connection) +{ + EXPECT_TRUE(server_->waitForDisconnection(std::chrono::milliseconds(0))); + for (int iteration = 0; iteration < 5; ++iteration) + { + SCOPED_TRACE(iteration); + ASSERT_TRUE(client_->init()); + EXPECT_FALSE(server_->waitForDisconnection(std::chrono::milliseconds(1))); + client_.reset(); + ASSERT_TRUE(server_->waitForDisconnection(std::chrono::seconds(1))); + EXPECT_TRUE(server_->waitForDisconnection(std::chrono::milliseconds(0))); + client_ = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, g_RTDE_FREQUENCY); + } +} + +TEST_F(RTDEClientFakeServerTest, repeated_output_negotiation_failures_recover) +{ + for (int iteration = 0; iteration < 50; ++iteration) + { + SCOPED_TRACE(iteration); + if (iteration % 2 == 0) + { + server_->setOutputTypeReply(std::vector{ "DOUBLE", "VECTOR6D", "DOUBLE", "UNKNOWN_TYPE" }); + } + else + { + server_->setOutputTypeReply(std::vector{ "DOUBLE", "VECTOR6D", "DOUBLE", "UINT32", "DOUBLE" }); + } + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); + client_.reset(); + ASSERT_TRUE(server_->waitForDisconnection(std::chrono::seconds(1))); + client_ = makeClient(g_OUTPUT_RECIPE, g_INPUT_RECIPE, g_RTDE_FREQUENCY); + } +} + +TEST_F(RTDEClientFakeServerTest, input_in_use_exhausts_retries_then_recovers) +{ + server_->setInputTypeReply(std::vector{ "IN_USE", "DOUBLE" }); + ASSERT_NO_FATAL_FAILURE(expectFailedNegotiationThenRecovery()); + // Two failed handshakes and the successful recovery each negotiate v2 exactly once. + EXPECT_EQ(server_->requestedProtocolVersions(), (std::vector{ 2, 2, 2 })); +} + +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(); +} + +// Without a library-owned field table a typo and a field of a newer robot look the same. Both are +// stripped when ignore_unavailable_outputs is set. +TEST_F(RTDEClientFakeServerTest, ignore_unavailable_outputs_strips_a_typo_and_an_unknown_newer_field) +{ + auto client = makeClient({ "timestamp", "actual_q", "typo_output", "field_of_a_newer_robot" }, 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); +} + +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, create_input_data_package_before_init_throws) +{ + EXPECT_THROW(client_->createInputDataPackage(), UrException); +} + +TEST_F(RTDEClientFakeServerTest, empty_input_recipe_does_not_start_the_writer) +{ + auto client = makeClient(g_OUTPUT_RECIPE, {}, g_RTDE_FREQUENCY); + + ASSERT_TRUE(client->init()); + EXPECT_THROW(client->createInputDataPackage(), UrException); + + ASSERT_TRUE(client->start(true)); + rtde_interface::DataPackage data_pkg(client->getOutputRecipe()); + EXPECT_TRUE(client->getDataPackage(data_pkg, g_READ_TIMEOUT)); + double timestamp = 0.0; + EXPECT_TRUE(data_pkg.getData("timestamp", timestamp)); + + client->pause(); +} + +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..04dcec3e0 --- /dev/null +++ b/tests/test_rtde_client_reconnect.cpp @@ -0,0 +1,355 @@ +// -- 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() + { + // TCPServer listens on IPv4 only. Avoid an initial IPv6 connection attempt for localhost: + // on Windows its fallback delay can exhaust the teardown stress test's timeout over 50 cycles. + client_ = std::make_unique("127.0.0.1", 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(); +} + +// A START acknowledgement can reach the client before the server callback creates its sender. +// Exercise teardown immediately after that acknowledgement, without a sleep to hide the race. +TEST_F(RTDEClientReconnectTest, destroying_the_server_immediately_after_start) +{ + for (int attempt = 0; attempt < 50; ++attempt) + { + SCOPED_TRACE(attempt); + startServer(); + makeClient(); + ASSERT_TRUE(client_->init()); + // No background reader: this checks server teardown without also launching a reconnect. + ASSERT_TRUE(client_->start(false)); + server_.reset(); + 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); + + // Each failed handshake tries protocol versions 2 and 1. Wait for both attempts rather + // than assuming they finish within a fixed delay: reconnect() waits in 250 ms increments + // even with a 50 ms initialization timeout, and scheduling can delay either attempt. + const std::vector expected_requests{ 2, 1, 2, 1 }; + const auto deadline = std::chrono::steady_clock::now() + g_STATE_CHANGE_TIMEOUT; + while (server_->requestedProtocolVersions().size() < expected_requests.size() && + std::chrono::steady_clock::now() < deadline) + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + keep_running = false; + data_consumer.join(); + + ASSERT_EQ(server_->requestedProtocolVersions(), expected_requests) << "the client did not perform two failed " + "handshakes"; + EXPECT_TRUE(waitForState(rtde_interface::ClientState::UNINITIALIZED)); + // During a retry the client is UNINITIALIZED between attempts, so a later state check alone + // cannot prove it stopped. Observe for longer than the 250 ms retry-wait increment so an + // unwanted third attempt can be detected, rather than sampling during the final retry wait. + std::this_thread::sleep_for(std::chrono::seconds(1)); + EXPECT_EQ(client_->getClientState(), rtde_interface::ClientState::UNINITIALIZED); + EXPECT_EQ(server_->requestedProtocolVersions(), expected_requests) << "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..4b4e8d280 100644 --- a/tests/test_rtde_data_package.cpp +++ b/tests/test_rtde_data_package.cpp @@ -27,15 +27,27 @@ //---------------------------------------------------------------------- #include +#include +#include +#include +#include +#include +#include +#include +#include #include +#include "rtde_test_helpers.h" + using namespace urcl; +using urcl::test::typedPackage; TEST(rtde_data_package, serialize_pkg) { std::vector recipe{ "speed_slider_mask" }; - rtde_interface::DataPackage package(recipe); + std::vector types{ "UINT32" }; + auto package = typedPackage(recipe, types); uint32_t value = 1; package.setData("speed_slider_mask", value); @@ -57,8 +69,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 +104,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 +136,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 +172,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 +182,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 +200,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 +215,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 +234,969 @@ 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_THROW(package.getData("f_bool", double_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_uint8", uint32_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_uint32", int32_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_uint64", uint32_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_int32", uint32_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_double", uint64_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_vector3d", vector6d_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_vector6d", vector3d_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_v6int32", v6uint32_value), std::bad_variant_access); + EXPECT_THROW(package.getData("f_v6uint32", v6int32_value), std::bad_variant_access); +} + +// 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, every_data_type_obeys_get_set_invariants) +{ + using Value = rtde_interface::DataPackage::_rtde_type_variant; + const std::vector> cases{ + { "BOOL", true }, + { "UINT8", uint8_t{ 0xa5 } }, + { "UINT32", uint32_t{ 0x12345678 } }, + { "UINT64", uint64_t{ 0x0123456789abcdef } }, + { "INT32", int32_t{ -12345 } }, + { "DOUBLE", -12.5 }, + { "VECTOR3D", vector3d_t{ 1.5, -2.5, 3.5 } }, + { "VECTOR6D", vector6d_t{ 1, -2, 3, -4, 5, -6 } }, + { "VECTOR6INT32", vector6int32_t{ -1, 2, -3, 4, -5, 6 } }, + { "VECTOR6UINT32", vector6uint32_t{ 1, 2, 3, 4, 5, 0xffffffffu } }, + }; + + for (const auto& entry : cases) + { + SCOPED_TRACE(entry.first); + rtde_interface::DataPackage untyped({ "field" }); + auto typed = typedPackage({ "field" }, { entry.first }); + std::visit( + [&](const auto& value) { + using T = std::decay_t; + if constexpr (!std::is_same_v) + { + for (auto* package : { &untyped, &typed }) + { + const bool was_typed = package->isTyped(); + SCOPED_TRACE(was_typed ? "robot-typed" : "assignment-typed"); + const auto recipe = package->recipeHash(); + const auto before = package->layoutHash(); + T read = value; + EXPECT_FALSE(package->getData("missing", read)); + EXPECT_EQ(read, value); + EXPECT_FALSE(package->setData("missing", value)); + EXPECT_EQ(package->layoutHash(), before); + if (!was_typed) + { + EXPECT_THROW(package->getData("field", read), std::bad_variant_access); + EXPECT_EQ(read, value); + } + + ASSERT_TRUE(package->setData("field", T{})); + ASSERT_TRUE(package->isTyped()); + EXPECT_EQ(package->getDataType("field"), typed.getDataType("field")); + const auto established = package->layoutHash(); + if (!was_typed) + { + EXPECT_NE(established, before); + } + else + { + EXPECT_EQ(established, before); + } + EXPECT_EQ(established, typed.layoutHash()); + ASSERT_TRUE(package->setData("field", value)); + EXPECT_EQ(package->layoutHash(), established); + + using WrongType = std::conditional_t, uint8_t, bool>; + EXPECT_FALSE(package->setData("field", WrongType{ 1 })); + read = T{}; + ASSERT_TRUE(package->getData("field", read)); + EXPECT_EQ(read, value); + EXPECT_EQ(package->layoutHash(), established); + EXPECT_EQ(package->recipeHash(), recipe); + } + } + }, + entry.second); + } +} + +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_THROW(package.getData("timestamp", timestamp), std::bad_variant_access); + + 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; + EXPECT_THROW(package.getData("timestamp", timestamp), std::bad_variant_access); + + 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_THROW(package.getData("speed_slider_mask", speed_slider_mask), std::bad_variant_access); + 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_throws) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + + uint32_t timestamp = 0; + EXPECT_THROW(package.getData("timestamp", timestamp), std::bad_variant_access); +} + +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_when_protocol_version_changes) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + const uint64_t version_two = package.layoutHash(); + + package.setProtocolVersion(1); + + EXPECT_NE(package.layoutHash(), version_two); +} + +TEST(rtde_data_package, untyped_layout_hash_changes_when_protocol_version_changes) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + const uint64_t version_two = package.layoutHash(); + EXPECT_FALSE(package.isTyped()); + + package.setProtocolVersion(1); + + EXPECT_NE(package.layoutHash(), version_two); + EXPECT_EQ(package.emptyCopy().layoutHash(), package.layoutHash()); +} + +TEST(rtde_data_package, partially_typed_layout_hash_changes_when_protocol_version_changes) +{ + rtde_interface::DataPackage package({ "timestamp", "actual_q" }); + ASSERT_TRUE(package.setData("timestamp", 1.0)); + EXPECT_FALSE(package.isTyped()); + const uint64_t version_two = package.layoutHash(); + + package.setProtocolVersion(1); + + EXPECT_NE(package.layoutHash(), version_two); + EXPECT_EQ(package.emptyCopy().layoutHash(), package.layoutHash()); +} + +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); +} + +TEST(rtde_data_package, copy_from_fills_untyped_fields_with_zeros) +{ + 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, partial_copy_validates_all_fields_before_writing) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + ASSERT_TRUE(destination.setData("speed_slider_mask", uint32_t{ 7 })); + ASSERT_TRUE(destination.setData("speed_slider_fraction", 0.5)); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", uint64_t{ 1 })); + + EXPECT_FALSE(destination.copyFrom(source)); + uint32_t mask = 0; + double fraction = 0.0; + ASSERT_TRUE(destination.getData("speed_slider_mask", mask)); + ASSERT_TRUE(destination.getData("speed_slider_fraction", fraction)); + EXPECT_EQ(mask, 7u); + EXPECT_DOUBLE_EQ(fraction, 0.5); +} + +TEST(rtde_data_package, copying_unset_fields_preserves_destination_layout_and_source) +{ + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + destination.setRecipeID(7); + const auto layout = destination.layoutHash(); + ASSERT_TRUE(destination.setData("speed_slider_mask", uint32_t{ 1 })); + ASSERT_TRUE(destination.setData("speed_slider_fraction", 0.5)); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }, 1); + source.setRecipeID(3); + + ASSERT_TRUE(destination.copyFrom(source)); + EXPECT_EQ(destination.layoutHash(), layout); + EXPECT_FALSE(source.isTyped()); + EXPECT_FALSE(source.getDataType("speed_slider_mask").has_value()); + EXPECT_FALSE(source.getDataType("speed_slider_fraction").has_value()); + uint8_t buffer[64]{}; + ASSERT_EQ(destination.serializePackage(buffer), 16u); + EXPECT_EQ(buffer[3], 7u); + for (size_t i = 4; i < 16; ++i) + { + EXPECT_EQ(buffer[i], 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()); +} + +// memcpy of overlapping source and destination is undefined; a package copied onto itself has to +// succeed without taking that path. +TEST(rtde_data_package, copy_from_the_same_package_succeeds) +{ + auto package = typedPackage({ "timestamp", "actual_q" }, { "DOUBLE", "VECTOR6D" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + ASSERT_TRUE(package.setData("actual_q", vector6d_t{ 1, 2, 3, 4, 5, 6 })); + + ASSERT_TRUE(package.copyFrom(package)); + + double timestamp = 0.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + vector6d_t actual_q{}; + ASSERT_TRUE(package.getData("actual_q", actual_q)); + EXPECT_EQ(actual_q, vector6d_t({ 1, 2, 3, 4, 5, 6 })); +} + +TEST(rtde_data_package, reset_data_unknown_name_fails) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + EXPECT_FALSE(package.resetData("not_in_the_recipe")); +} + +TEST(rtde_data_package, reset_data_on_an_untyped_field_keeps_it_untyped) +{ + rtde_interface::DataPackage package({ "timestamp" }); + ASSERT_TRUE(package.resetData("timestamp")); + EXPECT_FALSE(package.getDataType("timestamp").has_value()); +} + +TEST(rtde_data_package, reset_data_on_a_typed_field_keeps_the_type) +{ + auto package = typedPackage({ "timestamp" }, { "DOUBLE" }); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + + ASSERT_TRUE(package.resetData("timestamp")); + + 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); +} + +// An empty recipe has no monostate fields, so isTyped() is vacuously true. Parse and serialize +// then write only the header (and the v2 recipe-id). +TEST(rtde_data_package, empty_recipe_package_is_typed_and_serializable) +{ + rtde_interface::DataPackage package(std::vector{}); + EXPECT_TRUE(package.isTyped()); + + package.setRecipeID(1); + uint8_t buffer[4096]; + const size_t size = package.serializePackage(buffer); + EXPECT_EQ(size, 4); + + comm::BinParser bp(buffer + 3, size - 3); + EXPECT_TRUE(package.parseWith(bp)); + EXPECT_TRUE(bp.empty()); +} + +TEST(rtde_data_package, bitset_get_data_fails_on_an_untyped_field) +{ + rtde_interface::DataPackage package({ "robot_status_bits" }); + std::bitset<4> bits; + EXPECT_THROW(package.getData("robot_status_bits", bits), std::bad_variant_access); +} + +TEST(rtde_data_package, bitset_get_data_missing_field_preserves_value) +{ + auto package = typedPackage({ "robot_status_bits" }, { "UINT32" }); + const std::bitset<8> expected(0xa5); + auto bits = expected; + + EXPECT_FALSE(package.getData("missing", bits)); + EXPECT_EQ(bits, expected); +} + +TEST(rtde_data_package, distinct_empty_packages_copy_and_init_empty_preserve_layout_and_serialization) +{ + for (const uint16_t version : { uint16_t{ 1 }, uint16_t{ 2 } }) + { + SCOPED_TRACE(version); + auto source = typedPackage({}, {}); + auto destination = typedPackage({}, {}); + source.setProtocolVersion(version); + destination.setProtocolVersion(version); + source.setRecipeID(3); + destination.setRecipeID(7); + const auto recipe = source.recipeHash(); + const auto layout = source.layoutHash(); + + // Distinct objects exercise the empty storage path, not the self-copy shortcut. + ASSERT_TRUE(destination.copyFrom(source)); + destination.initEmpty(); + auto copy = destination.emptyCopy(); + rtde_interface::DataPackage constructed(destination); + const std::vector> packages{ + { &source, 3 }, { &destination, 7 }, { ©, 0 }, { &constructed, 7 } + }; + for (const auto& entry : packages) + { + EXPECT_TRUE(entry.first->isTyped()); + EXPECT_EQ(entry.first->recipeHash(), recipe); + EXPECT_EQ(entry.first->layoutHash(), layout); + std::vector expected{ 0, static_cast(version == 2 ? 4 : 3), 0x55 }; + if (version == 2) + { + expected.push_back(entry.second); + } + uint8_t buffer[4]{}; + ASSERT_EQ(entry.first->serializePackage(buffer), expected.size()); + EXPECT_EQ(std::vector(buffer, buffer + expected.size()), expected); + } + } +} + +TEST(rtde_data_package, bitset_get_data_fails_when_the_underlying_type_is_wrong) +{ + auto package = typedPackage({ "robot_status_bits" }, { "UINT32" }); + ASSERT_TRUE(package.setData("robot_status_bits", static_cast(0x5))); + + std::bitset<8> bits; + EXPECT_THROW(package.getData("robot_status_bits", bits), std::bad_variant_access); +} + +TEST(rtde_data_package, to_string_covers_every_data_type) +{ + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::BOOL), "BOOL"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::UINT8), "UINT8"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::UINT32), "UINT32"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::UINT64), "UINT64"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::INT32), "INT32"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::DOUBLE), "DOUBLE"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::VECTOR3D), "VECTOR3D"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::VECTOR6D), "VECTOR6D"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::VECTOR6INT32), "VECTOR6INT32"); + EXPECT_EQ(rtde_interface::toString(rtde_interface::DataType::VECTOR6UINT32), "VECTOR6UINT32"); +} + +TEST(rtde_data_package, to_string_rejects_invalid_data_type) +{ + EXPECT_THROW(rtde_interface::toString(static_cast(0xff)), UrException); +} + +// emplace() keeps the first index when a recipe repeats a name. Looking a field up by name +// therefore addresses the first occurrence. +TEST(rtde_data_package, duplicate_field_name_keeps_the_first_index) +{ + rtde_interface::DataPackage package({ "timestamp", "timestamp" }); + package.setTypes({ "DOUBLE", "UINT32" }); + + ASSERT_TRUE(package.setData("timestamp", 42.0)); + EXPECT_EQ(package.getDataType("timestamp"), rtde_interface::DataType::DOUBLE); + double timestamp = 0.0; + ASSERT_TRUE(package.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + EXPECT_FALSE(package.setData("timestamp", static_cast(1))); +} + +TEST(rtde_data_package, get_data_type_reports_the_stored_type_after_set_data) +{ + rtde_interface::DataPackage package({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(package.setData("speed_slider_fraction", 0.5)); + + EXPECT_EQ(package.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); + EXPECT_FALSE(package.getDataType("speed_slider_mask").has_value()); + EXPECT_FALSE(package.getDataType("not_in_the_recipe").has_value()); + + package.setTypes({ "UINT32", "DOUBLE" }); + EXPECT_EQ(package.getDataType("speed_slider_mask"), rtde_interface::DataType::UINT32); + EXPECT_EQ(package.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); +} + +namespace +{ +class CapturingLogHandler : public LogHandler +{ +public: + void log(const char*, int, LogLevel loglevel, const char* log) override + { + if (loglevel == LogLevel::WARN) + { + warnings_.emplace_back(log); + } + } + + std::vector warnings_; +}; + +} // namespace + +TEST(rtde_data_package, copy_from_a_fully_typed_package_does_not_warn) +{ + auto handler = std::make_unique(); + auto* captured = handler.get(); + registerLogHandler(std::move(handler)); + setLogLevel(LogLevel::WARN); + + { + 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)); + } + + EXPECT_TRUE(captured->warnings_.empty()); + unregisterLogHandler(); + setLogLevel(LogLevel::ERROR); +} + +TEST(rtde_data_package, copy_from_a_partial_package_does_not_warn) +{ + auto handler = std::make_unique(); + auto* captured = handler.get(); + registerLogHandler(std::move(handler)); + setLogLevel(LogLevel::WARN); + + { + auto destination = typedPackage({ "speed_slider_mask", "speed_slider_fraction" }, { "UINT32", "DOUBLE" }); + rtde_interface::DataPackage source({ "speed_slider_mask", "speed_slider_fraction" }); + ASSERT_TRUE(source.setData("speed_slider_fraction", 0.5)); + EXPECT_TRUE(destination.copyFrom(source)); + } + + EXPECT_TRUE(captured->warnings_.empty()); + unregisterLogHandler(); + setLogLevel(LogLevel::ERROR); +} + +// The documented wire size of each protocol type, so a serializer that pads or truncates one of +// them fails here rather than only against a robot. +TEST(rtde_data_package, each_data_type_has_the_documented_wire_size) +{ + 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" }; + auto package = typedPackage(recipe, types); + package.setRecipeID(1); + + uint8_t buffer[4096]; + const size_t size = package.serializePackage(buffer); + const size_t header_and_recipe_id = 4; + const size_t payload = 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_and_recipe_id + payload); + EXPECT_EQ(sizeof(vector3d_t), 3 * sizeof(double)); + EXPECT_EQ(sizeof(vector6d_t), 6 * sizeof(double)); + EXPECT_EQ(sizeof(vector6int32_t), 6 * sizeof(int32_t)); + EXPECT_EQ(sizeof(vector6uint32_t), 6 * sizeof(uint32_t)); +} + 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..9846e545b 100644 --- a/tests/test_rtde_parser.cpp +++ b/tests/test_rtde_parser.cpp @@ -27,10 +27,14 @@ //---------------------------------------------------------------------- #include +#include +#include #include #include +#include "rtde_test_helpers.h" + using namespace urcl; TEST(rtde_parser, request_protocol_version) @@ -201,6 +205,7 @@ TEST(rtde_parser, data_package) std::unique_ptr product; std::vector recipe = { "timestamp", "target_speed_fraction" }; rtde_interface::RTDEParser parser(recipe); + parser.setExpectedDataPackage(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" })); parser.setProtocolVersion(2); parser.parse(bp, product); @@ -220,6 +225,108 @@ 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)); +} + +// DataPackage types are owned by the client and must be applied before parsing. +TEST(rtde_parser, untyped_pre_allocated_data_package_is_rejected) +{ + 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.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }).layoutHash()); + + std::unique_ptr product = std::make_unique(recipe); + const rtde_interface::RTDEPackage* package_address = product.get(); + + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); +} + +// A package with a different typed layout is rejected before payload parsing. +TEST(rtde_parser, wrongly_typed_pre_allocated_package_is_rejected) +{ + 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.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }).layoutHash()); + + 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(); + + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); +} + +TEST(rtde_parser, pre_allocated_package_with_a_different_recipe_is_rejected) +{ + 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.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }).layoutHash()); + + std::unique_ptr product = + std::make_unique(std::vector{ "foo", "bar" }); + const rtde_interface::RTDEPackage* package_address = product.get(); + + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), package_address); +} + +TEST(rtde_parser, typed_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); + auto expected_package = test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }); + expected_package.setProtocolVersion(1); + parser.setExpectedLayoutHash(expected_package.layoutHash()); + parser.setProtocolVersion(1); + + auto package = std::make_unique(recipe); + package->setTypes({ "DOUBLE", "DOUBLE" }); + std::unique_ptr product = std::move(package); + + 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 @@ -259,6 +366,28 @@ TEST(rtde_parser, test_buffer_too_long) EXPECT_FALSE(parser.parse(bp, product)); } +// The single-pointer parse consumes one package. Two concatenated data packages therefore leave +// leftover bytes, which the parser reports as a failure rather than silently dropping the second. +TEST(rtde_parser, two_data_packages_in_one_buffer_leave_leftover_bytes) +{ + unsigned char first[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xd0, 0x07, 0x0d, 0x2f, 0x1a, + 0x9f, 0xbe, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char second[] = { 0x00, 0x14, 0x55, 0x01, 0x40, 0xc3, 0x88, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char raw_data[sizeof(first) + sizeof(second)]; + std::memcpy(raw_data, first, sizeof(first)); + std::memcpy(raw_data + sizeof(first), second, sizeof(second)); + comm::BinParser bp(raw_data, sizeof(raw_data)); + + std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(recipe); + parser.setExpectedDataPackage(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" })); + parser.setProtocolVersion(2); + + std::unique_ptr product; + EXPECT_FALSE(parser.parse(bp, product)); +} + TEST(rtde_parser, test_deprecated_parse_method) { // received data package, @@ -266,17 +395,18 @@ 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.setExpectedDataPackage(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" })); parser.setProtocolVersion(2); std::vector> products; { comm::BinParser bp(raw_data, sizeof(raw_data)); URCL_SILENCE_DEPRECATED_BEGIN - parser.parse(bp, products); + ASSERT_TRUE(parser.parse(bp, products)); URCL_SILENCE_DEPRECATED_END } - EXPECT_EQ(products.size(), 1); + ASSERT_EQ(products.size(), 1); if (rtde_interface::DataPackage* data = dynamic_cast(products[0].get())) { @@ -294,6 +424,656 @@ TEST(rtde_parser, test_deprecated_parse_method) } } +TEST(rtde_parser, deprecated_parse_without_registration_rejects_typed_package) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto package = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + package.setProtocolVersion(2); + ASSERT_TRUE(package.setData("timestamp", 42.0)); + std::vector> products; + products.push_back(std::make_unique(package)); + const auto* original = products.back().get(); + + comm::BinParser bp(raw_data, sizeof(raw_data)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + ASSERT_EQ(products.size(), 1u); + EXPECT_EQ(products.back().get(), original); + auto* data = dynamic_cast(products.back().get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +TEST(rtde_parser, deprecated_hash_only_parse_rejects_empty_vector) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + std::vector> products; + + comm::BinParser bp(raw_data, sizeof(raw_data)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + EXPECT_TRUE(products.empty()); +} + +TEST(rtde_parser, deprecated_hash_only_parse_rejects_null_or_non_data_last_entry) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + for (const bool null_last_entry : { false, true }) + { + SCOPED_TRACE(null_last_entry); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + ASSERT_TRUE(expected.setData("timestamp", 42.0)); + std::vector> products; + // A matching earlier entry must not be used in place of an invalid last entry. + products.push_back(std::make_unique(expected)); + const auto* first = products.front().get(); + if (null_last_entry) + { + products.push_back(nullptr); + } + else + { + auto control = std::make_unique(); + control->accepted_ = true; + products.push_back(std::move(control)); + } + const auto* last = products.back().get(); + + comm::BinParser bp(raw_data, sizeof(raw_data)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + ASSERT_EQ(products.size(), 2u); + EXPECT_EQ(products.front().get(), first); + EXPECT_EQ(products.back().get(), last); + if (!null_last_entry) + { + auto* start = dynamic_cast(products.back().get()); + ASSERT_NE(start, nullptr); + EXPECT_TRUE(start->accepted_); + } + auto* data = dynamic_cast(products.front().get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + } +} + +TEST(rtde_parser, deprecated_hash_only_parse_reuses_last_package_repeatedly) +{ + // Complete v2 packets: (timestamp, target_speed_fraction) = (1, 0.5), then (2, 1). + unsigned char packets[][20] = { { 0x00, 0x14, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, + { 0x00, 0x14, 0x55, 0x01, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 } }; + const std::vector recipe = { "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(recipe); + parser.setProtocolVersion(2); + auto expected = test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }); + expected.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + ASSERT_TRUE(expected.setData("timestamp", 42.0)); + ASSERT_TRUE(expected.setData("target_speed_fraction", 0.25)); + std::vector> products; + products.push_back(std::make_unique(expected)); + products.push_back(std::make_unique(expected)); + const auto* first = products.front().get(); + const auto* last = products.back().get(); + + for (size_t i = 0; i < 2; ++i) + { + SCOPED_TRACE(i); + comm::BinParser bp(packets[i], sizeof(packets[i])); + URCL_SILENCE_DEPRECATED_BEGIN + ASSERT_TRUE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + EXPECT_TRUE(bp.empty()); + ASSERT_EQ(products.size(), 2u); + EXPECT_EQ(products.front().get(), first); + EXPECT_EQ(products.back().get(), last); + for (size_t j = 0; j < products.size(); ++j) + { + auto* data = dynamic_cast(products[j].get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->layoutHash(), expected.layoutHash()); + 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, j == 0 ? 42.0 : (i == 0 ? 1.0 : 2.0)); + EXPECT_DOUBLE_EQ(target_speed_fraction, j == 0 ? 0.25 : (i == 0 ? 0.5 : 1.0)); + } + } +} + +TEST(rtde_parser, deprecated_hash_only_parse_repairs_protocol_only_mismatch) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + for (const uint16_t version : std::initializer_list{ 1, 2 }) + { + SCOPED_TRACE(version); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(version); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(version); + parser.setExpectedLayoutHash(expected.layoutHash()); + auto package = std::make_unique(expected); + package->setProtocolVersion(version == 1 ? 2 : 1); + ASSERT_TRUE(package->setData("timestamp", 42.0)); + ASSERT_NE(package->layoutHash(), expected.layoutHash()); + std::vector> products; + products.push_back(std::move(package)); + const auto* original = products.back().get(); + + comm::BinParser bp(version == 1 ? version1 : version2, version == 1 ? sizeof(version1) : sizeof(version2)); + URCL_SILENCE_DEPRECATED_BEGIN + ASSERT_TRUE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + EXPECT_TRUE(bp.empty()); + ASSERT_EQ(products.size(), 1u); + EXPECT_EQ(products.back().get(), original); + auto* data = dynamic_cast(products.back().get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->layoutHash(), expected.layoutHash()); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); + } +} + +TEST(rtde_parser, deprecated_hash_only_parse_rejects_wrong_recipe_after_protocol_resync) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + for (const uint16_t version : std::initializer_list{ 1, 2 }) + { + SCOPED_TRACE(version); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(version); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(version); + parser.setExpectedLayoutHash(expected.layoutHash()); + auto wrong_recipe = test::typedPackage({ "target_speed_fraction" }, { "DOUBLE" }); + wrong_recipe.setProtocolVersion(version); + ASSERT_NE(wrong_recipe.layoutHash(), expected.layoutHash()); + auto package = std::make_unique(wrong_recipe); + package->setProtocolVersion(version == 1 ? 2 : 1); + ASSERT_NE(package->layoutHash(), wrong_recipe.layoutHash()); + ASSERT_TRUE(package->setData("target_speed_fraction", 0.25)); + std::vector> products; + products.push_back(std::move(package)); + const auto* original = products.back().get(); + + comm::BinParser bp(version == 1 ? version1 : version2, version == 1 ? sizeof(version1) : sizeof(version2)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + ASSERT_EQ(products.size(), 1u); + EXPECT_EQ(products.back().get(), original); + auto* data = dynamic_cast(products.back().get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->layoutHash(), wrong_recipe.layoutHash()); + EXPECT_NE(data->layoutHash(), expected.layoutHash()); + double target_speed_fraction = 0.0; + ASSERT_TRUE(data->getData("target_speed_fraction", target_speed_fraction)); + EXPECT_DOUBLE_EQ(target_speed_fraction, 0.25); + } +} + +TEST(rtde_parser, deprecated_hash_only_parse_rejects_same_width_wrong_type_after_protocol_resync) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + for (const uint16_t version : std::initializer_list{ 1, 2 }) + { + SCOPED_TRACE(version); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(version); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(version); + parser.setExpectedLayoutHash(expected.layoutHash()); + // UINT64 and DOUBLE both occupy eight wire bytes: size alone cannot detect the mismatch. + auto wrong_type = test::typedPackage({ "timestamp" }, { "UINT64" }); + wrong_type.setProtocolVersion(version); + ASSERT_NE(wrong_type.layoutHash(), expected.layoutHash()); + auto package = std::make_unique(wrong_type); + package->setProtocolVersion(version == 1 ? 2 : 1); + ASSERT_NE(package->layoutHash(), wrong_type.layoutHash()); + ASSERT_TRUE(package->setData("timestamp", uint64_t{ 42 })); + std::vector> products; + products.push_back(std::move(package)); + const auto* original = products.back().get(); + + comm::BinParser bp(version == 1 ? version1 : version2, version == 1 ? sizeof(version1) : sizeof(version2)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + ASSERT_EQ(products.size(), 1u); + EXPECT_EQ(products.back().get(), original); + auto* data = dynamic_cast(products.back().get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->layoutHash(), wrong_type.layoutHash()); + EXPECT_NE(data->layoutHash(), expected.layoutHash()); + uint64_t timestamp = 0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_EQ(timestamp, uint64_t{ 42 }); + } +} + +TEST(rtde_parser, hash_only_parse_rejects_non_data_pointer) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + auto control = std::make_unique(); + control->accepted_ = true; + std::unique_ptr product = std::move(control); + const auto* original = product.get(); + + comm::BinParser bp(raw_data, sizeof(raw_data)); + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), original); + auto* start = dynamic_cast(product.get()); + ASSERT_NE(start, nullptr); + EXPECT_TRUE(start->accepted_); +} + +TEST(rtde_parser, typed_template_replaces_a_non_data_package) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + comm::BinParser bp(raw_data, sizeof(raw_data)); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(test::typedPackage({ "timestamp" }, { "DOUBLE" })); + std::unique_ptr product = std::make_unique(); + + ASSERT_TRUE(parser.parse(bp, product)); + auto* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); +} + +TEST(rtde_parser, typed_template_follows_protocol_changes) +{ + unsigned char raw_data[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + for (const bool set_version_first : { false, true }) + { + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + if (set_version_first) + { + parser.setProtocolVersion(1); + } + parser.setExpectedDataPackage(expected); + parser.setProtocolVersion(1); + comm::BinParser bp(raw_data, sizeof(raw_data)); + std::unique_ptr product; + ASSERT_TRUE(parser.parse(bp, product)); + auto* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); + } +} + +TEST(rtde_parser, reused_package_follows_protocol_changes_in_place) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setExpectedDataPackage(expected); + std::unique_ptr product = std::make_unique(expected); + auto* original = product.get(); + + for (const uint16_t version : std::initializer_list{ 1, 2, 1 }) + { + parser.setProtocolVersion(version); + comm::BinParser bp(version == 1 ? version1 : version2, version == 1 ? sizeof(version1) : sizeof(version2)); + ASSERT_TRUE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), original); + auto* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + expected.setProtocolVersion(version); + EXPECT_EQ(data->layoutHash(), expected.layoutHash()); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); + } +} + +TEST(rtde_parser, hash_only_registration_is_invalidated_when_protocol_changes) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + ASSERT_TRUE(expected.setData("timestamp", 42.0)); + const uint64_t original_hash = expected.layoutHash(); + + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedLayoutHash(original_hash); + parser.setProtocolVersion(1); + + auto dest = expected; + { + comm::BinParser bp(version2, sizeof(version2)); + EXPECT_FALSE(parser.parseDataPackage(bp, dest)); + } + EXPECT_EQ(dest.layoutHash(), original_hash); + double timestamp = 0.0; + ASSERT_TRUE(dest.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + + { + comm::BinParser bp(version1, sizeof(version1)); + EXPECT_FALSE(parser.parseDataPackage(bp, dest)); + } + EXPECT_EQ(dest.layoutHash(), original_hash); + timestamp = 0.0; + ASSERT_TRUE(dest.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); + + std::unique_ptr product = std::make_unique(expected); + const auto* original = product.get(); + comm::BinParser bp(version2, sizeof(version2)); + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product.get(), original); + auto* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + EXPECT_EQ(data->layoutHash(), original_hash); + timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 42.0); +} + +TEST(rtde_parser, hash_only_same_protocol_version_keeps_registration) +{ + unsigned char version2[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + parser.setProtocolVersion(2); + + auto dest = expected; + comm::BinParser bp(version2, sizeof(version2)); + ASSERT_TRUE(parser.parseDataPackage(bp, dest)); + double timestamp = 0.0; + ASSERT_TRUE(dest.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); +} + +TEST(rtde_parser, hash_only_parse_succeeds_after_re_registering_new_protocol) +{ + unsigned char version1[] = { 0x00, 0x0b, 0x55, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + expected.setProtocolVersion(2); + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedLayoutHash(expected.layoutHash()); + parser.setProtocolVersion(1); + + expected.setProtocolVersion(1); + parser.setExpectedLayoutHash(expected.layoutHash()); + auto dest = expected; + comm::BinParser bp(version1, sizeof(version1)); + ASSERT_TRUE(parser.parseDataPackage(bp, dest)); + double timestamp = 0.0; + ASSERT_TRUE(dest.getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); +} + +TEST(rtde_parser, reused_pointer_handles_data_control_data_sequence) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + unsigned char control[] = { 0x00, 0x04, 0x53, 0x01 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(test::typedPackage({ "timestamp" }, { "DOUBLE" })); + std::unique_ptr product; + + for (int i = 0; i < 2; ++i) + { + comm::BinParser bp(raw_data, sizeof(raw_data)); + ASSERT_TRUE(parser.parse(bp, product)); + auto* data = dynamic_cast(product.get()); + ASSERT_NE(data, nullptr); + double timestamp = 0.0; + ASSERT_TRUE(data->getData("timestamp", timestamp)); + EXPECT_DOUBLE_EQ(timestamp, 1.0); + + comm::BinParser control_bp(control, sizeof(control)); + ASSERT_TRUE(parser.parse(control_bp, product)); + auto* start = dynamic_cast(product.get()); + ASSERT_NE(start, nullptr); + EXPECT_TRUE(start->accepted_); + } +} + +TEST(rtde_parser, hash_registration_clears_the_allocation_template) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto expected = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + parser.setExpectedDataPackage(expected); + parser.setExpectedLayoutHash(expected.layoutHash()); + std::unique_ptr product; + comm::BinParser bp(raw_data, sizeof(raw_data)); + EXPECT_FALSE(parser.parse(bp, product)); + EXPECT_EQ(product, nullptr); +} + +TEST(rtde_parser, untyped_template_is_rejected_without_changing_registration) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(test::typedPackage({ "timestamp" }, { "DOUBLE" })); + EXPECT_THROW(parser.setExpectedDataPackage(rtde_interface::DataPackage({ "timestamp" })), UrException); + comm::BinParser bp(raw_data, sizeof(raw_data)); + std::unique_ptr product; + EXPECT_TRUE(parser.parse(bp, product)); +} + +TEST(rtde_parser, foreign_typed_templates_leave_registration_unchanged) +{ + const std::vector expected_recipe{ "timestamp", "target_speed_fraction" }; + rtde_interface::RTDEParser parser(expected_recipe); + parser.setProtocolVersion(2); + auto expected = test::typedPackage(expected_recipe, { "DOUBLE", "DOUBLE" }); + parser.setExpectedDataPackage(expected); + const std::vector> recipes{ { "actual_q" }, + { "target_speed_fraction", "timestamp" }, + { "timestamp", "other_double" } }; + for (const auto& recipe : recipes) + { + const std::vector types = + recipe.size() == 1 ? std::vector{ "VECTOR6D" } : std::vector{ "DOUBLE", "DOUBLE" }; + EXPECT_THROW(parser.setExpectedDataPackage(test::typedPackage(recipe, types)), UrException); + uint8_t bytes[128]; + const auto size = expected.serializePackage(bytes); + comm::BinParser bp(bytes, size); + std::unique_ptr result; + ASSERT_TRUE(parser.parse(bp, result)); + EXPECT_TRUE(dynamic_cast(*result).hasRecipe(expected_recipe)); + } +} + +TEST(rtde_parser, borrowed_data_parse_rejects_other_frames_and_recovers) +{ + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto output = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + ASSERT_TRUE(output.setData("timestamp", 42.0)); + parser.setExpectedDataPackage(output); + std::vector> frames{ + { 0x00, 0x04, 0x53, 0x01 }, // START acknowledgement + { 0x00, 0x07, 0x4d, 0x01, 'x', 0x00, 0x01 }, // Valid text + { 0x00, 0x04, 0x4d, 0xff }, // Truncated text + { 0x00, 0x04, 0x55, 0x01 }, // Truncated data + { 0x00 }, // Truncated header + }; + for (auto& frame : frames) + { + comm::BinParser bp(frame.data(), frame.size()); + EXPECT_FALSE(parser.parseDataPackage(bp, output)); + double timestamp = 0; + ASSERT_TRUE(output.getData("timestamp", timestamp)); + EXPECT_EQ(timestamp, 42.0); + } + uint8_t bytes[64]; + const auto size = output.serializePackage(bytes); + comm::BinParser bp(bytes, size); + EXPECT_TRUE(parser.parseDataPackage(bp, output)); +} + +TEST(rtde_parser, borrowed_data_parse_requires_known_matching_layout) +{ + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + auto output = test::typedPackage({ "timestamp" }, { "DOUBLE" }); + uint8_t bytes[64]; + const auto size = output.serializePackage(bytes); + comm::BinParser unknown(bytes, size); + EXPECT_FALSE(parser.parseDataPackage(unknown, output)); + parser.setExpectedDataPackage(output); + auto foreign = test::typedPackage({ "other" }, { "DOUBLE" }); + comm::BinParser mismatch(bytes, size); + EXPECT_FALSE(parser.parseDataPackage(mismatch, foreign)); + bytes[size] = 0; + comm::BinParser trailing(bytes, size + 1); + EXPECT_FALSE(parser.parseDataPackage(trailing, output)); +} + +TEST(rtde_parser, deprecated_parse_appends_only_complete_packages) +{ + unsigned char raw_data[] = { 0x00, 0x0c, 0x55, 0x01, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff }; + rtde_interface::RTDEParser parser({ "timestamp" }); + parser.setProtocolVersion(2); + parser.setExpectedDataPackage(test::typedPackage({ "timestamp" }, { "DOUBLE" })); + std::vector> products; + for (size_t i = 0; i < 2; ++i) + { + comm::BinParser bp(raw_data, sizeof(raw_data) - 1); + URCL_SILENCE_DEPRECATED_BEGIN + ASSERT_TRUE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + ASSERT_EQ(products.size(), i + 1); + } + EXPECT_NE(products[0].get(), products[1].get()); + comm::BinParser bp(raw_data, sizeof(raw_data)); + URCL_SILENCE_DEPRECATED_BEGIN + EXPECT_FALSE(parser.parse(bp, products)); + URCL_SILENCE_DEPRECATED_END + EXPECT_EQ(products.size(), 2u); +} + +// 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.setProtocolVersion(2); + parser.setExpectedLayoutHash(test::typedPackage(recipe, { "DOUBLE", "DOUBLE" }).layoutHash()); + + auto package = std::make_unique(recipe); + package->setTypes({ "DOUBLE", "DOUBLE" }); + std::unique_ptr product = std::move(package); + { + 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..8008f4610 100644 --- a/tests/test_rtde_writer.cpp +++ b/tests/test_rtde_writer.cpp @@ -30,10 +30,16 @@ #include #include +#include +#include +#include #include #include #include +#include + +#include "rtde_test_helpers.h" using namespace urcl; @@ -54,6 +60,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); } @@ -68,24 +75,36 @@ class RTDEWriterTest : public ::testing::Test void messageCallback([[maybe_unused]] const socket_t filedescriptor, char* buffer, int nbytesrecv) { std::lock_guard lk(message_mutex_); - uint8_t* buf = reinterpret_cast(buffer); - comm::BinParser bp(buf, nbytesrecv); - // These might be needed in the test - uint16_t size; - uint8_t type, recipe_id; - bp.parse(size); - bp.parse(type); - bp.parse(recipe_id); - parseMessage(bp); - message_cv_.notify_one(); - message_callback_ = true; + const auto* bytes = reinterpret_cast(buffer); + received_bytes_.insert(received_bytes_.end(), bytes, bytes + nbytesrecv); + // TCP callbacks may split or combine frames. Only acknowledge a fully parsed package. + while (received_bytes_.size() >= 3) + { + const size_t frame_size = (static_cast(received_bytes_[0]) << 8) | received_bytes_[1]; + ASSERT_GE(frame_size, 4u); + if (received_bytes_.size() < frame_size) + { + return; + } + comm::BinParser bp(received_bytes_.data(), frame_size); + uint16_t size; + uint8_t type, recipe_id; + bp.parse(size); + bp.parse(type); + bp.parse(recipe_id); + EXPECT_EQ(type, 0x55); + parseMessage(bp); + EXPECT_TRUE(bp.empty()); + received_bytes_.erase(received_bytes_.begin(), received_bytes_.begin() + frame_size); + message_callback_ = true; + message_cv_.notify_one(); + } } bool waitForMessageCallback(int milliseconds = 100) { std::unique_lock lk(message_mutex_); - if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || - message_callback_ == true) + if (message_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds), [this] { return message_callback_; })) { message_callback_ = false; return true; @@ -124,13 +143,17 @@ 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_; std::unordered_map parsed_data_; private: - void parseMessage(comm::BinParser bp) + void parseMessage(comm::BinParser& bp) { for (auto& item : input_recipe_) { @@ -146,6 +169,7 @@ class RTDEWriterTest : public ::testing::Test std::condition_variable message_cv_; std::mutex message_mutex_; bool message_callback_ = false; + std::vector received_bytes_; std::unordered_map input_map_types_ = { { "speed_slider_mask", uint32_t() }, @@ -191,6 +215,43 @@ 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, partial_packages_send_zeros_for_unset_fields) +{ + ASSERT_TRUE(writer_->sendSpeedSlider(0.5)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + rtde_interface::DataPackage package(input_recipe_); + ASSERT_TRUE(package.setData("input_double_register_25", 0.75)); + ASSERT_TRUE(writer_->sendPackage(package)); + ASSERT_TRUE(waitForMessageCallback(1000)); + + EXPECT_DOUBLE_EQ(std::get(parsed_data_.at("input_double_register_25")), 0.75); + EXPECT_DOUBLE_EQ(std::get(parsed_data_.at("speed_slider_fraction")), 0.0); + EXPECT_EQ(std::get(parsed_data_.at("speed_slider_mask")), 0u); + EXPECT_EQ(std::get(parsed_data_.at("standard_digital_output_mask")), 0u); + EXPECT_FALSE(std::get(parsed_data_.at("input_bit_register_65"))); + EXPECT_EQ(std::get(parsed_data_.at("input_int_register_25")), 0); + EXPECT_EQ(std::get(parsed_data_.at("external_force_torque")), vector6d_t{}); + EXPECT_FALSE(package.isTyped()); +} + TEST_F(RTDEWriterTest, send_standard_digital_output) { uint8_t expected_standard_digital_output_mask = 4; @@ -520,6 +581,265 @@ 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); +} + +TEST_F(RTDEWriterTest, create_data_package_while_running_without_types_throws) +{ + writer_->stop(); + rtde_interface::RTDEWriter writer(stream_.get(), input_recipe_); + ASSERT_NO_THROW(writer.init(1)); + + // Isolate the running-but-untyped guard without queuing an unserializable send. + EXPECT_THROW(writer.createDataPackage(), UrException); + writer.stop(); + EXPECT_FALSE(waitForMessageCallback(100)); + EXPECT_TRUE(parsed_data_.empty()); +} + +TEST_F(RTDEWriterTest, invalid_recipe_types_while_stopped_allow_valid_setup_and_init) +{ + writer_->stop(); + writer_->setInputRecipe(input_recipe_); + auto too_few = input_recipe_types_; + too_few.pop_back(); + auto too_many = input_recipe_types_; + too_many.push_back("DOUBLE"); + auto unknown = input_recipe_types_; + unknown.back() = "NOT_A_TYPE"; + + EXPECT_THROW(writer_->setRecipeTypes(too_few), UrException); + EXPECT_THROW(writer_->setRecipeTypes(too_many), UrException); + EXPECT_THROW(writer_->setRecipeTypes(unknown), UrException); + ASSERT_NO_THROW(writer_->setRecipeTypes(input_recipe_types_)); + ASSERT_NO_THROW(writer_->init(1)); + + auto package = writer_->createDataPackage(); + EXPECT_TRUE(package.isTyped()); + const auto expected = urcl::test::typedPackage(input_recipe_, input_recipe_types_); + EXPECT_EQ(package.layoutHash(), expected.layoutHash()); + for (const auto& field : input_recipe_) + { + EXPECT_EQ(package.getDataType(field), expected.getDataType(field)) << field; + } + writer_->stop(); +} + +TEST_F(RTDEWriterTest, typing_and_retyping_initialize_both_send_buffers) +{ + // Compare parsed fields, not TCP callbacks or raw receive chunks. Each send below is + // acknowledged before another is queued, since the writer may coalesce pending writes. + const auto expect_package = [this](const rtde_interface::DataPackage& expected) { + ASSERT_EQ(parsed_data_.size(), input_recipe_.size()); + for (const auto& field : input_recipe_) + { + ASSERT_TRUE(dataFieldExist(field)); + std::visit( + [&](const auto& received) { + std::decay_t value{}; + ASSERT_TRUE(expected.getData(field, value)); + EXPECT_EQ(received, value) << field; + }, + parsed_data_.at(field)); + } + }; + const std::vector byte_masks{ "standard_digital_output_mask", "configurable_digital_output_mask", + "tool_digital_output_mask", "standard_analog_output_mask" }; + std::vector packages{ writer_->createDataPackage(), writer_->createDataPackage() }; + for (size_t i = 0; i < packages.size(); ++i) + { + SCOPED_TRACE(i); + auto& package = packages[i]; + const double value = i == 0 ? 0.25 : 0.75; + ASSERT_TRUE(package.setData("speed_slider_mask", uint32_t{ 1 })); + for (const auto& mask : byte_masks) + { + ASSERT_TRUE(package.setData(mask, uint8_t{ 1 })); + } + ASSERT_TRUE(package.setData("speed_slider_fraction", value)); + ASSERT_TRUE(package.setData("standard_digital_output", uint8_t{ 0xa5 })); + ASSERT_TRUE(package.setData("configurable_digital_output", uint8_t{ 0x5a })); + ASSERT_TRUE(package.setData("tool_digital_output", uint8_t{ 3 })); + ASSERT_TRUE(package.setData("standard_analog_output_type", uint8_t{ 3 })); + ASSERT_TRUE(package.setData("standard_analog_output_0", value)); + ASSERT_TRUE(package.setData("standard_analog_output_1", value)); + ASSERT_TRUE(package.setData("input_bit_register_65", true)); + ASSERT_TRUE(package.setData("input_int_register_25", int32_t{ -42 })); + ASSERT_TRUE(package.setData("input_double_register_25", value)); + ASSERT_TRUE(package.setData("external_force_torque", vector6d_t{ value, -2, 3, -4, 5, -6 })); + ASSERT_TRUE(writer_->sendPackage(package)); + ASSERT_TRUE(waitForMessageCallback(1000)); + ASSERT_NO_FATAL_FAILURE(expect_package(package)); + } + + // Reuse both buffers without overwriting the whole package. Masks must have been reset, + // while each buffer's distinct non-mask values remain intact. + for (size_t i = 0; i < packages.size(); ++i) + { + SCOPED_TRACE(i); + auto& expected = packages[i]; + ASSERT_TRUE(expected.resetData("speed_slider_mask")); + for (const auto& mask : byte_masks) + { + ASSERT_TRUE(expected.resetData(mask)); + } + const int32_t value = i == 0 ? -123 : 456; + ASSERT_TRUE(expected.setData("input_int_register_25", value)); + ASSERT_TRUE(writer_->sendInputIntRegister(25, value)); + ASSERT_TRUE(waitForMessageCallback(1000)); + ASSERT_NO_FATAL_FAILURE(expect_package(expected)); + } + + writer_->stop(); + // Change the layout while stopped, then restore the fixture's wire types. No sends occur + // under the temporary recipe types, which the fixture is not configured to parse. + writer_->setRecipeTypes(std::vector(input_recipe_.size(), "UINT8")); + writer_->setRecipeTypes(input_recipe_types_); + writer_->init(1); + for (const double value : { -1.25, 2.5 }) + { + SCOPED_TRACE(value); + auto expected = urcl::test::typedPackage(input_recipe_, input_recipe_types_); + ASSERT_TRUE(expected.setData("input_double_register_25", value)); + ASSERT_TRUE(writer_->sendInputDoubleRegister(25, value)); + ASSERT_TRUE(waitForMessageCallback(1000)); + // Both buffers must have their old data and masks cleared, not just the current store. + ASSERT_NO_FATAL_FAILURE(expect_package(expected)); + } +} + +// 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); @@ -530,6 +850,95 @@ TEST_F(RTDEWriterTest, set_recipe_while_running_throws) EXPECT_THROW(writer_->setInputRecipe(input_recipe_), UrException); } +TEST_F(RTDEWriterTest, setup_mutators_throw_while_running_and_work_after_stop) +{ + EXPECT_THROW(writer_->setRecipeTypes(input_recipe_types_), UrException); + EXPECT_THROW(writer_->setProtocolVersion(1), UrException); + + writer_->stop(); + + EXPECT_NO_THROW(writer_->setRecipeTypes(input_recipe_types_)); + EXPECT_NO_THROW(writer_->setProtocolVersion(1)); +} + +TEST_F(RTDEWriterTest, create_data_package_after_stop_throws) +{ + writer_->stop(); + EXPECT_THROW(writer_->createDataPackage(), UrException); +} + +TEST_F(RTDEWriterTest, set_input_recipe_after_stop_succeeds) +{ + writer_->stop(); + + const std::vector new_recipe{ "speed_slider_mask", "speed_slider_fraction" }; + const std::vector new_types{ "UINT32", "DOUBLE" }; + EXPECT_NO_THROW(writer_->setInputRecipe(new_recipe)); + EXPECT_NO_THROW(writer_->setRecipeTypes(new_types)); + EXPECT_NO_THROW(writer_->init(1)); + + rtde_interface::DataPackage data_package = writer_->createDataPackage(); + EXPECT_TRUE(data_package.isTyped()); + EXPECT_EQ(data_package.getDataType("speed_slider_fraction"), rtde_interface::DataType::DOUBLE); + ASSERT_TRUE(data_package.setData("speed_slider_fraction", 0.4)); + EXPECT_FALSE(data_package.getDataType("standard_digital_output").has_value()); +} + +TEST(rtde_writer, serializes_protocol_version_1_without_a_recipe_id) +{ + comm::TCPServer server(60014); + std::mutex mutex; + std::condition_variable cv; + bool received = false; + std::vector payload; + server.setMessageCallback([&](const socket_t, char* buffer, int nbytesrecv) { + std::lock_guard lock(mutex); + const auto* bytes = reinterpret_cast(buffer); + payload.insert(payload.end(), bytes, bytes + nbytesrecv); + // TCP may split one frame over several callbacks, so the bytes are only reported once the + // length in the two-byte header says the frame is complete. + if (payload.size() >= sizeof(rtde_interface::PackageHeader::_package_size_type)) + { + const size_t frame_size = (static_cast(payload[0]) << 8) | payload[1]; + if (payload.size() >= frame_size) + { + received = true; + cv.notify_one(); + } + } + }); + server.start(); + + comm::URStream stream("127.0.0.1", 60014); + ASSERT_TRUE(stream.connect()); + + const std::vector recipe{ "speed_slider_mask" }; + const std::vector types{ "UINT32" }; + rtde_interface::RTDEWriter writer(&stream, recipe); + writer.setRecipeTypes(types); + writer.setProtocolVersion(1); + writer.init(1); + + rtde_interface::DataPackage package = writer.createDataPackage(); + ASSERT_TRUE(package.setData("speed_slider_mask", static_cast(0x12345678))); + ASSERT_TRUE(writer.sendPackage(package)); + + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, std::chrono::seconds(1), [&] { return received; })); + } + writer.stop(); + + ASSERT_GE(payload.size(), 7u); + EXPECT_EQ(payload[0], 0x00); + EXPECT_EQ(payload[1], 0x07); + EXPECT_EQ(payload[2], 0x55); + EXPECT_EQ(payload[3], 0x12); + EXPECT_EQ(payload[4], 0x34); + EXPECT_EQ(payload[5], 0x56); + EXPECT_EQ(payload[6], 0x78); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); diff --git a/tests/test_tcp_server.cpp b/tests/test_tcp_server.cpp index 634d51b4d..0714e35e1 100644 --- a/tests/test_tcp_server.cpp +++ b/tests/test_tcp_server.cpp @@ -703,6 +703,27 @@ TEST_F(TCPServerTest, message_not_lost_when_client_connects_concurrently) } } +// The previous implementation read `written` before assigning it, so the fake RTDE server +// silently sent nothing. writeUnchecked must report the number of bytes it actually wrote. +TEST_F(TCPServerTest, write_unchecked_reports_bytes_written) +{ + TestableTcpServer server(port_); + server.start(); + + Client client(port_); + ASSERT_TRUE(server.waitForConnectionCallback()); + + const std::vector fds = server.getClientFDs(); + ASSERT_EQ(fds.size(), 1u); + + const uint8_t message[] = { 'h', 'e', 'l', 'l', 'o', '\n' }; + size_t written = 0xdeadbeef; + ASSERT_TRUE(server.writeUnchecked(fds.front(), message, sizeof(message), written)); + EXPECT_EQ(written, sizeof(message)); + + EXPECT_EQ(client.recv(), "hello\n"); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);