Implemented parser functionality

include/ur_rtde_driver/rtde/rtde_parser.h

@@ -0,0 +1,60 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+#include <vector>
+#include "ur_rtde_driver/comm/parser.h"
+#include "ur_rtde_driver/comm/bin_parser.h"
+#include "ur_rtde_driver/comm/pipeline.h"
+#include "ur_rtde_driver/rtde/package_header.h"
+
+namespace ur_driver
+{
+namespace rtde_interface
+{
+using namespace comm;
+template <typename T>
+class RTDEParser : comm::Parser<rtde_interface::PackageHeader>
+{
+public:
+  bool parse(BinParser& bp, std::vector<std::unique_ptr<PackageHeader>>& results)
+
+  {
+    int32_t packet_size = bp.peek<int32_t>();
+
+    if (!bp.checkSize(packet_size))
+    {
+      LOG_ERROR("Buffer len shorter than expected packet length");
+      return false;
+    }
+
+    bp.parse(packet_size);  // consumes the peeked data
+
+    std::unique_ptr<PackageHeader> packet(new T);
+
+    if (!packet->parseWith(bp))
+      return false;
+
+    results.push_back(std::move(packet));
+
+    return true;
+  }
+};
+
+}  // namespace rtde_interface
+}  // namespace ur_driver