Refactor rtde parsing

CMakeLists.txt

@@ -40,6 +40,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/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,48 @@ 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. For this two modes are available:
+
+- **Background read**: When background read is enabled (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.
+
+  .. note:: This methods allocates a new data package on each call. We recommend using the blocking
+     read method explained below.
+- **Blocking synchronous read**: When background read is not enabled, 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.
+
+Which of the above strategies is used can be specified when starting RTDE communication using the
+``start()`` method. In our 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: std::unique_ptr<rtde_interface::DataPackage> data_pkg =
    :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 +102,13 @@ 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.
+

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,28 @@ int main(int argc, char* argv[])
   double target_speed_fraction = 1.0;
   double speed_slider_increment = 0.01;
 
+  std::unique_ptr<rtde_interface::DataPackage> 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);
 
   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 +141,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;
 }

include/ur_client_library/comm/package.h

@@ -35,9 +35,9 @@ namespace urcl
 namespace comm
 {
 /*!
- * \brief The URPackage a parent class. From that two implementations are inherited,
- * one for the primary, one for the rtde interface (primary_interface::primaryPackage;
- * rtde_interface::rtdePackage). The URPackage makes use of the template HeaderT.
+ * \brief The URPackage is an abstract class. From that two implementations are inherited,
+ * one for the primary, one for the RTDE interface (primary_interface::PrimaryPackage;
+ * rtde_interface::RTDEPackage). The URPackage makes use of the template HeaderT.
  */
 template <typename HeaderT>
 class URPackage

include/ur_client_library/comm/parser.h

@@ -41,13 +41,28 @@ class Parser
   virtual ~Parser() = default;
 
   /*!
-   * \brief Declares the parse function.
+   * \brief Parses data from a binary parser into a vector of packages.
+   *
+   * This will create new packages and push them to the results vector, thus new memory will be
+   * allocated for each package. Do not use this in a real-time context.
    *
    * \param bp Instance of class binaryParser
-   * \param results A unique pointer
+   * \param results A vector of unique pointers to packages
    */
   virtual bool parse(BinParser& bp, std::vector<std::unique_ptr<T>>& results) = 0;
 
+  /*!
+   * \brief Parses data from a binary parser into a single package.
+   *
+   * This implementation may try to store data in the passed package instance to avoid memory
+   * allocations. Thus, this function may be used in a real-time context. Refer to the specific
+   * implementation and parser for details.
+   *
+   * \param bp Instance of class binaryParser
+   * \param result A unique pointer to a package where parsed data should be stored.
+   */
+  virtual bool parse(BinParser& bp, std::unique_ptr<T>& result) = 0;
+
 private:
   typename T::HeaderType header_;
   // URProducer producer_;

include/ur_client_library/comm/pipeline.h

@@ -245,6 +245,17 @@ class IProducer
    * \returns Success of the package production.
    */
   virtual bool tryGet(std::vector<std::unique_ptr<T>>& products) = 0;
+
+  /*!
+   * \brief Reads a single package from some source and produces the corresponding object.
+   *
+   * If possible this function should try to reuse existing memory in the passed unique pointer.
+   *
+   * \param product Unique pointer to be set to the produced package.
+   *
+   * \returns Success of the package production.
+   */
+  virtual bool tryGet(std::unique_ptr<T>& product) = 0;
 };
 
 /*!

include/ur_client_library/comm/producer.h

@@ -47,6 +47,57 @@ class URProducer : public IProducer<T>
 
   bool running_;
 
+  template <typename ProductT>
+  bool tryGetImpl(ProductT& product)
+  {
+    // TODO This function has become really ugly! That should be refactored!
+
+    // 4KB should be enough to hold any packet received from UR
+    uint8_t buf[4096];
+    size_t read = 0;
+    // expoential backoff reconnects
+    while (true)
+    {
+      if (stream_.read(buf, sizeof(buf), read))
+      {
+        // reset sleep amount
+        timeout_ = std::chrono::seconds(1);
+        BinParser bp(buf, read);
+        return parser_.parse(bp, product);
+      }
+
+      if (!running_)
+        return true;
+
+      if (stream_.getState() == SocketState::Connected)
+      {
+        continue;
+      }
+
+      if (stream_.closed())
+        return false;
+
+      if (on_reconnect_cb_)
+      {
+        URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer");
+        on_reconnect_cb_();
+        return false;
+      }
+
+      URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
+      std::this_thread::sleep_for(timeout_);
+
+      if (stream_.connect())
+        continue;
+
+      auto next = timeout_ * 2;
+      if (next <= std::chrono::seconds(120))
+        timeout_ = next;
+    }
+
+    return false;
+  }
+
 public:
   /*!
    * \brief Creates a URProducer object, registering a stream and a parser.
@@ -106,52 +157,22 @@ class URProducer : public IProducer<T>
    */
   bool tryGet(std::vector<std::unique_ptr<T>>& products) override
   {
-    // TODO This function has become really ugly! That should be refactored!
-
-    // 4KB should be enough to hold any packet received from UR
-    uint8_t buf[4096];
-    size_t read = 0;
-    // expoential backoff reconnects
-    while (true)
-    {
-      if (stream_.read(buf, sizeof(buf), read))
-      {
-        // reset sleep amount
-        timeout_ = std::chrono::seconds(1);
-        BinParser bp(buf, read);
-        return parser_.parse(bp, products);
-      }
-
-      if (!running_)
-        return true;
-
-      if (stream_.getState() == SocketState::Connected)
-      {
-        continue;
-      }
-
-      if (stream_.closed())
-        return false;
-
-      if (on_reconnect_cb_)
-      {
-        URCL_LOG_WARN("Failed to read from stream, invoking on reconnect callback and stopping the producer");
-        on_reconnect_cb_();
-        return false;
-      }
-
-      URCL_LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
-      std::this_thread::sleep_for(timeout_);
-
-      if (stream_.connect())
-        continue;
-
-      auto next = timeout_ * 2;
-      if (next <= std::chrono::seconds(120))
-        timeout_ = next;
-    }
+    return tryGetImpl(products);
+  }
 
-    return false;
+  /*!
+   * \brief Attempts to read byte stream from the robot and parse it as a URPackage.
+   *
+   * If supported by the parser, this function will try to reuse existing memory in the passed
+   * unique pointer.
+   *
+   * \param product Unique pointer to hold the produced package
+   *
+   * \returns Success of reading and parsing the package
+   */
+  bool tryGet(std::unique_ptr<T>& product) override
+  {
+    return tryGetImpl(product);
   }
 
   /*!

include/ur_client_library/exceptions.h

@@ -272,5 +272,51 @@ class UnexpectedResponse : public UrException
 private:
   std::string text_;
 };
+
+class RTDEInvalidKeyException : public UrException
+{
+public:
+  explicit RTDEInvalidKeyException() : std::runtime_error("RTDE Invalid Key Exception")
+  {
+  }
+
+  explicit RTDEInvalidKeyException(const std::string& text) : std::runtime_error(text)
+  {
+  }
+
+  virtual ~RTDEInvalidKeyException() = default;
+
+  virtual const char* what() const noexcept override
+  {
+    return std::runtime_error::what();
+  }
+};
+
+class RTDEInputConflictException : public UrException
+{
+public:
+  explicit RTDEInputConflictException() : std::runtime_error("RTDE Output Conflict Exception")
+  {
+  }
+
+  explicit RTDEInputConflictException(const std::string& key)
+    : std::runtime_error("RTDE Output Conflict for key: " + key), key_(key)
+  {
+    message_ = "Variable '" + key_ +
+               "' is currently controlled by another RTDE client. The input recipe can't be used as "
+               "configured";
+  }
+
+  virtual ~RTDEInputConflictException() = default;
+
+  virtual const char* what() const noexcept override
+  {
+    return message_.c_str();
+  }
+
+private:
+  std::string key_;
+  std::string message_;
+};
 }  // namespace urcl
 #endif  // ifndef UR_CLIENT_LIBRARY_EXCEPTIONS_H_INCLUDED