WIP: Rework deserialization

rmw_cyclonedds_cpp/CMakeLists.txt

@@ -61,7 +61,7 @@ add_library(rmw_cyclonedds_cpp
   src/demangle.cpp
   src/deserialization_exception.cpp
   src/Serialization.cpp
-  src/TypeSupport2.cpp)
+  src/TypeSupport2.cpp src/Deserialization.cpp src/Deserialization.hpp src/CDRCursor.hpp src/CDR.hpp)
 
 target_include_directories(rmw_cyclonedds_cpp PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

rmw_cyclonedds_cpp/src/CDR.hpp

@@ -0,0 +1,127 @@
+//
+// Created by Dan Rose on 2019-12-05.
+//
+#ifndef ROS2_MASTER_CDR_HPP
+#define ROS2_MASTER_CDR_HPP
+
+#include <array>
+#include <cstddef>
+#include <cstdint>
+
+
+// https://www.omg.org/spec/DDS-XTypes/1.3/PDF
+#include "bytewise.hpp"
+namespace rmw_cyclonedds_cpp
+{
+enum class EncodingVersion
+{
+  CDR_Legacy,
+  CDR1,
+  CDR2,
+};
+
+enum class EncodingType
+{
+  PLAIN_CDR,
+  PL_CDR,
+  PLAIN_CDR2,
+  PL_CDR2,
+  DELIMIT_CDR,
+  XML,
+};
+
+/// aka ENC_HEADER
+struct EncapsulationHeader
+{
+  /// stream endianness
+  endian m_endian = native_endian();
+  /// encoding type
+  EncodingType m_enc_type;
+  /// encoding options
+  std::array<byte, 2> m_options;
+};
+
+/// aka DHEADER
+struct DelimiterHeaderData
+{
+  uint32_t size;
+};
+
+/// aka LC
+enum class LengthCode
+{
+  Bytes1 = 0,
+  Bytes2 = 1,
+  Bytes4 = 2,
+  Bytes8 = 3,
+  BytesN = 4,
+  BytesN_ = 5,
+  Bytes4N = 6,
+  Byten8N = 7,
+};
+
+/// aka EMHEADER
+struct MemberHeader
+{
+  bool must_understand = false;
+  LengthCode length_code;
+  uint32_t next_int;
+};
+
+class CDREncodingInfo
+{
+  EncodingVersion m_version;
+
+public:
+  explicit CDREncodingInfo(EncodingVersion version) {m_version = version;}
+
+  size_t max_align() const
+  {
+    switch (m_version) {
+      case EncodingVersion::CDR_Legacy:
+      case EncodingVersion::CDR1:
+        return 8;
+      case EncodingVersion::CDR2:
+        return 4;
+    }
+  }
+
+  size_t get_size_of_primitive(ROSIDL_TypeKind tk) const
+  {
+    /// return 0 if the value type is not primitive
+    /// else returns the number of bytes it should serialize to
+    switch (tk) {
+      case ROSIDL_TypeKind::BOOLEAN:
+      case ROSIDL_TypeKind::OCTET:
+      case ROSIDL_TypeKind::UINT8:
+      case ROSIDL_TypeKind::INT8:
+      case ROSIDL_TypeKind::CHAR:
+        return 1;
+      case ROSIDL_TypeKind::UINT16:
+      case ROSIDL_TypeKind::INT16:
+      case ROSIDL_TypeKind::WCHAR:
+        return 2;
+      case ROSIDL_TypeKind::UINT32:
+      case ROSIDL_TypeKind::INT32:
+      case ROSIDL_TypeKind::FLOAT:
+        return 4;
+      case ROSIDL_TypeKind::UINT64:
+      case ROSIDL_TypeKind::INT64:
+      case ROSIDL_TypeKind::DOUBLE:
+        return 8;
+      case ROSIDL_TypeKind::LONG_DOUBLE:
+        return 16;
+      default:
+        return 0;
+    }
+  }
+
+  size_t get_align_of_primitive(ROSIDL_TypeKind tk) const
+  {
+    size_t sizeof_ = get_size_of_primitive(tk);
+    return std::min(sizeof_, max_align());
+  }
+};
+}  // namespace rmw_cyclonedds_cpp
+
+#endif  //ROS2_MASTER_CDR_HPP

rmw_cyclonedds_cpp/src/CDRCursor.hpp

@@ -0,0 +1,130 @@
+//
+// Created by Dan Rose on 2019-12-05.
+//
+
+#ifndef ROS2_MASTER_CDRCURSOR_HPP
+#define ROS2_MASTER_CDRCURSOR_HPP
+namespace rmw_cyclonedds_cpp
+{
+
+struct AbstractCDRCursor
+{
+  AbstractCDRCursor() = default;
+  ~AbstractCDRCursor() = default;
+
+  // don't want to accidentally copy
+  explicit AbstractCDRCursor(AbstractCDRCursor const &) = delete;
+  void operator=(AbstractCDRCursor const & x) = delete;
+
+  virtual endian stream_endian() const = 0;
+
+  // virtual functions to be implemented
+  // get the cursor's current offset.
+  virtual size_t offset() const = 0;
+  // advance the cursor.
+  virtual void advance(size_t n_bytes) = 0;
+  // Move the logical origin this many places
+  virtual void rebase(ptrdiff_t relative_origin) = 0;
+
+  // Does this cursor yield real data?
+  virtual bool ignores_data() const = 0;
+
+  void align(size_t n_bytes)
+  {
+    assert(n_bytes > 0);
+    size_t start_offset = offset();
+    if (n_bytes == 1 || start_offset % n_bytes == 0) {
+      return;
+    }
+    advance(n_bytes - start_offset % n_bytes);
+    assert(offset() - start_offset < n_bytes);
+    assert(offset() % n_bytes == 0);
+  }
+
+  ptrdiff_t operator-(const AbstractCDRCursor & other) const
+  {
+    return static_cast<ptrdiff_t>(offset()) - static_cast<ptrdiff_t>(other.offset());
+  }
+};
+
+struct AbstractCDRWritingCursor : AbstractCDRCursor
+{
+  virtual void put_bytes(const void *, size_t n_bytes) = 0;
+  endian stream_endian() const override {
+    return native_endian();
+  }
+};
+
+struct CDRWritingCursor : public AbstractCDRWritingCursor
+{
+  const void * origin;
+  void * position;
+
+  explicit CDRWritingCursor(void * position)
+    : origin(position), position(position) {}
+
+  size_t offset() const final {return (const byte *)position - (const byte *)origin;}
+  void advance(size_t n_bytes) final
+  {
+    std::memset(position, '\0', n_bytes);
+    position = byte_offset(position, n_bytes);
+  }
+  void put_bytes(const void * bytes, size_t n_bytes) final
+  {
+    if (n_bytes == 0) {
+      return;
+    }
+    std::memcpy(position, bytes, n_bytes);
+    position = byte_offset(position, n_bytes);
+  }
+  bool ignores_data() const final {return false;}
+  void rebase(ptrdiff_t relative_origin) final {origin = byte_offset(origin, relative_origin);}
+};
+
+struct AbstractCDRReadingCursor : AbstractCDRCursor
+{
+  virtual void get_bytes(void *, size_t n_bytes) = 0;
+};
+
+struct CDRDummyWritingCursor : public AbstractCDRWritingCursor
+{
+  CDRDummyWritingCursor()
+    : CDRDummyWritingCursor(0) {}
+  explicit CDRDummyWritingCursor(size_t initial_offset)
+    : m_offset(initial_offset) {}
+  explicit CDRDummyWritingCursor(AbstractCDRCursor & c)
+    : m_offset(c.offset()) {}
+
+  size_t m_offset;
+
+  size_t offset() const final {return m_offset;}
+  void advance(size_t n_bytes) final { m_offset += n_bytes; }
+  void put_bytes(const void *, size_t n_bytes) final { advance(n_bytes); }
+  bool ignores_data() const final { return true; }
+  void rebase(ptrdiff_t relative_origin) override
+  {
+    // we're moving the *origin* so this has to change in the *opposite* direction
+    m_offset -= relative_origin;
+  }
+};
+
+struct CDRReadCursor : public AbstractCDRReadingCursor
+{
+  const void * origin;
+  void * position;
+  explicit CDRReadCursor(void * position) : origin(position), position(position) {}
+  size_t offset() const final { return (const byte *)position - (const byte *)origin; }
+  void advance(size_t n_bytes) final { position = byte_offset(position, n_bytes); }
+  void get_bytes(void * dest, size_t n_bytes) final {
+    if (n_bytes == 0) {
+      return;
+    }
+    std::memcpy(dest, position, n_bytes);
+    position = byte_offset(position, n_bytes);
+  }
+  void rebase(ptrdiff_t relative_origin) final { origin = byte_offset(origin, relative_origin); }
+};
+
+
+}  // namespace rmw_cyclonedds_cpp
+#endif  //ROS2_MASTER_CDRCURSOR_HPP