Adapt RTDE output recipe based on robot response

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"
@@ -206,14 +208,15 @@ class RTDEClient
   {
     return output_recipe_;
   }
-
+  
 private:
   comm::URStream<RTDEPackage> stream_;
   std::vector<std::string> output_recipe_;
   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 +244,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

src/rtde/rtde_client.cpp

@@ -40,8 +40,9 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
   , output_recipe_(ensureTimestampIsPresent(readRecipe(output_recipe_file)))
   , 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)
@@ -55,8 +56,9 @@ RTDEClient::RTDEClient(std::string robot_ip, comm::INotifier& notifier, const st
   , output_recipe_(ensureTimestampIsPresent(output_recipe))
   , 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)
@@ -83,8 +85,8 @@ bool RTDEClient::init(const size_t max_num_tries, const std::chrono::millisecond
     if (client_state_ == ClientState::INITIALIZED)
       return true;
 
-    URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 10 seconds");
-    std::this_thread::sleep_for(std::chrono::seconds(10));
+    URCL_LOG_ERROR("Failed to initialize RTDE client, retrying in 1 seconds");
+    std::this_thread::sleep_for(std::chrono::seconds(1));
     attempts++;
   }
   std::stringstream ss;
@@ -96,8 +98,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 +153,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 +176,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 +222,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 +251,51 @@ 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_INFO("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,17 +307,33 @@ 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;
       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);
+          const std::string message = "Variable '" + variable_name + "' not recognized by the robot. "
+                                  "Either your output recipe contains errors or the urcontrol version "
+                                  "does not support it. It will be removed from the output recipe.";
+          URCL_LOG_WARN("%s", message.c_str());
+        } 
+        else 
+        {
+          available_variables.push_back(variable_name);
         }
       }
+
+      if (available_variables.size() == output_recipe_.size()) {
+        // All variables are accounted for in the RTDE package
+        return;
+      }
+
+      // Some variables are not available so retry setting up the communication with a stripped-down output recipe
+      resetOutputRecipe(available_variables);
       return;
     }
     else
@@ -339,7 +369,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 +425,7 @@ void RTDEClient::disconnect()
   if (client_state_ > ClientState::UNINITIALIZED)
   {
     sendPause();
-    pipeline_.stop();
+    pipeline_->stop();
     stream_.disconnect();
   }
   client_state_ = ClientState::UNINITIALIZED;
@@ -421,7 +451,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 +481,7 @@ bool RTDEClient::start()
     return false;
   }
 
-  pipeline_.run();
+  pipeline_->run();
 
   if (sendStart())
   {
@@ -501,7 +531,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 +573,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 +635,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)