Make the RTDE output recipe adaptation opt-in

Author: remi-siffert-ocado

include/ur_client_library/rtde/rtde_client.h

@@ -103,10 +103,13 @@ class RTDEClient
    * \param notifier The notifier to use in the pipeline
    * \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 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.
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
    */
   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, bool ignore_unavailable_outputs = false,
+             double target_frequency = 0.0);
 
   /*!
    * \brief Creates a new RTDEClient object, including a used URStream and Pipeline to handle the
@@ -116,10 +119,13 @@ class RTDEClient
    * \param notifier The notifier to use in the pipeline
    * \param output_recipe Vector containing the output recipe
    * \param input_recipe Vector containing the input recipe
+   * \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.
    * \param target_frequency Frequency to run at. Defaults to 0.0 which means maximum frequency.
    */
   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, bool ignore_available_outputs = false,
+             double target_frequency = 0.0);
   ~RTDEClient();
   /*!
    * \brief Sets up RTDE communication with the robot. The handshake includes negotiation of the
@@ -208,10 +214,11 @@ class RTDEClient
   {
     return output_recipe_;
   }
-  
+
 private:
   comm::URStream<RTDEPackage> stream_;
   std::vector<std::string> output_recipe_;
+  bool ignore_unavailable_outputs_;
   std::vector<std::string> input_recipe_;
   RTDEParser parser_;
   std::unique_ptr<comm::URProducer<RTDEPackage>> prod_;

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 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.
+   * \param target_frequency
+   * Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
    */
   void resetRTDEClient(const std::vector<std::string>& output_recipe, const std::vector<std::string>& input_recipe,
-                       double target_frequency = 0.0);
+                       bool ignore_unavailable_outputs = false, double target_frequency = 0.0);
 
   /**
    * \brief Reset the RTDE client. As during initialization the driver will start RTDE communication
@@ -620,10 +623,12 @@ 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 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.
    * \param target_frequency Frequency to run the RTDE client at. Defaults to 0.0 which means maximum frequency.
    */
   void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
-                       double target_frequency = 0.0);
+                       bool ignore_unavailable_outputs = false, double target_frequency = 0.0);
 
 private:
   static std::string readScriptFile(const std::string& filename);

src/rtde/rtde_client.cpp

@@ -35,9 +35,10 @@ 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, bool ignore_unavailable_outputs, double target_frequency)
   : 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_(std::make_unique<comm::URProducer<RTDEPackage>>(stream_, parser_))
@@ -51,9 +52,11 @@ 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, bool ignore_unavailable_outputs,
+                       double target_frequency)
   : 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_(std::make_unique<comm::URProducer<RTDEPackage>>(stream_, parser_))
@@ -251,11 +254,12 @@ void RTDEClient::queryURControlVersion()
   throw UrException(ss.str());
 }
 
-void RTDEClient::resetOutputRecipe(const std::vector<std::string> new_recipe) {
+void RTDEClient::resetOutputRecipe(const std::vector<std::string> new_recipe)
+{
   prod_->teardownProducer();
   disconnect();
 
-  output_recipe_.assign(new_recipe.begin(), new_recipe.end());      
+  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);
@@ -308,33 +312,52 @@ 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)
       {
         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")
         {
-          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 
+          unavailable_variables.push_back(variable_name);
+        }
+        else
         {
           available_variables.push_back(variable_name);
         }
       }
 
-      if (available_variables.size() == output_recipe_.size()) {
+      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;
       }
-
-      // Some variables are not available so retry setting up the communication with a stripped-down output recipe
-      resetOutputRecipe(available_variables);
-      return;
     }
     else
     {

src/ur/ur_driver.cpp

@@ -691,18 +691,19 @@ void UrDriver::setKeepaliveCount(const uint32_t count)
 }
 
 void UrDriver::resetRTDEClient(const std::vector<std::string>& output_recipe,
-                               const std::vector<std::string>& input_recipe, double target_frequency)
+                               const std::vector<std::string>& input_recipe, bool ignore_unavailable_outputs,
+                               double target_frequency)
 {
   rtde_client_.reset(
       new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe, input_recipe, target_frequency));
   initRTDE();
 }
 
 void UrDriver::resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
-                               double target_frequency)
+                               bool ignore_unavailable_outputs, double target_frequency)
 {
   rtde_client_.reset(new rtde_interface::RTDEClient(robot_ip_, notifier_, output_recipe_filename, input_recipe_filename,
-                                                    target_frequency));
+                                                    ignore_unavailable_outputs, target_frequency));
   initRTDE();
 }
 

tests/test_rtde_client.cpp

@@ -373,7 +373,9 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist)
 {
   client_->init();
 
-  client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_));
+  // Ignore unknown output variables to account for variables not available in old urcontrol versions.
+  client_.reset(
+      new rtde_interface::RTDEClient(ROBOT_IP, notifier_, exhaustive_output_recipe_file_, input_recipe_file_, true));
 
   EXPECT_NO_THROW(client_->init());
   client_->start();
@@ -394,6 +396,19 @@ TEST_F(RTDEClientTest, check_all_rtde_output_variables_exist)
   client_->pause();
 }
 
+TEST_F(RTDEClientTest, check_unknown_rtde_output_variable)
+{
+  client_->init();
+
+  std::vector<std::string> incorrect_output_recipe = client_->getOutputRecipe();
+  incorrect_output_recipe.push_back("unknown_rtde_variable");
+
+  client_.reset(
+      new rtde_interface::RTDEClient(ROBOT_IP, notifier_, incorrect_output_recipe, resources_input_recipe_, false));
+
+  EXPECT_THROW(client_->init(), UrException);
+}
+
 int main(int argc, char* argv[])
 {
   ::testing::InitGoogleTest(&argc, argv);