implemented usage of recipe read by rtde client in the parser and the

rtde data package

include/ur_rtde_driver/rtde/data_package.h

@@ -100,6 +100,7 @@ public:
 private:
   // Const would be better here
   static std::unordered_map<std::string, _rtde_type_variant> type_list_;
+  uint8_t recipe_id_;
   std::unordered_map<std::string, _rtde_type_variant> data_;
   std::vector<std::string> recipe_;
 };

include/ur_rtde_driver/rtde/package_header.h

@@ -25,7 +25,7 @@ namespace ur_driver
 {
 namespace rtde_interface
 {
-enum class PackageType
+enum class PackageType : uint8_t
 {
   RTDE_REQUEST_PROTOCOL_VERSION = 86,       // ascii V
   RTDE_GET_URCONTROL_VERSION = 118,         // ascii v
@@ -55,7 +55,7 @@ public:
   static size_t serializeHeader(uint8_t* buffer, PackageType package_type, uint16_t payload_length)
   {
     uint16_t header_size = sizeof(_package_size_type) + sizeof(PackageType);
-    uint16_t size = header_size+payload_length;
+    uint16_t size = header_size + payload_length;
     comm::PackageSerializer::serialize(buffer, size);
     comm::PackageSerializer::serialize(buffer + sizeof(size), package_type);
     return header_size;

include/ur_rtde_driver/rtde/rtde_package.h

@@ -55,6 +55,8 @@ public:
   virtual std::string toString() const;
 
 protected:
+  std::unique_ptr<uint8_t> buffer_;
+  size_t buffer_length_;
   PackageType type_;
 };
 

include/ur_rtde_driver/rtde/rtde_parser.h

@@ -22,21 +22,27 @@
 #include "ur_rtde_driver/comm/bin_parser.h"
 #include "ur_rtde_driver/comm/pipeline.h"
 
-#include "ur_rtde_driver/rtde/package_header.h"
+#include "ur_rtde_driver/rtde/control_package_pause.h"
+#include "ur_rtde_driver/rtde/control_package_setup_inputs.h"
+#include "ur_rtde_driver/rtde/control_package_setup_outputs.h"
+#include "ur_rtde_driver/rtde/control_package_start.h"
 #include "ur_rtde_driver/rtde/data_package.h"
-#include "ur_rtde_driver/rtde/text_message.h"
-#include "ur_rtde_driver/rtde/request_protocol_version.h"
 #include "ur_rtde_driver/rtde/get_urcontrol_version.h"
+#include "ur_rtde_driver/rtde/package_header.h"
+#include "ur_rtde_driver/rtde/request_protocol_version.h"
+#include "ur_rtde_driver/rtde/text_message.h"
 
 namespace ur_driver
 {
 namespace rtde_interface
 {
-template <typename T>
-class RTDEParser : comm::Parser<rtde_interface::PackageHeader>
+class RTDEParser : public comm::Parser<PackageHeader>
 {
 public:
-  RTDEParser() = default;
+  RTDEParser() = delete;
+  RTDEParser(const std::vector<std::string>& recipe) : recipe_(recipe)
+  {
+  }
   virtual ~RTDEParser() = default;
 
   bool parse(comm::BinParser& bp, std::vector<std::unique_ptr<comm::URPackage<PackageHeader>>>& results)
@@ -47,7 +53,7 @@ public:
     bp.parse(size);
     bp.parse(type);
 
-    if (!bp.checkSize(size))
+    if (!bp.checkSize(size - sizeof(size) - sizeof(type)))
     {
       LOG_ERROR("Buffer len shorter than expected packet length");
       return false;
@@ -57,12 +63,11 @@ public:
     {
       case PackageType::RTDE_DATA_PACKAGE:
       {
-        // TODO: get proper recipe here
-        std::vector<std::string> recipe;
-        std::unique_ptr<RTDEPackage> package(new DataPackage(recipe));
+        std::unique_ptr<RTDEPackage> package(new DataPackage(recipe_));
 
         if (!package->parseWith(bp))
         {
+          LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
           return false;
         }
         results.push_back(std::move(package));
@@ -92,6 +97,7 @@ public:
   }
 
 private:
+  std::vector<std::string> recipe_;
   RTDEPackage* package_from_type(PackageType type)
   {
     switch (type)
@@ -105,6 +111,18 @@ private:
       case PackageType::RTDE_REQUEST_PROTOCOL_VERSION:
         return new RequestProtocolVersion;
         break;
+      case PackageType::RTDE_CONTROL_PACKAGE_PAUSE:
+        return new ControlPackagePause;
+        break;
+      case PackageType::RTDE_CONTROL_PACKAGE_SETUP_INPUTS:
+        return new ControlPackageSetupInputs;
+        break;
+      case PackageType::RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS:
+        return new ControlPackageSetupOutputs;
+        break;
+      case PackageType::RTDE_CONTROL_PACKAGE_START:
+        return new ControlPackageStart;
+        break;
       default:
         return new RTDEPackage(type);
     }