added handling of protocol-version 1 and CB3 robots

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<std::string> variable_names);
 
+  static size_t generateSerializedRequest(uint8_t* buffer, std::vector<std::string> variable_names);
+
   double output_frequency_;
   std::string variable_names_;
 

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

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<PackageHeader> prod_;
   comm::Pipeline<PackageHeader> pipeline_;
 
+  constexpr static const double CB3_MAX_FREQUENCY = 125.0;
+  constexpr static const double URE_MAX_FREQUENCY = 500.0;
+
   std::vector<std::string> readRecipe();
 };
 

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<std::string> 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

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

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<comm::URPackage<PackageHeader>> package;
-
   pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
-  size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, 500.0, readRecipe());
+  rtde_interface::RequestProtocolVersion* tmp_version =
+      dynamic_cast<rtde_interface::RequestProtocolVersion*>(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<rtde_interface::RequestProtocolVersion*>(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<rtde_interface::GetUrcontrolVersion*>(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<comm::URPackage<PackageHeader>> 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<rtde_interface::ControlPackageStart*>(package.get());
+  return tmp->accepted_;
 }
 std::vector<std::string> RTDEClient::readRecipe()
 {