From 9bafd44687ee618d154049cf5d022eb6906957d1 Mon Sep 17 00:00:00 2001 From: Tristan Schnell Date: Wed, 17 Apr 2019 16:53:39 +0200 Subject: [PATCH] added handling of protocol-version 1 and CB3 robots --- .../rtde/control_package_setup_outputs.h | 2 + .../rtde/get_urcontrol_version.h | 6 +++ include/ur_rtde_driver/rtde/rtde_client.h | 4 ++ src/rtde/control_package_setup_outputs.cpp | 21 ++++++++ src/rtde/get_urcontrol_version.cpp | 5 ++ src/rtde/rtde_client.cpp | 48 +++++++++++++++++-- 6 files changed, 82 insertions(+), 4 deletions(-) diff --git a/include/ur_rtde_driver/rtde/control_package_setup_outputs.h b/include/ur_rtde_driver/rtde/control_package_setup_outputs.h index 510b91f..c32ba21 100644 --- a/include/ur_rtde_driver/rtde/control_package_setup_outputs.h +++ b/include/ur_rtde_driver/rtde/control_package_setup_outputs.h @@ -57,6 +57,8 @@ public: static size_t generateSerializedRequest(uint8_t* buffer, double output_frequency, std::vector variable_names); + static size_t generateSerializedRequest(uint8_t* buffer, std::vector variable_names); + double output_frequency_; std::string variable_names_; diff --git a/include/ur_rtde_driver/rtde/get_urcontrol_version.h b/include/ur_rtde_driver/rtde/get_urcontrol_version.h index ef87ec9..a562ce1 100644 --- a/include/ur_rtde_driver/rtde/get_urcontrol_version.h +++ b/include/ur_rtde_driver/rtde/get_urcontrol_version.h @@ -54,6 +54,12 @@ class GetUrcontrolVersionRequest : public RTDEPackage public: GetUrcontrolVersionRequest() = default; virtual ~GetUrcontrolVersionRequest() = default; + + static size_t generateSerializedRequest(uint8_t* buffer); + +private: + static const uint16_t PAYLOAD_SIZE = 0; + static const PackageType PACKAGE_TYPE = PackageType::RTDE_GET_URCONTROL_VERSION; }; } // namespace rtde_interface diff --git a/include/ur_rtde_driver/rtde/rtde_client.h b/include/ur_rtde_driver/rtde/rtde_client.h index 6a917e8..98fb1ac 100644 --- a/include/ur_rtde_driver/rtde/rtde_client.h +++ b/include/ur_rtde_driver/rtde/rtde_client.h @@ -38,6 +38,7 @@ #include "ur_rtde_driver/rtde/request_protocol_version.h" #include "ur_rtde_driver/rtde/control_package_setup_outputs.h" #include "ur_rtde_driver/rtde/control_package_start.h" +#include "ur_rtde_driver/log.h" static const int UR_RTDE_PORT = 30004; static const std::string PIPELINE_NAME = "RTDE Data Pipeline"; @@ -62,6 +63,9 @@ private: comm::URProducer prod_; comm::Pipeline pipeline_; + constexpr static const double CB3_MAX_FREQUENCY = 125.0; + constexpr static const double URE_MAX_FREQUENCY = 500.0; + std::vector readRecipe(); }; diff --git a/src/rtde/control_package_setup_outputs.cpp b/src/rtde/control_package_setup_outputs.cpp index 2ad6f06..03f4cea 100644 --- a/src/rtde/control_package_setup_outputs.cpp +++ b/src/rtde/control_package_setup_outputs.cpp @@ -67,5 +67,26 @@ size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buf return size; } + +size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer, + std::vector variable_names) +{ + if (variable_names.size() == 0) + { + return 0; + } + std::string variables; + for (const auto& piece : variable_names) + variables += (piece + ","); + variables.pop_back(); + uint16_t payload_size = sizeof(double) + variables.size(); + + size_t size = 0; + size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, payload_size); + size += comm::PackageSerializer::serialize(buffer + size, variables); + + return size; +} + } // namespace rtde_interface } // namespace ur_driver diff --git a/src/rtde/get_urcontrol_version.cpp b/src/rtde/get_urcontrol_version.cpp index dad4aed..586662f 100644 --- a/src/rtde/get_urcontrol_version.cpp +++ b/src/rtde/get_urcontrol_version.cpp @@ -48,5 +48,10 @@ std::string GetUrcontrolVersion::toString() const return ss.str(); } + +size_t GetUrcontrolVersionRequest::generateSerializedRequest(uint8_t* buffer) +{ + return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE); +} } // namespace rtde_interface } // namespace ur_driver diff --git a/src/rtde/rtde_client.cpp b/src/rtde/rtde_client.cpp index a0c5f97..e5f5c65 100644 --- a/src/rtde/rtde_client.cpp +++ b/src/rtde/rtde_client.cpp @@ -45,12 +45,50 @@ bool RTDEClient::init() uint8_t buffer[4096]; size_t size; size_t written; - size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, 2); + // negotiate version + uint16_t protocol_version = 2; + size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); stream_.write(buffer, size, written); std::unique_ptr> package; - pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); - size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, 500.0, readRecipe()); + rtde_interface::RequestProtocolVersion* tmp_version = + dynamic_cast(package.get()); + if (!tmp_version->accepted_) + { + protocol_version = 1; + size = RequestProtocolVersionRequest::generateSerializedRequest(buffer, protocol_version); + stream_.write(buffer, size, written); + pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); + tmp_version = dynamic_cast(package.get()); + if (!tmp_version->accepted_) + { + LOG_ERROR("Could not negotiate protocol version"); + return false; + } + } + + // determine maximum frequency from ur-control version + double max_frequency = URE_MAX_FREQUENCY; + size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer); + stream_.write(buffer, size, written); + pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); + rtde_interface::GetUrcontrolVersion* tmp_control_version = + dynamic_cast(package.get()); + if (tmp_control_version->major_ < 5) + { + max_frequency = CB3_MAX_FREQUENCY; + } + + // sending output recipe + LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency); + if (protocol_version == 2) + { + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency, readRecipe()); + } + else + { + size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, readRecipe()); + } stream_.write(buffer, size, written); return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); } @@ -62,7 +100,9 @@ bool RTDEClient::start() size = ControlPackageStartRequest::generateSerializedRequest(buffer); std::unique_ptr> package; stream_.write(buffer, size, written); - return pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); + pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000)); + rtde_interface::ControlPackageStart* tmp = dynamic_cast(package.get()); + return tmp->accepted_; } std::vector RTDEClient::readRecipe() {