Retry reading RTDE interface when unexpected messages appear during s… (#186)

* Retry reading RTDE interface when unexpected messages appear during startup

At startup we make a couple of requests to the RTDE interface. If the interface
publishes messages by itself, a simple read() from the interface might grab another
message than the answer.

This change checks whether an answer to our requests was received and reports
a warning otherwise while retrying.

Author: fmauch

ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h

@@ -48,6 +48,9 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
+static const uint16_t MAX_RTDE_PROTOCOL_VERSION = 2;
+static const unsigned MAX_REQUEST_RETRIES = 5;
+
 enum class UrRtdeRobotStatusBits
 {
   IS_POWER_ON = 0,
@@ -166,7 +169,7 @@ class RTDEClient
 
   std::vector<std::string> readRecipe(const std::string& recipe_file);
 
-  uint16_t negotiateProtocolVersion();
+  bool negotiateProtocolVersion(const uint16_t protocol_version);
   void queryURControlVersion();
   void setupOutputs(const uint16_t protocol_version);
   void setupInputs();

ur_robot_driver/src/rtde/rtde_client.cpp

@@ -51,7 +51,18 @@ bool RTDEClient::init()
   pipeline_.init();
   pipeline_.run();
 
-  uint16_t protocol_version = negotiateProtocolVersion();
+  uint16_t protocol_version = MAX_RTDE_PROTOCOL_VERSION;
+  while (!negotiateProtocolVersion(protocol_version))
+  {
+    LOG_INFO("Robot did not accept RTDE protocol version '%hu'. Trying lower protocol version", protocol_version);
+    protocol_version--;
+    if (protocol_version == 0)
+    {
+      throw UrException("Protocol version for RTDE communication could not be established. Robot didn't accept any of "
+                        "the suggested versions.");
+    }
+  }
+  LOG_INFO("Negotiated RTDE protocol version to %hu.", protocol_version);
   parser_.setProtocolVersion(protocol_version);
 
   queryURControlVersion();
@@ -70,64 +81,93 @@ bool RTDEClient::init()
   return true;
 }
 
-uint16_t RTDEClient::negotiateProtocolVersion()
+bool RTDEClient::negotiateProtocolVersion(const uint16_t protocol_version)
 {
+  static unsigned num_retries = 0;
   uint8_t buffer[4096];
   size_t size;
   size_t written;
-  uint16_t protocol_version = 2;
   size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
   if (!stream_.write(buffer, size, written))
     throw UrException("Sending protocol version query to robot failed.");
+
   std::unique_ptr<RTDEPackage> package;
-  if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
-    throw UrException("Could not get urcontrol version from robot. This should not happen!");
-  rtde_interface::RequestProtocolVersion* tmp_version =
-      dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
-  if (!tmp_version->accepted_)
+  while (num_retries < MAX_REQUEST_RETRIES)
   {
-    protocol_version = 1;
-    size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version);
-    if (!stream_.write(buffer, size, written))
-      throw UrException("Sending protocol version query to robot failed.");
     if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
-      throw UrException("Could not get urcontrol version from robot. This should not happen!");
-    tmp_version = dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get());
-    if (!tmp_version->accepted_)
     {
-      throw UrException("Neither protocol version 1 nor 2 was accepted by the robot. This should not happen!");
+      throw UrException("No answer to RTDE protocol version negotiation request was received from robot. This should "
+                        "not "
+                        "happen!");
+    }
+
+    if (rtde_interface::RequestProtocolVersion* tmp_version =
+            dynamic_cast<rtde_interface::RequestProtocolVersion*>(package.get()))
+    {
+      // Reset the num_tries variable in case we have to try with another protocol version.
+      num_retries = 0;
+      return tmp_version->accepted_;
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl
+         << package->toString() << ". Retrying...";
+      num_retries++;
+      LOG_WARN("%s", ss.str().c_str());
     }
   }
-  return protocol_version;
+  std::stringstream ss;
+  ss << "Could not negotiate RTDE protocol version after " << MAX_REQUEST_RETRIES
+     << " tries. Please check the output of the "
+        "negotiation attempts above to get a hint what could be wrong.";
+  throw UrException(ss.str());
 }
 
 void RTDEClient::queryURControlVersion()
 {
+  static unsigned num_retries = 0;
   uint8_t buffer[4096];
   size_t size;
   size_t written;
-  std::unique_ptr<RTDEPackage> package;
   size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer);
   if (!stream_.write(buffer, size, written))
     throw UrException("Sending urcontrol version query request to robot failed.");
-  if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
-    throw UrException("Could not get urcontrol version from robot. This should not happen!");
-  rtde_interface::GetUrcontrolVersion* tmp_urcontrol_version =
-      dynamic_cast<rtde_interface::GetUrcontrolVersion*>(package.get());
 
-  if (tmp_urcontrol_version == nullptr)
+  std::unique_ptr<RTDEPackage> package;
+  while (num_retries < MAX_REQUEST_RETRIES)
   {
-    throw UrException("Could not get urcontrol version from robot. This should not happen!");
+    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+      throw UrException("No answer to urcontrol version query was received from robot. This should not happen!");
+
+    if (rtde_interface::GetUrcontrolVersion* tmp_urcontrol_version =
+            dynamic_cast<rtde_interface::GetUrcontrolVersion*>(package.get()))
+    {
+      urcontrol_version_ = tmp_urcontrol_version->version_information_;
+      return;
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "Did not receive protocol negotiation answer from robot. Message received instead: " << std::endl
+         << package->toString() << ". Retrying...";
+      num_retries++;
+      LOG_WARN("%s", ss.str().c_str());
+    }
   }
-  urcontrol_version_ = tmp_urcontrol_version->version_information_;
+  std::stringstream ss;
+  ss << "Could not query urcontrol version after " << MAX_REQUEST_RETRIES
+     << " tries. Please check the output of the "
+        "negotiation attempts above to get a hint what could be wrong.";
+  throw UrException(ss.str());
 }
 
 void RTDEClient::setupOutputs(const uint16_t protocol_version)
 {
+  static unsigned num_retries = 0;
   size_t size;
   size_t written;
   uint8_t buffer[4096];
-  std::unique_ptr<RTDEPackage> package;
   LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency_);
   if (protocol_version == 2)
   {
@@ -141,71 +181,112 @@ void RTDEClient::setupOutputs(const uint16_t protocol_version)
   // Send output recipe to robot
   if (!stream_.write(buffer, size, written))
     throw UrException("Could not send RTDE output recipe to robot.");
-  if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+
+  std::unique_ptr<RTDEPackage> package;
+  while (num_retries < MAX_REQUEST_RETRIES)
   {
-    throw UrException("Did not receive confirmation on RTDE output recipe.");
-  }
+    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+    {
+      throw UrException("Did not receive confirmation on RTDE output recipe.");
+    }
 
-  rtde_interface::ControlPackageSetupOutputs* tmp_output =
-      dynamic_cast<rtde_interface::ControlPackageSetupOutputs*>(package.get());
+    if (rtde_interface::ControlPackageSetupOutputs* tmp_output =
+            dynamic_cast<rtde_interface::ControlPackageSetupOutputs*>(package.get()))
 
-  std::vector<std::string> variable_types = splitVariableTypes(tmp_output->variable_types_);
-  assert(output_recipe_.size() == variable_types.size());
-  for (std::size_t i = 0; i < variable_types.size(); ++i)
-  {
-    LOG_DEBUG("%s confirmed as datatype: %s", output_recipe_[i].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);
+      std::vector<std::string> variable_types = splitVariableTypes(tmp_output->variable_types_);
+      assert(output_recipe_.size() == variable_types.size());
+      for (std::size_t i = 0; i < variable_types.size(); ++i)
+      {
+        LOG_DEBUG("%s confirmed as datatype: %s", output_recipe_[i].c_str(), variable_types[i].c_str());
+        return;
+        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);
+        }
+      }
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "Did not receive answer to RTDE output setup. Message received instead: " << std::endl
+         << package->toString() << ". Retrying...";
+      num_retries++;
+      LOG_WARN("%s", ss.str().c_str());
     }
   }
+  std::stringstream ss;
+  ss << "Could not setup RTDE outputs after " << MAX_REQUEST_RETRIES
+     << " tries. Please check the output of the "
+        "negotiation attempts above to get a hint what could be wrong.";
+  throw UrException(ss.str());
 }
 
 void RTDEClient::setupInputs()
 {
+  static unsigned num_retries = 0;
   size_t size;
   size_t written;
   uint8_t buffer[4096];
-  std::unique_ptr<RTDEPackage> package;
   size = ControlPackageSetupInputsRequest::generateSerializedRequest(buffer, input_recipe_);
   if (!stream_.write(buffer, size, written))
     throw UrException("Could not send RTDE input recipe to robot.");
-  if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
-    throw UrException("Did not receive confirmation on RTDE input recipe.");
-  rtde_interface::ControlPackageSetupInputs* tmp_input =
-      dynamic_cast<rtde_interface::ControlPackageSetupInputs*>(package.get());
-  if (tmp_input == nullptr)
-  {
-    throw UrException("Could not setup RTDE inputs.");
-  }
 
-  std::vector<std::string> variable_types = splitVariableTypes(tmp_input->variable_types_);
-  assert(input_recipe_.size() == variable_types.size());
-  for (std::size_t i = 0; i < variable_types.size(); ++i)
+  std::unique_ptr<RTDEPackage> package;
+  while (num_retries < MAX_REQUEST_RETRIES)
   {
-    LOG_DEBUG("%s confirmed as datatype: %s", input_recipe_[i].c_str(), variable_types[i].c_str());
-    if (variable_types[i] == "NOT_FOUND")
+    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+      throw UrException("Did not receive confirmation on RTDE input recipe.");
+
+    if (rtde_interface::ControlPackageSetupInputs* tmp_input =
+            dynamic_cast<rtde_interface::ControlPackageSetupInputs*>(package.get()))
+
     {
-      std::string message =
-          "Variable '" + input_recipe_[i] + "' not recognized by the robot. Probably your input recipe contains errors";
-      throw UrException(message);
+      std::vector<std::string> variable_types = splitVariableTypes(tmp_input->variable_types_);
+      assert(input_recipe_.size() == variable_types.size());
+      for (std::size_t i = 0; i < variable_types.size(); ++i)
+      {
+        LOG_DEBUG("%s confirmed as datatype: %s", input_recipe_[i].c_str(), variable_types[i].c_str());
+        if (variable_types[i] == "NOT_FOUND")
+        {
+          std::string message = "Variable '" + input_recipe_[i] +
+                                "' not recognized by the robot. Probably your input recipe contains errors";
+          throw UrException(message);
+        }
+        else if (variable_types[i] == "IN_USE")
+        {
+          std::string message = "Variable '" + input_recipe_[i] +
+                                "' is currently controlled by another RTDE client. The input recipe can't be used as "
+                                "configured";
+          throw UrException(message);
+        }
+      }
+
+      writer_.init(tmp_input->input_recipe_id_);
+
+      return;
     }
-    else if (variable_types[i] == "IN_USE")
+    else
     {
-      std::string message = "Variable '" + input_recipe_[i] +
-                            "' is currently controlled by another RTDE client. The input recipe can't be used as "
-                            "configured";
-      throw UrException(message);
+      std::stringstream ss;
+      ss << "Did not receive answer to RTDE input setup. Message received instead: " << std::endl
+         << package->toString() << ". Retrying...";
+      num_retries++;
+      LOG_WARN("%s", ss.str().c_str());
     }
   }
-
-  writer_.init(tmp_input->input_recipe_id_);
+  std::stringstream ss;
+  ss << "Could not setup RTDE inputs after " << MAX_REQUEST_RETRIES
+     << " tries. Please check the output of the "
+        "negotiation attempts above to get a hint what could be wrong.";
+  throw UrException(ss.str());
 }
 
 bool RTDEClient::start()
 {
+  static unsigned num_retries = 0;
   uint8_t buffer[4096];
   size_t size;
   size_t written;
@@ -214,12 +295,30 @@ bool RTDEClient::start()
   std::unique_ptr<RTDEPackage> package;
   if (!stream_.write(buffer, size, written))
     throw UrException("Sending RTDE start command failed!");
-  if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
-    throw UrException("Could not get response to RTDE communication start request from robot. This should not "
-                      "happen!");
-  rtde_interface::ControlPackageStart* tmp = dynamic_cast<rtde_interface::ControlPackageStart*>(package.get());
-  return tmp->accepted_;
+  while (num_retries < MAX_REQUEST_RETRIES)
+  {
+    if (!pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)))
+      throw UrException("Could not get response to RTDE communication start request from robot. This should not "
+                        "happen!");
+    if (rtde_interface::ControlPackageStart* tmp = dynamic_cast<rtde_interface::ControlPackageStart*>(package.get()))
+    {
+      return tmp->accepted_;
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "Did not receive answer to RTDE start request. Message received instead: " << std::endl
+         << package->toString() << ". Retrying...";
+      LOG_WARN("%s", ss.str().c_str());
+    }
+  }
+  std::stringstream ss;
+  ss << "Could not start RTDE communication after " << MAX_REQUEST_RETRIES
+     << " tries. Please check the output of the "
+        "negotiation attempts above to get a hint what could be wrong.";
+  throw UrException(ss.str());
 }
+
 std::vector<std::string> RTDEClient::readRecipe(const std::string& recipe_file)
 {
   std::vector<std::string> recipe;