Skip to content

Commit

Permalink
Retry reading RTDE interface when unexpected messages appear during s… (
Browse files Browse the repository at this point in the history 
UniversalRobots#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.
  • Loading branch information
@fmauch
fmauch authored Jun 12, 2020
1 parent 8d8e909 commit 8e2ee63
Show file tree
Hide file tree
Showing 2 changed files with 172 additions and 70 deletions.
5 changes: 4 additions & 1 deletion ur_robot_driver/include/ur_robot_driver/rtde/rtde_client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down
237 changes: 168 additions & 69 deletions ur_robot_driver/src/rtde/rtde_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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)
{
Expand All @@ -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;
Expand All @@ -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;
Expand Down

0 comments on commit 8e2ee63

Please sign in to comment.