From 8975957b5f03d9934804bb32f987f0a6e323235f Mon Sep 17 00:00:00 2001
From: Tristan Schnell <schnell@fzi.de>
Date: Thu, 11 Apr 2019 13:57:36 +0200
Subject: [PATCH] implemented serializing of the output setup and control start
 packages

---
 .../ur_rtde_driver/comm/package_serializer.h  | 11 ++++++++++
 .../rtde/control_package_setup_outputs.h      |  7 +++++++
 .../rtde/control_package_start.h              |  6 ++++++
 src/rtde/control_package_setup_outputs.cpp    | 21 +++++++++++++++++++
 src/rtde/control_package_start.cpp            |  5 +++++
 5 files changed, 50 insertions(+)

diff --git a/include/ur_rtde_driver/comm/package_serializer.h b/include/ur_rtde_driver/comm/package_serializer.h
index e885ec1..1acf53a 100644
--- a/include/ur_rtde_driver/comm/package_serializer.h
+++ b/include/ur_rtde_driver/comm/package_serializer.h
@@ -47,6 +47,17 @@ public:
     return size;
   }
 
+  static size_t serialize(uint8_t* buffer, std::string val)
+  {
+    const uint8_t* c_val = reinterpret_cast<const uint8_t*>(val.c_str());
+
+    for (size_t i = 0; i < val.size(); i++)
+    {
+      buffer[i] = c_val[i];
+    }
+    return val.size();
+  }
+
 private:
   template <typename T>
   static T encode(T val)
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 d8e02f1..510b91f 100644
--- a/include/ur_rtde_driver/rtde/control_package_setup_outputs.h
+++ b/include/ur_rtde_driver/rtde/control_package_setup_outputs.h
@@ -29,6 +29,7 @@
 #define UR_RTDE_DRIVER_CONTROL_PACKAGE_SETUP_OUTPUTS_H_INCLUDED
 
 #include "ur_rtde_driver/rtde/rtde_package.h"
+#include "ur_rtde_driver/rtde/package_header.h"
 
 namespace ur_driver
 {
@@ -53,8 +54,14 @@ public:
   ControlPackageSetupOutputsRequest() = default;
   virtual ~ControlPackageSetupOutputsRequest() = default;
 
+  static size_t generateSerializedRequest(uint8_t* buffer, double output_frequency,
+                                          std::vector<std::string> variable_names);
+
   double output_frequency_;
   std::string variable_names_;
+
+private:
+  static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
 };
 
 }  // namespace rtde_interface
diff --git a/include/ur_rtde_driver/rtde/control_package_start.h b/include/ur_rtde_driver/rtde/control_package_start.h
index 2eb9c3e..7eba3ab 100644
--- a/include/ur_rtde_driver/rtde/control_package_start.h
+++ b/include/ur_rtde_driver/rtde/control_package_start.h
@@ -51,6 +51,12 @@ class ControlPackageStartRequest : public RTDEPackage
 public:
   ControlPackageStartRequest() = default;
   virtual ~ControlPackageStartRequest() = default;
+
+  static size_t generateSerializedRequest(uint8_t* buffer);
+
+private:
+  static const uint16_t PAYLOAD_SIZE = 0;
+  static const PackageType PACKAGE_TYPE = PackageType::RTDE_CONTROL_PACKAGE_START;
 };
 
 }  // namespace rtde_interface
diff --git a/src/rtde/control_package_setup_outputs.cpp b/src/rtde/control_package_setup_outputs.cpp
index aa2fc61..2b6f1ba 100644
--- a/src/rtde/control_package_setup_outputs.cpp
+++ b/src/rtde/control_package_setup_outputs.cpp
@@ -46,5 +46,26 @@ std::string ControlPackageSetupOutputs::toString() const
 
   return ss.str();
 }
+
+size_t ControlPackageSetupOutputsRequest::generateSerializedRequest(uint8_t* buffer, double output_frequency,
+                                                                    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, output_frequency);
+  size += comm::PackageSerializer::serialize(buffer + size, variables);
+
+  return size;
+}
 }  // namespace rtde_interface
 }  // namespace ur_driver
diff --git a/src/rtde/control_package_start.cpp b/src/rtde/control_package_start.cpp
index 2202192..536412b 100644
--- a/src/rtde/control_package_start.cpp
+++ b/src/rtde/control_package_start.cpp
@@ -44,5 +44,10 @@ std::string ControlPackageStart::toString() const
 
   return ss.str();
 }
+
+size_t ControlPackageStartRequest::generateSerializedRequest(uint8_t* buffer)
+{
+  return PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
+}
 }  // namespace rtde_interface
 }  // namespace ur_driver