Refactor rtde parsing for RT and performance improvements (#419)

This is a refactor of RTDE communication effectively improving two
things:

- No more heap memory allocations during communication once everything
is setup
- Reduced parsing and access times of RTDE data that reduces the time of
read and send operations

There is a backwards-compatible implementation that will produce a deprecation warning when compiling.

**RTDE read path is refactored to avoid per-cycle heap allocations and
reduce overhead.** `RTDEClient` no longer relies on the `Pipeline` for
reads; it uses `URProducer` directly plus a double-buffered background
read thread, and introduces `start(bool read_packages_in_background)`
with new `getDataPackage(DataPackage&/unique_ptr&, timeout)` and
`getDataPackageBlocking(...)` APIs while deprecating the allocating
`getDataPackage(timeout)` overload.
 
 **RTDE parsing and packages are updated to support reuse and stricter
validation.** `Parser`/`IProducer` add single-product parse/tryGet
overloads, `RTDEParser` is split into a new `rtde_parser.cpp` with a
prealloc-friendly parse path (vector-based parse deprecated),
`RTDEPackage` exposes `getType()`, and `DataPackage` switches from an
`unordered_map` to an ordered vector store, initializes empty data on
construction, validates recipe keys (throwing new
`RTDEInvalidKeyException`), adds type checking on `setData`, and
improves `toString()` formatting.

**RTDE write path is refactored for fewer allocations and batching.**
`RTDEWriter` replaces the moodycamel queue with double-buffered storage
+ condition variable, adds `sendPackage()` for sending a fully-prepared
package, preallocates register key strings, and resets mask fields after
sending; it now throws on `init()`/`setInputRecipe()` while running.

**API consumers, docs, and tests are migrated to the new
allocation-free patterns.** Examples and documentation now show
preallocating `DataPackage` and using the new read modes;
`ExampleRobotWrapper` removes its custom RTDE consumer thread and
forwards the background-read choice to `UrDriver`. Tests are
expanded/updated for background vs blocking read, reconnection, invalid
keys/conflicts, `DataPackage` parsing/toString, and parser
single-product mode.

**CI and tooling tweaks:** workflow disables URSim port forwarding,
gates coverage uploads on successful build, shortens example sleep
between runs, and improves fake RTDE server thread shutdown safety.

---------

Co-authored-by: Rune Søe-Knudsen <41109954+urrsk@users.noreply.github.com>

Author: urfeex

.github/workflows/ci.yml

@@ -40,7 +40,7 @@ jobs:
       - uses: actions/checkout@v6
       - name: start ursim
         run: |
-          scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d
+          scripts/start_ursim.sh -m $ROBOT_MODEL -v $URSIM_VERSION -p $PROGRAM_FOLDER -d -f DISABLED
         env:
           DOCKER_RUN_OPTS: --network ursim_net
           ROBOT_MODEL: ${{matrix.env.ROBOT_MODEL}}
@@ -64,6 +64,7 @@ jobs:
           CFLAGS: -g -O2 -fprofile-arcs -ftest-coverage
           LDFLAGS: -fprofile-arcs -ftest-coverage
       - name: build
+        id: build
         run: cmake --build build --config Debug
       - name: Create folder for test artifacts
         run: mkdir -p test_artifacts
@@ -75,18 +76,21 @@ jobs:
         env:
           URSIM_VERSION: ${{matrix.env.URSIM_VERSION}}
       - name: Upload test results to Codecov
-        if: ${{ !cancelled() }}
+        if: ${{ !cancelled() && steps.build.outcome == 'success' }}
         uses: codecov/test-results-action@v1
         with:
           token: ${{ secrets.CODECOV_TOKEN }}
           fail_ci_if_error: true
       - name: run examples
         run: ./run_examples.sh "192.168.56.101" 1
       - name: install gcovr
+        if: ${{ !cancelled() && steps.build.outcome == 'success' }}
         run: sudo apt-get install -y gcovr
       - name: gcovr
+        if: ${{ !cancelled() && steps.build.outcome == 'success' }}
         run: cd build && gcovr -r .. --gcov-ignore-parse-errors negative_hits.warn_once_per_file --exclude "../3rdparty"
       - name: Upload coverage to Codecov
+        if: ${{ !cancelled() && steps.build.outcome == 'success' }}
         uses: codecov/codecov-action@v5
         with:
           fail_ci_if_error: true

CMakeLists.txt

@@ -43,6 +43,7 @@ add_library(urcl
     src/rtde/get_urcontrol_version.cpp
     src/rtde/request_protocol_version.cpp
     src/rtde/rtde_package.cpp
+    src/rtde/rtde_parser.cpp
     src/rtde/text_message.cpp
     src/rtde/rtde_client.cpp
     src/ur/ur_driver.cpp

doc/architecture/rtde_client.rst

@@ -7,17 +7,34 @@ RTDEClient
 
 The Real Time Data Exchange Client, ``RTDEClient``, class serves as a standalone
 `RTDE <https://www.universal-robots.com/articles/ur-articles/real-time-data-exchange-rtde-guide/>`_
-client. To use the RTDE-Client, you'll have to initialize and start it separately:
+client. To use the RTDE-Client, you'll have to initialize and start it separately. When starting
+it, it can be chosen whether data should be read in a background thread or if the user has to poll
+data in each cycle.
+
+- **Background read**: When background read is enabled (``start(true)``) (default), the RTDE client
+  will start a background thread that continuously reads data from the robot. The latest data
+  package can be fetched using the ``getDataPackage()`` method. This method returns immediately
+  with the latest data package received from the robot. If no data has been received since last
+  calling this function, it will block for a specified timeout waiting for new data to arrive.
+
+- **Blocking synchronous read**: When background read is not enabled (``start(false)``), data can
+  (and has to be) fetched using the ``getDataPackageBlocking()`` method. This call waits for a new
+  data package to arrive and parses that into the passed ``DataPackage`` object. This has to be
+  called with the RTDE control frequency, as the robot will shutdown RTDE communication if data is
+  not read by the client.
+
+The following example uses the background read method to fetch data from the RTDE interface. See
+the :ref:`rtde_client_example` for an example of the blocking read method.
 
 .. code-block:: c++
 
    rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, INPUT_RECIPE_FILE);
    my_client.init();
-   my_client.start();
+   my_client.start(true); // Start background read
+   rtde_interface::DataPackage data_pkg(my_client.getOutputRecipe());
    while (true)
    {
-     std::unique_ptr<rtde_interface::DataPackage> data_pkg = my_client.getDataPackage(READ_TIMEOUT);
-     if (data_pkg)
+     if (my_client.getDataPackage(data_pkg, READ_TIMEOUT))
      {
        std::cout << data_pkg->toString() << std::endl;
      }
@@ -28,27 +45,32 @@ outputs. Please refer to the `RTDE
 guide <https://www.universal-robots.com/articles/ur-articles/real-time-data-exchange-rtde-guide/>`_
 on which elements are available.
 
-.. note::
+The recipes can be either passed as a filename or as a list of strings directly. E.g. the
+following will work
 
-   The recipes can be either passed as a filename or as a list of strings directly. E.g. the
-   following will work
+.. code-block:: c++
+
+   rtde_interface::RTDEClient my_client(
+     ROBOT_IP,
+     notifier,
+     {"timestamp", "actual_q"},
+     {"speed_slider_mask", "speed_slider_fraction"}
+   );
+
+.. note::
+   ``timestamp`` will always be a part of the output recipe and will be added afterwards, if not defined. As the ``timestamp`` used for verifying the connectivity.
 
-   .. code-block:: c++
+Reading data
+------------
 
-      rtde_interface::RTDEClient my_client(
-        ROBOT_IP,
-        notifier,
-        {"timestamp", "actual_q"},
-        {"speed_slider_mask", "speed_slider_fraction"}
-      );
+After calling ``my_client.start()``, data can be read from the
+``RTDEClient`` by calling ``getDataPackage()`` (with background thread running) or ``getDataPackageBlocking()`` (without background thread running) respectively.
 
-Inside the ``RTDEclient`` data is received in a separate thread, parsed by the ``RTDEParser`` and
-added to a pipeline queue.
+Remember that, when not using a background thread, data has to be polled regularly, as the robot
+will shutdown RTDE communication if the receiving side doesn't empty its buffer.
 
-Right after calling ``my_client.start()``, it should be made sure to read the buffer from the
-``RTDEClient`` by calling ``getDataPackage()`` frequently. The Client's queue can only contain a
-restricted number of items at a time, so a ``Pipeline producer overflowed!`` error will be raised
-if the buffer isn't read frequently enough.
+Writing data
+------------
 
 For writing data to the RTDE interface, use the ``RTDEWriter`` member of the ``RTDEClient``. It can be
 retrieved by calling ``getWriter()`` method. The ``RTDEWriter`` provides convenience methods to write
@@ -83,11 +105,11 @@ an empty input recipe, like this:
    // Alternatively, pass an empty filename when using recipe files
    // rtde_interface::RTDEClient my_client(ROBOT_IP, notifier, OUTPUT_RECIPE_FILE, "");
    my_client.init();
+   auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client->getOutputRecipe());
    my_client.start();
    while (true)
    {
-     std::unique_ptr<rtde_interface::DataPackage> data_pkg = my_client.getDataPackage(READ_TIMEOUT);
-     if (data_pkg)
+     if (my_client.getDataPackage(data_package, READ_TIMEOUT))
      {
        std::cout << data_pkg->toString() << std::endl;
      }

doc/examples/direct_torque_control.rst

@@ -77,4 +77,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th
    :linenos:
    :lineno-match:
    :start-at: // Setting the RobotReceiveTimeout
-   :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+   :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());

doc/examples/rtde_client.rst

@@ -25,11 +25,6 @@ to initialize the RTDE client.
    :start-at: const std::string OUTPUT_RECIPE
    :end-at: const std::string INPUT_RECIPE
 
-
-Internally, the RTDE client uses the same producer / consumer architecture as show in the
-:ref:`primary_pipeline_example` example. However, it doesn't have a consumer thread, so data has to
-be read by the user to avoid the pipeline's queue from overflowing.
-
 Creating an RTDE Client
 -----------------------
 
@@ -45,25 +40,32 @@ omitted, RTDE communication will be established at the robot's control frequency
    :start-at: comm::INotifier notifier;
    :end-at: my_client.init();
 
-An RTDE data package containing every key-value pair from the output recipe can be fetched using
-the ``getDataPackage()`` method. This method will block until a new package is available.
-
 
 Reading data from the RTDE client
 ---------------------------------
 
-Once the RTDE client is initialized, we'll have to start communication separately. As mentioned
-above, we'll have to read data from the client once communication is started, hence we start
-communication right before a loop reading data.
+To read data received by the RTDE client, it has to be polled. See the :ref:`rtde_client` section
+for details on two possible strategies. In this example, we do not use background read and instead
+fetch data synchronously. Hence, we pass ``false`` to the ``start()`` method.
 
 .. literalinclude:: ../../examples/rtde_client.cpp
    :language: c++
    :caption: examples/rtde_client.cpp
    :linenos:
    :lineno-match:
-   :start-at: // Once RTDE communication is started
+   :start-at: auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client.getOutputRecipe());
    :end-before: // Change the speed slider
 
+In our main loop, we wait for a new data package to arrive using the blocking read method. Once
+received, data from the received package can be accessed using the ``getData()`` method of the
+``DataPackage`` object. This method takes the key of the data to be accessed as a parameter and
+returns the corresponding value.
+
+.. note:: The key used to access data has to be part of the output recipe used to initialize the RTDE
+   client. Passing a string literal, e.g. ``"actual_q"``, is possible but not recommended as it is
+   converted to an ``std::string`` automatically, causing heap allocations which should be avoided
+   in Real-Time contexts.
+
 Writing Data to the RTDE client
 -------------------------------
 
@@ -84,6 +86,12 @@ initialize the RTDE client has to contain the keys necessary to send that specif
    :end-at: }
 
 
-.. note:: Many RTDE inputs require setting up the data key and a mask key. See the `RTDE guide
+.. note:: Many RTDE inputs require setting up the data key and a mask key. That is done
+   internally, but the mask keys have to be part of the input recipe, as well. See the `RTDE guide
    <https://www.universal-robots.com/articles/ur/interface-communication/real-time-data-exchange-rtde-guide/>`_
    for more information.
+
+.. note:: Every ``send...`` call to the RTDEWriter triggers a package sent to the robot. If you
+   want to modify more than one input at a time, it is recommended to use the ``sendPackage()``
+   method. That allows setting up the complete data package with its input recipe and sending that
+   to the robot at once.

doc/examples/ur_driver.rst

@@ -70,4 +70,4 @@ To send the control command, the robot's :ref:`reverse_interface` is used via th
    :linenos:
    :lineno-match:
    :start-at: // Setting the RobotReceiveTimeout
-   :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+   :end-before: URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());

examples/direct_torque_control.cpp

@@ -96,20 +96,22 @@ int main(int argc, char* argv[])
   // loop.
   g_my_robot->getUrDriver()->startRTDECommunication();
   auto start_time = std::chrono::system_clock::now();
+
+  urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
+
   while (!(passed_positive_part && passed_negative_part))
   {
     // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
     // robot will effectively be in charge of setting the frequency of this loop.
     // In a real-world application this thread should be scheduled with real-time priority in order
     // to ensure that this is called in time.
-    std::unique_ptr<urcl::rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage();
-    if (!data_pkg)
+    if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
     {
       URCL_LOG_WARN("Could not get fresh data package from robot");
       return 1;
     }
     // Read current joint positions from robot data
-    if (!data_pkg->getData("actual_q", g_joint_positions))
+    if (!data_pkg.getData("actual_q", g_joint_positions))
     {
       // This throwing should never happen unless misconfigured
       std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -146,7 +148,7 @@ int main(int argc, char* argv[])
       URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
       return 1;
     }
-    URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+    URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
     if (second_to_run.count() > 0 && (std::chrono::system_clock::now() - start_time) > second_to_run)
     {
       URCL_LOG_WARN("Time limit reached, stopping movement. This is expected on a simualted robot, as it doesn't move "

examples/external_fts_through_rtde.cpp

@@ -211,11 +211,12 @@ void rtdeWorker(const int second_to_run)
 
   vector6d_t actual_tcp_force;
   auto start_time = std::chrono::steady_clock::now();
+  std::unique_ptr<rtde_interface::DataPackage> data_pkg =
+      std::make_unique<rtde_interface::DataPackage>(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
   while (g_RUNNING)
   {
     urcl::vector6d_t local_ft_vec = g_FT_VEC;
-    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage();
-    if (data_pkg)
+    if (g_my_robot->getUrDriver()->getDataPackageBlocking(data_pkg))
     {
       // Data fields in the data package are accessed by their name. Only names present in the
       // output recipe can be accessed. Otherwise this function will return false.

examples/full_driver.cpp

@@ -87,20 +87,20 @@ int main(int argc, char* argv[])
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
   // loop.
   g_my_robot->getUrDriver()->startRTDECommunication();
+  rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
   while (!(passed_positive_part && passed_negative_part))
   {
     // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the
     // robot will effectively be in charge of setting the frequency of this loop.
     // In a real-world application this thread should be scheduled with real-time priority in order
     // to ensure that this is called in time.
-    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage();
-    if (!data_pkg)
+    if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg))
     {
       URCL_LOG_WARN("Could not get fresh data package from robot");
       return 1;
     }
     // Read current joint positions from robot data
-    if (!data_pkg->getData("actual_q", g_joint_positions))
+    if (!data_pkg.getData("actual_q", g_joint_positions))
     {
       // This throwing should never happen unless misconfigured
       std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!";
@@ -140,7 +140,7 @@ int main(int argc, char* argv[])
       URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
       return 1;
     }
-    URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg->toString().c_str());
+    URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str());
   }
   g_my_robot->getUrDriver()->stopControl();
   URCL_LOG_INFO("Movement done");

examples/rtde_client.cpp

@@ -39,7 +39,9 @@ using namespace urcl;
 const std::string DEFAULT_ROBOT_IP = "192.168.56.101";
 const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt";
 const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt";
-const std::chrono::milliseconds READ_TIMEOUT{ 100 };
+
+// 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)
 {
@@ -81,28 +83,27 @@ int main(int argc, char* argv[])
   double target_speed_fraction = 1.0;
   double speed_slider_increment = 0.01;
 
+  auto data_pkg = std::make_unique<rtde_interface::DataPackage>(my_client.getOutputRecipe());
   // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
   // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main
   // loop.
-  my_client.start();
+  my_client.start(false);  // false -> do not start background read thread.
 
   auto start_time = std::chrono::steady_clock::now();
   while (second_to_run <= 0 ||
          std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now() - start_time).count() <
              second_to_run)
   {
-    // Read latest RTDE package. This will block for READ_TIMEOUT, so the
-    // robot will effectively be in charge of setting the frequency of this loop unless RTDE
-    // communication doesn't work in which case the user will be notified.
-    // In a real-world application this thread should be scheduled with real-time priority in order
-    // to ensure that this is called in time.
-    std::unique_ptr<rtde_interface::DataPackage> data_pkg = my_client.getDataPackage(READ_TIMEOUT);
-    if (data_pkg)
+    // Wait for a DataPackage. In a real-world application this thread should be scheduled with real-time priority in
+    // order to ensure that this is called in time.
+    bool success = my_client.getDataPackageBlocking(data_pkg);
+    if (success)
     {
       // Data fields in the data package are accessed by their name. Only names present in the
       // output recipe can be accessed. Otherwise this function will return false.
-      data_pkg->getData("target_speed_fraction", target_speed_fraction);
-      printFraction(target_speed_fraction, "target_speed_fraction");
+      // 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);
     }
     else
     {
@@ -139,5 +140,7 @@ int main(int argc, char* argv[])
   // Resetting the speedslider back to 100%
   my_client.getWriter().sendSpeedSlider(1);
 
+  URCL_LOG_INFO("Exiting RTDE read/write example.");
+
   return 0;
 }