Adapt RTDE output recipe based on robot response (#221)

Not all RTDE outputs are available in all SW versions. When working with
a fleet of robots with heterogeneous SW versions it's not practical to
have multiple RTDE output recipes to account for this.
This PR aims at solving this problem by adapting the RTDE output recipe
on-the-fly based on the robot response to the RTDE request. Outputs
which are not available for this particular SW version are simply
removed from the output recipe and a trimmed down output recipe is sent
to the robot.

Potential issues / improvements:
- variables removed from the output recipe only cause a warning. When
these variables are "optional" this is a fine behaviour but it could
potentially be masking incorrect variable names. Hence, this is an opt-in feature.
- an alternate implementation could be to hardcode the compatibility
information for RTDE outputs in `data_package.cpp` and validate the
output recipe beforehand. This seems less flexible though.
- the same logic could potentially be applied to the input recipe.
Having optional inputs seems less likely though.

Author: remi-siffert-ocado

include/ur_client_library/comm/pipeline.h

@@ -43,6 +43,8 @@ template <typename T>
 class IConsumer
 {
 public:
+  virtual ~IConsumer() = default;
+
   /*!
    * \brief Set-up functionality of the consumer.
    */
@@ -170,6 +172,8 @@ template <typename T>
 class IProducer
 {
 public:
+  virtual ~IProducer() = default;
+
   /*!
    * \brief Set-up functionality of the producers.
    *

include/ur_client_library/rtde/rtde_client.h

@@ -29,6 +29,8 @@
 #ifndef UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED
 #define UR_CLIENT_LIBRARY_RTDE_CLIENT_H_INCLUDED
 
+#include <memory>
+
 #include "ur_client_library/comm/pipeline.h"
 #include "ur_client_library/rtde/package_header.h"
 #include "ur_client_library/rtde/rtde_package.h"
@@ -102,9 +104,12 @@ class RTDEClient
    * \param output_recipe_file Path to the file containing the output recipe
    * \param input_recipe_file Path to the file containing the input recipe
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
+   * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
-             const std::string& input_recipe_file, double target_frequency = 0.0);
+             const std::string& input_recipe_file, double target_frequency = 0.0,
+             bool ignore_unavailable_outputs = false);
 
   /*!
    * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
@@ -115,9 +120,12 @@ class RTDEClient
    * \param output_recipe Vector containing the output recipe
    * \param input_recipe Vector containing the input recipe
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
+   * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector<std::string>& output_recipe,
-             const std::vector<std::string>& input_recipe, double target_frequency = 0.0);
+             const std::vector<std::string>& input_recipe, double target_frequency = 0.0,
+             bool ignore_unavailable_outputs = false);
   ~RTDEClient();
   /*!
    * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
@@ -210,10 +218,12 @@ class RTDEClient
 private:
   comm::URStream<RTDEPackage> stream_;
   std::vector<std::string> output_recipe_;
+  bool ignore_unavailable_outputs_;
   std::vector<std::string> input_recipe_;
   RTDEParser parser_;
-  comm::URProducer<RTDEPackage> prod_;
-  comm::Pipeline<RTDEPackage> pipeline_;
+  std::unique_ptr<comm::URProducer<RTDEPackage>> prod_;
+  comm::INotifier notifier_;
+  std::unique_ptr<comm::Pipeline<RTDEPackage>> pipeline_;
   RTDEWriter writer_;
 
   VersionInformation urcontrol_version_;
@@ -241,6 +251,14 @@ class RTDEClient
   void setupInputs();
   void disconnect();
 
+  /*!
+   * \brief Updates the output recipe to the given one and recreates all the objects which depend on it.
+   * It should only be called while setting up the communication.
+   *
+   * \param new_recipe the new output recipe to use
+   */
+  void resetOutputRecipe(const std::vector<std::string> new_recipe);
+
   /*!
    * \brief Checks whether the robot is booted, this is done by looking at the timestamp from the robot controller, this
    * will show the time in seconds since the controller was started. If the timestamp is below 40, we will read from

include/ur_client_library/ur/ur_driver.h

@@ -605,10 +605,13 @@ class UrDriver
    *
    * \param output_recipe Vector containing the output recipe
    * \param input_recipe Vector containing the input recipe
-   * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
+   * \param target_frequency
+   * Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
+   * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   void resetRTDEClient(const std::vector<std::string>& output_recipe, const std::vector<std::string>& input_recipe,
-                       double target_frequency = 0.0);
+                       double target_frequency = 0.0, bool ignore_unavailable_outputs = false);
 
   /**
    * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication
@@ -621,9 +624,11 @@ class UrDriver
    * \param output_recipe_filename Filename where the output recipe is stored in.
    * \param input_recipe_filename Filename where the input recipe is stored in.
    * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
+   * \param ignore_unavailable_outputs Configure the behaviour when a variable of the output recipe is not available
+   * from the robot: output is silently ignored if true, a UrException is raised otherwise.
    */
   void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
-                       double target_frequency = 0.0);
+                       double target_frequency = 0.0, bool ignore_unavailable_outputs = false);
 
 private:
   static std::string readScriptFile(const std::string& filename);

src/rtde/rtde_client.cpp

@@ -35,13 +35,15 @@ namespace urcl
 namespace rtde_interface
 {
 RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::string& output_recipe_file,
-                       const std::string& input_recipe_file, double target_frequency)
+                       const std::string& input_recipe_file, double target_frequency, bool ignore_unavailable_outputs)
   : stream_(robot_ip, UR_RTDE_PORT)
   , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file)))
+  , ignore_unavailable_outputs_(ignore_unavailable_outputs)
   , input_recipe_(readRecipe(input_recipe_file))
   , parser_(output_recipe_)
-  , prod_(stream_, parser_)
-  , pipeline_(prod_, PIPELINE_NAME, notifier, true)
+  , prod_(std::make_unique<comm::URProducer<RTDEPackage>>(stream_, parser_))
+  , notifier_(notifier)
+  , pipeline_(std::make_unique<comm::Pipeline<RTDEPackage>>(*prod_, PIPELINE_NAME, notifier, true))
   , writer_(&stream_, input_recipe_)
   , max_frequency_(URE_MAX_FREQUENCY)
   , target_frequency_(target_frequency)
@@ -50,13 +52,16 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
 }
 
 RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const std::vector<std::string>& output_recipe,
-                       const std::vector<std::string>& input_recipe, double target_frequency)
+                       const std::vector<std::string>& input_recipe, double target_frequency,
+                       bool ignore_unavailable_outputs)
   : stream_(robot_ip, UR_RTDE_PORT)
   , output_recipe_(ensureTimestampIsPresent(output_recipe))
+  , ignore_unavailable_outputs_(ignore_unavailable_outputs)
   , input_recipe_(input_recipe)
   , parser_(output_recipe_)
-  , prod_(stream_, parser_)
-  , pipeline_(prod_, PIPELINE_NAME, notifier, true)
+  , prod_(std::make_unique<comm::URProducer<RTDEPackage>>(stream_, parser_))
+  , notifier_(notifier)
+  , pipeline_(std::make_unique<comm::Pipeline<RTDEPackage>>(*prod_, PIPELINE_NAME, notifier, true))
   , writer_(&stream_, input_recipe_)
   , max_frequency_(URE_MAX_FREQUENCY)
   , target_frequency_(target_frequency)
@@ -96,8 +101,8 @@ void RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron
 {
   client_state_ = ClientState::INITIALIZING;
   // A running pipeline is needed inside setup
-  pipeline_.init(max_num_tries, reconnection_time);
-  pipeline_.run();
+  pipeline_->init(max_num_tries, reconnection_time);
+  pipeline_->run();
 
   uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION;
   while (!negotiateProtocolVersion(protocol_version) && client_state_ == ClientState::INITIALIZING)
@@ -151,7 +156,7 @@ void RTDEClient::setupCommunication(const size_t max_num_tries, const std::chron
     return;
 
   // We finished communication for now
-  pipeline_.stop();
+  pipeline_->stop();
   client_state_ = ClientState::INITIALIZED;
 }
 
@@ -174,7 +179,7 @@ bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version)
   while (num_retries < MAX_REQUEST_RETRIES)
   {
     std::unique_ptr<RTDEPackage> package;
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("failed to get package from rtde interface, disconnecting");
       disconnect();
@@ -220,7 +225,7 @@ void RTDEClient::queryURControlVersion()
   std::unique_ptr<RTDEPackage> package;
   while (num_retries < MAX_REQUEST_RETRIES)
   {
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("No answer to urcontrol version query was received from robot, disconnecting");
       disconnect();
@@ -249,39 +254,52 @@ void RTDEClient::queryURControlVersion()
   throw UrException(ss.str());
 }
 
+void RTDEClient::resetOutputRecipe(const std::vector<std::string> new_recipe)
+{
+  prod_->teardownProducer();
+  disconnect();
+
+  output_recipe_.assign(new_recipe.begin(), new_recipe.end());
+  parser_ = RTDEParser(output_recipe_);
+  prod_ = std::make_unique<comm::URProducer<RTDEPackage>>(stream_, parser_);
+  pipeline_ = std::make_unique<comm::Pipeline<RTDEPackage>>(*prod_, PIPELINE_NAME, notifier_, true);
+}
+
 void RTDEClient::setupOutputs(const uint16_t protocol_version)
 {
   unsigned int num_retries = 0;
   size_t size;
   size_t written;
   uint8_t buffer[8192];
   URCL_LOG_INFO("Setting up RTDE communication with frequency %f", target_frequency_);
-  if (protocol_version == 2)
-  {
-    size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_);
-  }
-  else
+
+  while (num_retries < MAX_REQUEST_RETRIES)
   {
-    if (target_frequency_ != max_frequency_)
+    URCL_LOG_DEBUG("Sending output recipe");
+    if (protocol_version == 2)
     {
-      URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency "
-                    "equivalent to the maximum frequency will be used instead.");
+      size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, target_frequency_, output_recipe_);
+    }
+    else
+    {
+      if (target_frequency_ != max_frequency_)
+      {
+        URCL_LOG_WARN("It is not possible to set a target frequency when using protocol version 1. A frequency "
+                      "equivalent to the maximum frequency will be used instead.");
+      }
+      size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_);
     }
-    size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, output_recipe_);
-  }
 
-  // Send output recipe to robot
-  if (!stream_.write(buffer, size, written))
-  {
-    URCL_LOG_ERROR("Could not send RTDE output recipe to robot, disconnecting");
-    disconnect();
-    return;
-  }
+    // Send output recipe to robot
+    if (!stream_.write(buffer, size, written))
+    {
+      URCL_LOG_ERROR("Could not send RTDE output recipe to robot, disconnecting");
+      disconnect();
+      return;
+    }
 
-  while (num_retries < MAX_REQUEST_RETRIES)
-  {
     std::unique_ptr<RTDEPackage> package;
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("Did not receive confirmation on RTDE output recipe, disconnecting");
       disconnect();
@@ -293,18 +311,53 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version)
 
     {
       std::vector<std::string> variable_types = splitVariableTypes(tmp_output->variable_types_);
+      std::vector<std::string> available_variables;
+      std::vector<std::string> unavailable_variables;
       assert(output_recipe_.size() == variable_types.size());
       for (std::size_t i = 0; i < variable_types.size(); ++i)
       {
-        URCL_LOG_DEBUG("%s confirmed as datatype: %s", output_recipe_[i].c_str(), variable_types[i].c_str());
+        const std::string variable_name = output_recipe_[i];
+        URCL_LOG_DEBUG("%s confirmed as datatype: %s", variable_name.c_str(), variable_types[i].c_str());
+
         if (variable_types[i] == "NOT_FOUND")
         {
-          std::string message = "Variable '" + output_recipe_[i] +
-                                "' not recognized by the robot. Probably your output recipe contains errors";
-          throw UrException(message);
+          unavailable_variables.push_back(variable_name);
+        }
+        else
+        {
+          available_variables.push_back(variable_name);
         }
       }
-      return;
+
+      if (!unavailable_variables.empty())
+      {
+        std::stringstream error_message;
+        error_message << "The following variables are not recognized by the robot: ";
+        std::for_each(unavailable_variables.begin(), unavailable_variables.end(),
+                      [&error_message](const std::string& variable_name) { error_message << variable_name << " "; });
+        error_message << ". Either your output recipe contains errors "
+                         "or the urcontrol version does not support "
+                         "them.";
+
+        if (ignore_unavailable_outputs_)
+        {
+          error_message << " They will be removed from the output recipe.";
+          URCL_LOG_WARN("%s", error_message.str().c_str());
+
+          // Some variables are not available so retry setting up the communication with a stripped-down output recipe
+          resetOutputRecipe(available_variables);
+        }
+        else
+        {
+          URCL_LOG_ERROR("%s", error_message.str().c_str());
+          throw UrException(error_message.str());
+        }
+      }
+      else
+      {
+        // All variables are accounted for in the RTDE package
+        return;
+      }
     }
     else
     {
@@ -339,7 +392,7 @@ void RTDEClient::setupInputs()
   while (num_retries < MAX_REQUEST_RETRIES)
   {
     std::unique_ptr<RTDEPackage> package;
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("Did not receive confirmation on RTDE input recipe, disconnecting");
       disconnect();
@@ -395,7 +448,7 @@ void RTDEClient::disconnect()
   if (client_state_ > ClientState::UNINITIALIZED)
   {
     sendPause();
-    pipeline_.stop();
+    pipeline_->stop();
     stream_.disconnect();
   }
   client_state_ = ClientState::UNINITIALIZED;
@@ -421,7 +474,7 @@ bool RTDEClient::isRobotBooted()
   {
     // Set timeout based on target frequency, to make sure that reading doesn't timeout
     int timeout = static_cast<int>((1 / target_frequency_) * 1000) * 10;
-    if (pipeline_.getLatestProduct(package, std::chrono::milliseconds(timeout)))
+    if (pipeline_->getLatestProduct(package, std::chrono::milliseconds(timeout)))
     {
       rtde_interface::DataPackage* tmp_input = dynamic_cast<rtde_interface::DataPackage*>(package.get());
       tmp_input->getData("timestamp", timestamp);
@@ -451,7 +504,7 @@ bool RTDEClient::start()
     return false;
   }
 
-  pipeline_.run();
+  pipeline_->run();
 
   if (sendStart())
   {
@@ -501,7 +554,7 @@ bool RTDEClient::sendStart()
   unsigned int num_retries = 0;
   while (num_retries < MAX_REQUEST_RETRIES)
   {
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("Could not get response to RTDE communication start request from robot");
       return false;
@@ -543,7 +596,7 @@ bool RTDEClient::sendPause()
   int seconds = 5;
   while (std::chrono::steady_clock::now() - start < std::chrono::seconds(seconds))
   {
-    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    if (!pipeline_->getLatestProduct(package, std::chrono::milliseconds(1000)))
     {
       URCL_LOG_ERROR("Could not get response to RTDE communication pause request from robot");
       return false;
@@ -605,7 +658,7 @@ std::vector<std::string> RTDEClient::ensureTimestampIsPresent(const std::vector<
 std::unique_ptr<rtde_interface::DataPackage> RTDEClient::getDataPackage(std::chrono::milliseconds timeout)
 {
   std::unique_ptr<RTDEPackage> urpackage;
-  if (pipeline_.getLatestProduct(urpackage, timeout))
+  if (pipeline_->getLatestProduct(urpackage, timeout))
   {
     rtde_interface::DataPackage* tmp = dynamic_cast<rtde_interface::DataPackage*>(urpackage.get());
     if (tmp != nullptr)