implemented serialization of general rtde headers and the request

protocol version package

include/ur_rtde_driver/comm/package_serializer.h

@@ -0,0 +1,84 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- htobeGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// 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.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Tristan Schnell schnell@fzi.de
+ * \date    2019-04-10
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED
+#define UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED
+
+#include <endian.h>
+#include <cstring>
+
+namespace ur_driver
+{
+namespace comm
+{
+class PackageSerializer
+{
+public:
+  template <typename T>
+  static size_t serialize(uint8_t* buffer, T val)
+  {
+    size_t size = sizeof(T);
+    T tmp = encode(val);
+    std::memcpy(buffer, &tmp, size);
+    return size;
+  }
+
+private:
+  template <typename T>
+  static T encode(T val)
+  {
+    return val;
+  }
+  static uint16_t encode(uint16_t val)
+  {
+    return htobe16(val);
+  }
+  static uint32_t encode(uint32_t val)
+  {
+    return htobe32(val);
+  }
+  static uint64_t encode(uint64_t val)
+  {
+    return htobe64(val);
+  }
+  static int16_t encode(int16_t val)
+  {
+    return htobe16(val);
+  }
+  static int32_t encode(int32_t val)
+  {
+    return htobe32(val);
+  }
+  static int64_t encode(int64_t val)
+  {
+    return htobe64(val);
+  }
+};
+
+}  // namespace comm
+}  // namespace ur_driver
+#endif  // UR_RTDE_DRIVER_PACKAGE_SERIALIZER_H_INCLUDED

include/ur_rtde_driver/rtde/package_header.h

@@ -19,6 +19,7 @@
 #include <cstddef>
 #include <endian.h>
 #include "ur_rtde_driver/types.h"
+#include "ur_rtde_driver/comm/package_serializer.h"
 
 namespace ur_driver
 {
@@ -51,6 +52,15 @@ public:
     return be16toh(*(reinterpret_cast<_package_size_type*>(buf)));
   }
 
+  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;
+    comm::PackageSerializer::serialize(buffer, size);
+    comm::PackageSerializer::serialize(buffer + sizeof(size), package_type);
+    return header_size;
+  }
+
 private:
   _package_size_type package_size_;
   PackageType package_type_;

include/ur_rtde_driver/rtde/request_protocol_version.h

@@ -29,6 +29,7 @@
 #define UR_RTDE_DRIVER_REQUEST_PROTOCOL_VERSION_H_INCLUDED
 
 #include "ur_rtde_driver/rtde/rtde_package.h"
+#include "ur_rtde_driver/rtde/package_header.h"
 
 namespace ur_driver
 {
@@ -52,7 +53,13 @@ public:
   RequestProtocolVersionRequest() = default;
   virtual ~RequestProtocolVersionRequest() = default;
 
+  static size_t generateSerializedRequest(uint8_t* buffer, uint16_t version);
+
   uint16_t protocol_version_;
+
+private:
+  static const uint16_t PAYLOAD_SIZE = sizeof(uint16_t);
+  static const PackageType PACKAGE_TYPE = PackageType::RTDE_REQUEST_PROTOCOL_VERSION;
 };
 
 }  // namespace rtde_interface

src/rtde/request_protocol_version.cpp

@@ -39,7 +39,19 @@ bool RequestProtocolVersion::parseWith(comm::BinParser& bp)
 }
 std::string RequestProtocolVersion::toString() const
 {
-  return "accepted: " + accepted_;
+  std::stringstream ss;
+  ss << "accepted: " << static_cast<int>(accepted_);
+  return ss.str();
+}
+
+size_t RequestProtocolVersionRequest::generateSerializedRequest(uint8_t* buffer, uint16_t version)
+{
+  size_t size = 0;
+  size += PackageHeader::serializeHeader(buffer, PACKAGE_TYPE, PAYLOAD_SIZE);
+
+  size += comm::PackageSerializer::serialize(buffer + size, version);
+
+  return size;
 }
 }  // namespace rtde_interface
 }  // namespace ur_driver