Take RTDE data types from the robot's setup acknowledgement

doc/architecture/rtde_client.rst

@@ -36,10 +36,20 @@ the :ref:`rtde_client_example` for an example of the blocking read method.
    {
      if (my_client.getDataPackage(data_pkg, READ_TIMEOUT))
      {
-       std::cout << data_pkg->toString() << std::endl;
+       std::cout << data_pkg.toString() << std::endl;
      }
    }
 
+.. note::
+   Constructing the ``DataPackage`` is where its memory is allocated, so create it before entering
+   your control loop and reuse it: ``getDataPackage()`` and ``getDataPackageBlocking()`` don't
+   allocate.
+
+   A recipe only lists field names. The data types belonging to them are reported by the robot when
+   it acknowledges the recipe, and the first read applies them to your ``DataPackage``, which costs
+   no memory. Until that has happened ``getData()`` on the package fails. See `Field data types`_
+   for how to ask a package what type it gave a field.
+
 Upon construction, two recipe files have to be given, one for the RTDE inputs, one for the RTDE
 outputs. Please refer to the `RTDE
 guide <https://www.universal-robots.com/articles/ur-articles/real-time-data-exchange-rtde-guide/>`_
@@ -69,6 +79,56 @@ After calling ``my_client.start()``, data can be read from the
 Remember that, when not using a background thread, data has to be polled regularly, as the robot
 will shutdown RTDE communication if the receiving side doesn't empty its buffer.
 
+Both methods deliver their data into a ``DataPackage`` that the caller owns, which is what keeps the
+read path free of memory allocations: ``getDataPackage()`` copies the background reader's latest
+package into it, ``getDataPackageBlocking()`` parses the next package straight into it. The
+deprecated ``getDataPackage(timeout)`` overload, which returns a new package instead, allocates on
+every call by design and is therefore not suited for real-time use.
+
+Field data types
+~~~~~~~~~~~~~~~~
+
+``getData()`` has to be given a variable of the field's own type, and returns ``false`` if it isn't.
+Rather than hardcoding which type a field has, ask the package: ``getDataType()`` reports the
+``DataType`` a field currently holds. After acknowledgement that is the type the robot reported;
+on an input package written with ``setData()`` before then, it is the type of that write. An
+untouched field has no type. This is useful for code that has to handle whatever recipe it is
+configured with, such as a bridge to another middleware:
+
+.. code-block:: c++
+
+   const std::optional<rtde_interface::DataType> type = data_pkg.getDataType(field_name);
+   if (!type)
+   {
+     // Not part of the recipe, or the field has no type yet
+     return;
+   }
+
+   // For "actual_q" this prints "VECTOR6D", the same spelling the RTDE guide uses
+   std::cout << field_name << " is a " << rtde_interface::toString(*type) << std::endl;
+
+   switch (*type)
+   {
+     case rtde_interface::DataType::DOUBLE:
+     {
+       double value;
+       data_pkg.getData(field_name, value);
+       break;
+     }
+     case rtde_interface::DataType::VECTOR6D:
+     {
+       vector6d_t value;
+       data_pkg.getData(field_name, value);
+       break;
+     }
+     // ... remaining types
+   }
+
+``DataType`` covers the complete set the protocol defines: ``BOOL``, ``UINT8``, ``UINT32``,
+``UINT64``, ``INT32``, ``DOUBLE``, ``VECTOR3D``, ``VECTOR6D``, ``VECTOR6INT32`` and
+``VECTOR6UINT32``. Switching over it exhaustively means the compiler will point out any case a
+future protocol addition leaves unhandled.
+
 Writing data
 ------------
 
@@ -105,11 +165,11 @@ an empty input recipe, like this:
    // Alternatively, pass an empty filename when using recipe files
    // rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, "");
    my_client.init();
-   auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client->getOutputRecipe());
+   auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client.getOutputRecipe());
    my_client.start();
    while (true)
    {
-     if (my_client.getDataPackage(data_package, READ_TIMEOUT))
+     if (my_client.getDataPackage(data_pkg, READ_TIMEOUT))
      {
        std::cout << data_pkg->toString() << std::endl;
      }
@@ -125,6 +185,22 @@ The class offers specific methods for every RTDE input possible to write.
 
 Data is sent asynchronously to the RTDE interface.
 
+To write several fields at once, ask the client for a package that already carries the data types
+the robot reported for the input recipe, fill the fields you care about and pass it to
+``sendPackage()``. Fields you leave alone are sent as zeros. Because the package is already typed,
+``setData()`` reports a value written with the wrong type immediately:
+
+.. code-block:: c++
+
+   rtde_interface::DataPackage input_pkg = my_client.createInputDataPackage();
+   input_pkg.setData("speed_slider_mask", uint32_t{ 1 });
+   input_pkg.setData("speed_slider_fraction", 0.5);
+   my_client.getWriter().sendPackage(input_pkg);
+
+A package constructed from ``getInputRecipe()`` still works. Its types are taken from the values
+written to it and are checked when the package is sent. See the :ref:`rtde_writer_example` for a
+complete example.
+
 .. note::
 
    The ``RTDEWriter`` will return ``false`` on any writing attempts for fields that have not been

doc/examples.rst

@@ -23,6 +23,7 @@ may be running forever until manually stopped.
    examples/primary_pipeline
    examples/primary_pipeline_calibration
    examples/rtde_client
+   examples/rtde_writer
    examples/external_fts_through_rtde
    examples/script_command_interface
    examples/script_sender

doc/examples/rtde_client.rst

@@ -56,15 +56,18 @@ fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method.
    :start-at: auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client.getOutputRecipe());
    :end-before: // Change the speed slider
 
+Creating the package we read into is the last allocation the read path makes; the loop below reuses
+the same package. The recipe only names the fields, so the first read is also what tells this one
+what type each of its fields has, which needs no further memory.
+
 In our main loop, we wait for a new data package to arrive using the blocking read method. Once
 received, data from the received package can be accessed using the ``getData()`` method of the
 ``DataPackage`` object. This method takes the key of the data to be accessed as a parameter and
 returns the corresponding value.
 
 .. note:: The key used to access data has to be part of the output recipe used to initialize the RTDE
-   client. Passing a string literal, e.g. ``"actual_q"``, is possible but not recommended as it is
-   converted to an ``std::string`` automatically, causing heap allocations which should be avoided
-   in Real-Time contexts.
+   client. ``getData()`` returns ``false`` for an unknown key, and also if the type of the passed
+   variable doesn't match the type the robot reported for that field.
 
 Writing Data to the RTDE client
 -------------------------------

doc/examples/rtde_writer.rst

@@ -0,0 +1,183 @@
+:github_url: https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/doc/examples/rtde_writer.rst
+
+.. _rtde_writer_example:
+
+RTDE writer example
+===================
+
+This example shows how to write several `Real-Time Data Exchange (RTDE)
+<https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/>`_
+inputs to the robot in a single package, at the robot's maximum frequency, and how to prove that
+the robot processed them.
+
+The one-field ``send...()`` helpers on ``RTDEWriter`` each produce a package of their own. When
+several general purpose registers have to change together, ``sendPackage()`` is the method that
+puts them on the wire in one RTDE package.
+
+The example's source code can be found in `rtde_writer.cpp
+<https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/rtde_writer.cpp>`_.
+
+.. note:: The robot has to be powered on and, on an e-Series, in *remote control mode* for the
+   register-processing program to be accepted.
+
+Recipes as argument lists
+-------------------------
+
+``RTDEClient`` takes the input and output recipes as two lists of field names. Recipe files work
+as well; see :ref:`rtde_client_example`. ``timestamp`` is part of the output recipe either way,
+because the client adds it if it is missing.
+
+The general purpose register ranges reserved for external RTDE clients are bit registers
+``64..127`` and integer and double registers ``24..47``.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const std::vector<std::string> INPUT_RECIPE
+   :end-at: const std::string OUTPUT_DOUBLE_REGISTER
+
+.. note:: Register fields, unlike the digital and analog outputs and the speed slider, need no
+   companion ``_mask`` key in the input recipe.
+
+Processing the registers on the robot
+-------------------------------------
+
+Input registers cannot be written from URScript, and output registers cannot be written through
+RTDE. Getting values back therefore requires a program on the robot.
+
+The program does not copy the values. RTDE also exposes the input registers as outputs, so a
+plain echo would be indistinguishable from that read-back. Instead the program inverts the bit,
+adds one to the integer and negates the double. A value that satisfies those relations can only
+have been produced by this program. ``sync()`` runs the loop once per control cycle.
+
+``sendScript()`` is used rather than ``sendScriptBlocking()``, because the latter would wait until
+the program stops, and this one loops forever.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: const std::string MIRROR_PROGRAM
+   :end-at: end)";
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Start the robot program that processes the registers
+   :end-at: // The program keeps running until we stop it later.
+
+An input package with the robot's field types
+---------------------------------------------
+
+The data types of the input recipe belong to the robot and arrive with the handshake, so the
+package has to be created after ``init()``. ``createInputDataPackage()`` returns a zeroed package
+that already carries those types: ``setData()`` then rejects a wrong type immediately, and
+copying the package into the send buffer is a single memcpy.
+
+A package constructed from ``getInputRecipe()`` still works. Its types are taken from the values
+written to it and are only checked when the package is sent.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // RTDE client at the robot's maximum frequency
+   :end-at: my_client.start(false);
+
+``target_frequency = 0.0`` (the default) requests the robot's maximum: 125 Hz on CB3, 500 Hz on
+e-Series. See :ref:`real time setup` and :ref:`rtde_client`.
+
+Both ``DataPackage`` objects are allocated before the loop, so the loop itself is allocation-free.
+The output package is built from ``getOutputRecipe()`` and is therefore still untyped; the first
+read applies the robot's types to it in place, which needs no memory.
+
+Letting the robot pace the loop
+-------------------------------
+
+``start(false)`` leaves the background read thread off. ``getDataPackageBlocking()`` returns once
+per RTDE cycle and is this loop's time base. The input package is produced immediately after the
+read so it reaches the robot in time to be acted on in the next cycle. Printing is throttled to
+about once per second, so it stays out of the hot path.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // The blocking read is this loop's clock
+   :end-at: URCL_LOG_ERROR("Could not get a fresh data package from the robot.");
+
+Writing several inputs in one package
+-------------------------------------
+
+Unwritten fields of the package are sent as zeros. One ``sendPackage()`` produces exactly one
+RTDE package; the ``send...()`` helpers would produce one package per field. The call only queues
+the values for the writer thread, so the loop stays aligned to the robot.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Writing several general purpose inputs in one package
+   :end-at: URCL_LOG_ERROR("Sending RTDE data failed.");
+
+Verifying that the robot processed the data
+-------------------------------------------
+
+All three values sent in a cycle are derived from the cycle counter, so the integer the robot
+returns identifies which cycle an answer belongs to. ``echoed_int - 1`` is that counter. The
+expected bit is its inversion and the expected double is the negated sine. The robot's double
+register is a 64-bit value, so the negated sine comes back bit for bit and is compared exactly.
+Together with the inverted bit, that is what makes an answer attributable to this program rather
+than to RTDE's own read-back of the input registers.
+
+Against URSim the lag is one cycle: the values written after the read of cycle N are processed by
+the robot and observed in the read of cycle N+1. ``getData()`` needs a variable of the field's own
+type; ``getDataType()`` reports that type if the recipe is not known in advance.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Reading what the robot made of the previous package
+   :end-at: ++mismatches;
+
+Cleanup
+-------
+
+The input registers are reset and the robot program is stopped. A failed stop is only logged,
+because CI runs the example for one second and still requires exit code 0.
+
+.. literalinclude:: ../../examples/rtde_writer.cpp
+   :language: c++
+   :caption: examples/rtde_writer.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: // Reset the input registers before leaving
+   :end-at: return 0;
+
+Example output
+--------------
+
+The following shows a run against URSim 5.25.1 asking for 500 Hz. The echoed integer trails the
+sent integer by one cycle, and ``verified=1`` means the bit and the double match the
+transformations the robot program applies to that cycle.
+
+.. code::
+
+   [INFO] RTDE target frequency: 500.000000 Hz
+   sent: bit=1 int=484 double=-0.991869 | robot: bit=0 int=483 double=0.994216 | verified=1 lag_cycles=1 freq=483.063 Hz playing=1
+   sent: bit=0 int=967 double=-0.242772 | robot: bit=1 int=966 double=0.223323 | verified=1 lag_cycles=1 freq=482.826 Hz playing=1
+   sent: bit=1 int=1450 double=0.934895 | robot: bit=0 int=1449 double=-0.941806 | verified=1 lag_cycles=1 freq=482.669 Hz playing=1
+   [INFO] Cycles: 1931, average frequency: 482.628400 Hz, verified: 1929, mismatches: 0, last lag: 1 cycles
+
+A simulator shares the host's CPU, so the measured frequency stays somewhat below the requested
+one; on a real controller it tracks the target closely.

doc/migration_notes.rst

@@ -5,6 +5,41 @@ This document contains notes on the migration of the ur_client_library between m
 
 It contains only breaking changes.
 
+RTDE field types come from the robot
+------------------------------------
+
+The data types of an RTDE recipe's fields are now taken from the robot's answer to the recipe setup,
+instead of from a table of field names maintained inside the library. No application code has to
+change for this: ``DataPackage`` is still constructed from a recipe, still allocates all of its
+storage there, and is typed by the robot's answer afterwards, which costs no memory.
+
+Four consequences are worth knowing about:
+
+- **A field name the robot doesn't know is reported later.** Since the library no longer has its own
+  list of field names, a typo is caught when the robot rejects the recipe during
+  ``RTDEClient::init()`` rather than while constructing the ``RTDEClient``. It is still an
+  ``RTDEInvalidKeyException``. ``ignore_unavailable_outputs`` now also strips a name no robot knows:
+  without a list of its own, the library cannot tell a typo from a field of a newer robot.
+- **A wrongly typed input field is reported when the package is sent.** ``DataPackage::setData()``
+  decides a field's type from the value passed to it, so it can no longer tell on its own that the
+  robot expects something else. ``RTDEWriter::sendPackage()`` checks the package against the robot's
+  answer and names the field and both types if they disagree. A typed input package is now also
+  available from ``RTDEClient::createInputDataPackage()`` after ``init()``, so filling several
+  input fields no longer depends on guessing the field types correctly; ``setData()`` then rejects
+  a mismatch immediately.
+- **Reading a field as the wrong type no longer throws.** ``DataPackage::getData()`` used to let a
+  ``std::bad_variant_access`` escape when the passed variable didn't match the field's type. It now
+  returns ``false`` and logs which type the robot reported for that field, matching what its
+  documentation always promised. Code that caught that exception should check the return value
+  instead.
+- **``getData()``/``setData()`` with a ``std::string`` is now a compile error.** That alternative
+  was never a protocol type, so those calls used to compile and return ``false`` at runtime. Nothing
+  could have relied on them working.
+
+On a ``DataPackage`` that hasn't been typed yet, meaning it has neither received data nor been
+written to, ``getData()`` fails with an explanatory message instead of returning stale values, and
+``getDataType()`` reports that the field has no type yet.
+
 Migrating from 1.x.x to 2.x.x
 -----------------------------
 

examples/CMakeLists.txt

@@ -23,6 +23,10 @@ add_executable(rtde_client_example
   rtde_client.cpp)
 target_link_libraries(rtde_client_example ur_client_library::urcl)
 
+add_executable(rtde_writer_example
+  rtde_writer.cpp)
+target_link_libraries(rtde_writer_example ur_client_library::urcl)
+
 add_executable(dashboard_example
   dashboard_example.cpp)
 target_link_libraries(dashboard_example ur_client_library::urcl)

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);
     }