added handling of protocol-version 1 and CB3 robots

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()
 {