Skip to content

Commit

Permalink
Adapt RTDE output recipe based on robot response
Browse files Browse the repository at this point in the history
  • Loading branch information
@remi-siffert-ocado
remi-siffert-ocado committed Oct 31, 2024
1 parent 85fbc55 commit 0a32de2
Show file tree
Hide file tree
Showing 5 changed files with 434 additions and 422 deletions.
4 changes: 4 additions & 0 deletions include/ur_client_library/comm/pipeline.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@ template <typename T>
class IConsumer
{
public:
virtual ~IConsumer() = default;

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

/*!
* \brief Set-up functionality of the producers.
*
Expand Down
17 changes: 14 additions & 3 deletions include/ur_client_library/rtde/rtde_client.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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_;
Expand Down Expand Up @@ -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
Expand Down
114 changes: 72 additions & 42 deletions src/rtde/rtde_client.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -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;
}

Expand All @@ -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();
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -395,7 +425,7 @@ void RTDEClient::disconnect()
if (client_state_ > ClientState::UNINITIALIZED)
{
sendPause();
pipeline_.stop();
pipeline_->stop();
stream_.disconnect();
}
client_state_ = ClientState::UNINITIALIZED;
Expand All @@ -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);
Expand Down Expand Up @@ -451,7 +481,7 @@ bool RTDEClient::start()
return false;
}

pipeline_.run();
pipeline_->run();

if (sendStart())
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand Down
Loading

0 comments on commit 0a32de2

Please sign in to comment.