diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt index 480f1ac9..60554956 100644 --- a/rmw_cyclonedds_cpp/CMakeLists.txt +++ b/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 $ diff --git a/rmw_cyclonedds_cpp/src/CDR.hpp b/rmw_cyclonedds_cpp/src/CDR.hpp new file mode 100644 index 00000000..cdc2982f --- /dev/null +++ b/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 +#include +#include + + +// 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 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 diff --git a/rmw_cyclonedds_cpp/src/CDRCursor.hpp b/rmw_cyclonedds_cpp/src/CDRCursor.hpp new file mode 100644 index 00000000..503152f0 --- /dev/null +++ b/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(offset()) - static_cast(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 diff --git a/rmw_cyclonedds_cpp/src/Deserialization.cpp b/rmw_cyclonedds_cpp/src/Deserialization.cpp new file mode 100644 index 00000000..2d4156e4 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/Deserialization.cpp @@ -0,0 +1,132 @@ +// Copyright 2019 Rover Robotics via Dan Rose +// +// 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. +#include "Deserialization.hpp" + +#include "CDR.hpp" +#include "CDRCursor.hpp" + +namespace rmw_cyclonedds_cpp +{ +class CDRReader : public AbstractCDRReader +{ +protected: + std::unique_ptr m_value_type; + CDREncodingInfo m_cdr; + +public: + explicit CDRReader(std::unique_ptr value_type) + : m_cdr{EncodingVersion::CDR_Legacy}, + m_value_type{std::move(value_type)} + {} + + void deserialize_top_level( + void * destination_object, const void * data, const StructValueType * ts) + { + + } + + uint32_t read_u32(CDRReadCursor * data_cursor) + { + uint32_t result; + data_cursor->align(m_cdr.get_align_of_primitive(ROSIDL_TypeKind::UINT32)); + // todo: handle endianness if needed + data_cursor->get_bytes(&result, m_cdr.get_size_of_primitive(ROSIDL_TypeKind::UINT32)); + return result; + } + + void deserialize( + void * destination_object, const PrimitiveValueType & vt, CDRReadCursor * data_cursor) + { + data_cursor->align(m_cdr.get_align_of_primitive(vt.type_kind())); + // todo: handle endianness if needed + data_cursor->get_bytes(destination_object, m_cdr.get_size_of_primitive(vt.type_kind())); + } + + void deserialize( + void * destination_object, const U8StringValueType & vt, CDRReadCursor * data_cursor) + { + size_t size = read_u32(data_cursor); + vt.assign(destination_object, static_cast(data_cursor->position), size); + } + + void deserialize( + void * destination_object, const U16StringValueType & vt, CDRReadCursor * data_cursor) + { + size_t size = read_u32(data_cursor); + // todo: handle endianness if needed + vt.assign(destination_object, static_cast(data_cursor->position), size); + } + + void deserialize( + void * destination_object, const ArrayValueType & vt, + CDRReadCursor * data_cursor) + { + for (size_t i = 0; i < vt.array_size(); i++) { + void * dest = byte_offset(destination_object, i * vt.element_value_type()->sizeof_type()); + deserialize(dest, vt.element_value_type(), data_cursor); + } + } + + void deserialize( + void * destination_object, const SpanSequenceValueType & vt, CDRReadCursor * data_cursor) + { + size_t size = read_u32(data_cursor); + vt.resize(destination_object, size); + void * sequence_contents = vt.sequence_contents(destination_object); + for (size_t i = 0; i < size; i++) { + void * dest = byte_offset(sequence_contents, i * vt.element_value_type()->sizeof_type()); + deserialize(dest, vt.element_value_type(), data_cursor); + } + } + + void deserialize( + void * destination_object, const StructValueType & vt, CDRReadCursor * data_cursor) + { + for (size_t i = 0; i < vt.n_members(); i++) { + auto member = vt.get_member(i); + auto member_object = byte_offset(destination_object, member->member_offset); + deserialize(member_object, member->value_type, data_cursor); + } + } + + void deserialize(void * destination_object, const AnyValueType * vt, CDRReadCursor * data_cursor) + { + switch (vt->e_value_type()) { + case EValueType::PrimitiveValueType: + deserialize(destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::U8StringValueType: + deserialize(destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::U16StringValueType: + deserialize(destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::StructValueType: + deserialize(destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::ArrayValueType: + deserialize(destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::SpanSequenceValueType: + deserialize( + destination_object, *static_cast(vt), data_cursor); + break; + case EValueType::BoolVectorValueType: + // todo + throw std::exception(); + break; + } + } +}; +} // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/src/Deserialization.hpp b/rmw_cyclonedds_cpp/src/Deserialization.hpp new file mode 100644 index 00000000..4be94d60 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/Deserialization.hpp @@ -0,0 +1,52 @@ +// Copyright 2019 Rover Robotics via Dan Rose +// +// 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. +#ifndef ROS2_MASTER_DESERIALIZATION_HPP +#define ROS2_MASTER_DESERIALIZATION_HPP + +#include "CDR.hpp" +#include "TypeSupport2.hpp" +#include "serdata.hpp" +namespace rmw_cyclonedds_cpp +{ + +void deserialize_top_level(void * destination_object, const void * data, const StructValueType * ts) +{ + EncapsulationHeader enc_hdr; + auto bdata = static_cast(data); + uint16_t benc_hdr = (((uint16_t)data[0])<<8) | data[1]; + enc_hdr.m_endian = (enc_hdr & 1) ? little_endian : big_endian; + switch(benc_hdr ) { + case 0x0000: + enc_hdr.m_endian + } + if (bdata[0] == 1) + enc_hdr.m_endian = + // todo: create deserialization stream using the encapsulation header (endian, eversion) + // todo: pull data out of that stream, structurally recursing on data and types +} + +class AbstractCDRReader +{ +public: + virtual size_t get_serialized_size(const void * data) const = 0; + virtual void serialize(void * dest, const void * data) const = 0; + virtual size_t get_serialized_size(const cdds_request_wrapper_t & request) const = 0; + virtual void serialize(void * dest, const cdds_request_wrapper_t & request) const = 0; + virtual ~AbstractCDRReader() = default; +}; + +std::unique_ptr make_cdr_reader(std::unique_ptr value_type); + +} +#endif //ROS2_MASTER_DESERIALIZATION_HPP diff --git a/rmw_cyclonedds_cpp/src/Serialization.cpp b/rmw_cyclonedds_cpp/src/Serialization.cpp index 8263c58c..e44bd424 100644 --- a/rmw_cyclonedds_cpp/src/Serialization.cpp +++ b/rmw_cyclonedds_cpp/src/Serialization.cpp @@ -27,103 +27,15 @@ #include #include +#include "CDR.hpp" +#include "CDRCursor.hpp" #include "TypeSupport2.hpp" #include "bytewise.hpp" namespace rmw_cyclonedds_cpp { -struct CDRCursor -{ - CDRCursor() = default; - ~CDRCursor() = default; - - // don't want to accidentally copy - explicit CDRCursor(CDRCursor const &) = delete; - void operator=(CDRCursor const & x) = delete; - - // 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; - // Copy bytes to the current cursor location (if needed) and advance the cursor - virtual void put_bytes(const void * data, size_t size) = 0; - virtual bool ignores_data() const = 0; - // Move the logical origin this many places - virtual void rebase(ptrdiff_t relative_origin) = 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 CDRCursor & other) const - { - return static_cast(offset()) - static_cast(other.offset()); - } -}; - -struct SizeCursor : public CDRCursor -{ - SizeCursor() - : SizeCursor(0) {} - explicit SizeCursor(size_t initial_offset) - : m_offset(initial_offset) {} - explicit SizeCursor(CDRCursor & 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 DataCursor : public CDRCursor -{ - const void * origin; - void * position; - - explicit DataCursor(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);} -}; - -enum class EncodingVersion -{ - CDR_Legacy, - CDR1, -}; - -class CDRWriter : public BaseCDRWriter +class CDRWriter : public AbstractCDRWriter { public: struct CacheKey @@ -208,7 +120,7 @@ class CDRWriter : public BaseCDRWriter } size_t get_serialized_size(const void * data) const override { - SizeCursor cursor; + CDRDummyWritingCursor cursor; serialize_top_level(&cursor, data); return cursor.offset(); @@ -216,14 +128,14 @@ class CDRWriter : public BaseCDRWriter void serialize(void * dest, const void * data) const override { - DataCursor cursor(dest); + CDRWritingCursor cursor(dest); serialize_top_level(&cursor, data); } size_t get_serialized_size( const cdds_request_wrapper_t & request) const override { - SizeCursor cursor; + CDRDummyWritingCursor cursor; serialize_top_level(&cursor, request); return cursor.offset(); } @@ -231,12 +143,11 @@ class CDRWriter : public BaseCDRWriter void serialize( void * dest, const cdds_request_wrapper_t & request) const override { - DataCursor cursor(dest); + CDRWritingCursor cursor(dest); serialize_top_level(&cursor, request); } - void serialize_top_level( - CDRCursor * cursor, const void * data) const + void serialize_top_level(AbstractCDRWritingCursor * cursor, const void * data) const { put_rtps_header(cursor); @@ -257,7 +168,7 @@ class CDRWriter : public BaseCDRWriter } void serialize_top_level( - CDRCursor * cursor, const cdds_request_wrapper_t & request) const + AbstractCDRWritingCursor * cursor, const cdds_request_wrapper_t & request) const { put_rtps_header(cursor); if (eversion == EncodingVersion::CDR_Legacy) { @@ -274,7 +185,7 @@ class CDRWriter : public BaseCDRWriter } protected: - void put_rtps_header(CDRCursor * cursor) const + void put_rtps_header(AbstractCDRWritingCursor * cursor) const { // beginning of message char eversion_byte; @@ -296,7 +207,7 @@ class CDRWriter : public BaseCDRWriter cursor->put_bytes(rtps_header.data(), rtps_header.size()); } - void serialize_u32(CDRCursor * cursor, size_t value) const + void serialize_u32(AbstractCDRWritingCursor * cursor, size_t value) const { assert(value <= std::numeric_limits::max()); auto u32_value = static_cast(value); @@ -427,7 +338,9 @@ class CDRWriter : public BaseCDRWriter return sizeof_ < max_align ? sizeof_ : max_align; } - void serialize(CDRCursor * cursor, const void * data, const PrimitiveValueType & value_type) const + void serialize( + AbstractCDRWritingCursor * cursor, const void * data, + const PrimitiveValueType & value_type) const { cursor->align(get_cdr_alignof_primitive(value_type.type_kind())); size_t n_bytes = get_cdr_size_of_primitive(value_type.type_kind()); @@ -472,7 +385,9 @@ class CDRWriter : public BaseCDRWriter } } - void serialize(CDRCursor * cursor, const void * data, const U8StringValueType & value_type) const + void serialize( + AbstractCDRWritingCursor * cursor, const void * data, + const U8StringValueType & value_type) const { auto str = value_type.data(data); serialize_u32(cursor, str.size() + 1); @@ -481,7 +396,9 @@ class CDRWriter : public BaseCDRWriter cursor->put_bytes(&terminator, 1); } - void serialize(CDRCursor * cursor, const void * data, const U16StringValueType & value_type) const + void serialize( + AbstractCDRWritingCursor * cursor, const void * data, + const U16StringValueType & value_type) const { auto str = value_type.data(data); if (eversion == EncodingVersion::CDR_Legacy) { @@ -499,14 +416,15 @@ class CDRWriter : public BaseCDRWriter } } - void serialize(CDRCursor * cursor, const void * data, const ArrayValueType & value_type) const + void serialize( + AbstractCDRWritingCursor * cursor, const void * data, const ArrayValueType & value_type) const { serialize_many( cursor, value_type.get_data(data), value_type.array_size(), value_type.element_value_type()); } void serialize( - CDRCursor * cursor, const void * data, + AbstractCDRWritingCursor * cursor, const void * data, const SpanSequenceValueType & value_type) const { size_t count = value_type.sequence_size(data); @@ -516,7 +434,7 @@ class CDRWriter : public BaseCDRWriter } void serialize( - CDRCursor * cursor, const void * data, + AbstractCDRWritingCursor * cursor, const void * data, const BoolVectorValueType & value_type) const { size_t count = value_type.size(data); @@ -531,7 +449,8 @@ class CDRWriter : public BaseCDRWriter } } - void serialize(CDRCursor * cursor, const void * data, const AnyValueType * value_type) const + void serialize( + AbstractCDRWritingCursor * cursor, const void * data, const AnyValueType * value_type) const { if (lookup_trivially_serialized(cursor->offset(), value_type)) { cursor->put_bytes(data, value_type->sizeof_type()); @@ -563,7 +482,7 @@ class CDRWriter : public BaseCDRWriter } void serialize_many( - CDRCursor * cursor, const void * data, size_t count, + AbstractCDRWritingCursor * cursor, const void * data, size_t count, const AnyValueType * vt) const { // nothing to do; not even alignment @@ -597,7 +516,7 @@ class CDRWriter : public BaseCDRWriter } void serialize( - CDRCursor * cursor, const void * struct_data, + AbstractCDRWritingCursor * cursor, const void * struct_data, const StructValueType & struct_info) const { for (size_t i = 0; i < struct_info.n_members(); i++) { @@ -609,7 +528,7 @@ class CDRWriter : public BaseCDRWriter } }; -std::unique_ptr make_cdr_writer(std::unique_ptr value_type) +std::unique_ptr make_cdr_writer(std::unique_ptr value_type) { return std::make_unique(std::move(value_type)); } diff --git a/rmw_cyclonedds_cpp/src/Serialization.hpp b/rmw_cyclonedds_cpp/src/Serialization.hpp index 5265b845..5142f360 100644 --- a/rmw_cyclonedds_cpp/src/Serialization.hpp +++ b/rmw_cyclonedds_cpp/src/Serialization.hpp @@ -23,17 +23,17 @@ namespace rmw_cyclonedds_cpp { -class BaseCDRWriter +class AbstractCDRWriter { public: virtual size_t get_serialized_size(const void * data) const = 0; virtual void serialize(void * dest, const void * data) const = 0; virtual size_t get_serialized_size(const cdds_request_wrapper_t & request) const = 0; virtual void serialize(void * dest, const cdds_request_wrapper_t & request) const = 0; - virtual ~BaseCDRWriter() = default; + virtual ~AbstractCDRWriter() = default; }; -std::unique_ptr make_cdr_writer(std::unique_ptr value_type); +std::unique_ptr make_cdr_writer(std::unique_ptr value_type); } // namespace rmw_cyclonedds_cpp #endif // SERIALIZATION_HPP_ diff --git a/rmw_cyclonedds_cpp/src/TypeSupport2.cpp b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp index 00bf6626..bafd1423 100644 --- a/rmw_cyclonedds_cpp/src/TypeSupport2.cpp +++ b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp @@ -41,6 +41,12 @@ class ROSIDLC_StructValueType : public StructValueType size_t sizeof_struct() const override {return impl->size_of_;} size_t n_members() const override {return impl->member_count_;} const Member * get_member(size_t index) const override {return &m_members.at(index);} + void ctor (void * obj) const override { + impl->init_function(obj, ROSIDL_RUNTIME_C_MSG_INIT_ZERO); + } + void dtor (void * obj) const override { + impl->fini_function(obj); + } }; class ROSIDLCPP_StructValueType : public StructValueType @@ -64,6 +70,12 @@ class ROSIDLCPP_StructValueType : public StructValueType size_t sizeof_struct() const override {return impl->size_of_;} size_t n_members() const override {return impl->member_count_;} const Member * get_member(size_t index) const final {return &m_members.at(index);} + void ctor (void * obj) const override { + impl->init_function(obj, rosidl_generator_cpp::MessageInitialization::ZERO); + } + void dtor (void * obj) const override { + impl->fini_function(obj); + } }; std::unique_ptr make_message_value_type(const rosidl_message_type_support_t * mts) @@ -116,12 +128,12 @@ ROSIDLC_StructValueType::ROSIDLC_StructValueType( : impl{impl}, m_members{}, m_inner_value_types{} { for (size_t index = 0; index < impl->member_count_; index++) { - auto member_impl = impl->members_[index]; + auto *member_impl = impl->members_ + index; const AnyValueType * element_value_type; - switch (ROSIDL_TypeKind(member_impl.type_id_)) { + switch (ROSIDL_TypeKind(member_impl->type_id_)) { case ROSIDL_TypeKind::MESSAGE: - m_inner_value_types.push_back(make_message_value_type(member_impl.members_)); + m_inner_value_types.push_back(make_message_value_type(member_impl->members_)); element_value_type = m_inner_value_types.back().get(); break; case ROSIDL_TypeKind::STRING: @@ -132,27 +144,27 @@ ROSIDLC_StructValueType::ROSIDLC_StructValueType( break; default: element_value_type = - make_value_type(ROSIDL_TypeKind(member_impl.type_id_)); + make_value_type(ROSIDL_TypeKind(member_impl->type_id_)); break; } const AnyValueType * member_value_type; - if (!member_impl.is_array_) { + if (!member_impl->is_array_) { member_value_type = element_value_type; - } else if (member_impl.array_size_ != 0 && !member_impl.is_upper_bound_) { + } else if (member_impl->array_size_ != 0 && !member_impl->is_upper_bound_) { member_value_type = make_value_type( - element_value_type, member_impl.array_size_); - } else if (member_impl.size_function) { - member_value_type = make_value_type( - element_value_type, member_impl.size_function, member_impl.get_const_function); + element_value_type, member_impl->array_size_); + } else if (member_impl->size_function) { + // TODO: do these cases need different handling? + member_value_type = make_value_type(member_impl, element_value_type); } else { - member_value_type = make_value_type(element_value_type); + member_value_type = make_value_type(member_impl, element_value_type); } m_members.push_back( Member{ - member_impl.name_, + member_impl->name_, member_value_type, - member_impl.offset_, + member_impl->offset_, }); } } @@ -162,12 +174,12 @@ ROSIDLCPP_StructValueType::ROSIDLCPP_StructValueType( : impl(impl) { for (size_t index = 0; index < impl->member_count_; index++) { - auto member_impl = impl->members_[index]; + auto member_impl = impl->members_ + index; const AnyValueType * element_value_type; - switch (ROSIDL_TypeKind(member_impl.type_id_)) { + switch (ROSIDL_TypeKind(member_impl->type_id_)) { case ROSIDL_TypeKind::MESSAGE: - m_inner_value_types.push_back(make_message_value_type(member_impl.members_)); + m_inner_value_types.push_back(make_message_value_type(member_impl->members_)); element_value_type = m_inner_value_types.back().get(); break; case ROSIDL_TypeKind::STRING: @@ -178,27 +190,28 @@ ROSIDLCPP_StructValueType::ROSIDLCPP_StructValueType( break; default: element_value_type = - make_value_type(ROSIDL_TypeKind(member_impl.type_id_)); + make_value_type(ROSIDL_TypeKind(member_impl->type_id_)); break; } const AnyValueType * member_value_type; - if (!member_impl.is_array_) { + if (!member_impl->is_array_) { member_value_type = element_value_type; - } else if (member_impl.array_size_ != 0 && !member_impl.is_upper_bound_) { + } else if (member_impl->array_size_ != 0 && !member_impl->is_upper_bound_) { member_value_type = make_value_type( - element_value_type, member_impl.array_size_); - } else if (ROSIDL_TypeKind(member_impl.type_id_) == ROSIDL_TypeKind::BOOLEAN) { + element_value_type, member_impl->array_size_); + } else if (ROSIDL_TypeKind(member_impl->type_id_) == ROSIDL_TypeKind::BOOLEAN) { member_value_type = make_value_type(); } else { - member_value_type = make_value_type( - element_value_type, member_impl.size_function, member_impl.get_const_function); + member_value_type = make_value_type( + member_impl, + element_value_type ); } m_members.push_back( Member { - member_impl.name_, + member_impl->name_, member_value_type, - member_impl.offset_, + member_impl->offset_, }); } } diff --git a/rmw_cyclonedds_cpp/src/TypeSupport2.hpp b/rmw_cyclonedds_cpp/src/TypeSupport2.hpp index 13ea0314..a97019eb 100644 --- a/rmw_cyclonedds_cpp/src/TypeSupport2.hpp +++ b/rmw_cyclonedds_cpp/src/TypeSupport2.hpp @@ -187,6 +187,8 @@ class StructValueType : public AnyValueType virtual size_t n_members() const = 0; virtual const Member * get_member(size_t) const = 0; EValueType e_value_type() const final {return EValueType::StructValueType;} + virtual void ctor(void * obj) const = 0; + virtual void dtor(void * obj) const = 0; }; class ArrayValueType : public AnyValueType @@ -214,48 +216,58 @@ class SpanSequenceValueType : public AnyValueType virtual const AnyValueType * element_value_type() const = 0; virtual size_t sequence_size(const void * ptr_to_sequence) const = 0; virtual const void * sequence_contents(const void * ptr_to_sequence) const = 0; + virtual void * sequence_contents(void * ptr_to_sequence) const = 0; EValueType e_value_type() const final {return EValueType::SpanSequenceValueType;} + + virtual void resize(void * ptr_to_sequence, size_t new_size) const = 0; }; -class CallbackSpanSequenceValueType : public SpanSequenceValueType +class ROSIDLCPP_SpanSequenceValueType : public SpanSequenceValueType { protected: + const rosidl_typesupport_introspection_cpp::MessageMember * m_message_member; const AnyValueType * m_element_value_type; - std::function m_size_function; - std::function m_get_const_function; public: - CallbackSpanSequenceValueType( - const AnyValueType * element_value_type, decltype(m_size_function) size_function, - decltype(m_get_const_function) get_const_function) - : m_element_value_type(element_value_type), - m_size_function(size_function), - m_get_const_function(get_const_function) + ROSIDLCPP_SpanSequenceValueType( + const rosidl_typesupport_introspection_cpp::MessageMember * message_member, + const AnyValueType * element_value_type) + : m_message_member(message_member), + m_element_value_type(element_value_type) { assert(m_element_value_type); - assert(size_function); - assert(get_const_function); + assert(m_message_member); } size_t sizeof_type() const override {throw std::logic_error("not implemented");} const AnyValueType * element_value_type() const override {return m_element_value_type;} size_t sequence_size(const void * ptr_to_sequence) const override { - return m_size_function(ptr_to_sequence); + return m_message_member->size_function(ptr_to_sequence); } const void * sequence_contents(const void * ptr_to_sequence) const override { + return nullptr; if (sequence_size(ptr_to_sequence) == 0) { - return nullptr; } - return m_get_const_function(ptr_to_sequence, 0); + return m_message_member->get_const_function(ptr_to_sequence, 0); + } + void * sequence_contents(void * ptr_to_sequence) const override + { + return m_message_member->get_function(ptr_to_sequence, 0); + } + void resize(void * ptr_to_sequence, size_t new_size) const final + { + m_message_member->resize_function(ptr_to_sequence, new_size); } }; class ROSIDLC_SpanSequenceValueType : public SpanSequenceValueType { protected: + const rosidl_typesupport_introspection_c__MessageMember * m_message_member; const AnyValueType * m_element_value_type; + struct ROSIDLC_SequenceObject { void * data; @@ -268,9 +280,17 @@ class ROSIDLC_SpanSequenceValueType : public SpanSequenceValueType return static_cast(ptr_to_sequence); } + const ROSIDLC_SequenceObject * get_value(void * ptr_to_sequence) const + { + return static_cast(ptr_to_sequence); + } + public: - explicit ROSIDLC_SpanSequenceValueType(const AnyValueType * element_value_type) - : m_element_value_type(element_value_type) + explicit ROSIDLC_SpanSequenceValueType( + const rosidl_typesupport_introspection_c__MessageMember * message_member, + const AnyValueType * element_value_type) + : m_message_member(message_member), + m_element_value_type(element_value_type) { } @@ -284,6 +304,17 @@ class ROSIDLC_SpanSequenceValueType : public SpanSequenceValueType { return get_value(ptr_to_sequence)->data; } + void * sequence_contents(void * ptr_to_sequence) const final + { + return get_value(ptr_to_sequence)->data; + } + + void resize(void * ptr_to_sequence, size_t new_size) const final + { + if (!m_message_member->resize_function(ptr_to_sequence, new_size)) { + throw std::runtime_error("Failed to resize"); + } + } }; struct PrimitiveValueType : public AnyValueType @@ -380,18 +411,22 @@ class U8StringValueType : public AnyValueType { public: using char_traits = std::char_traits; + EValueType e_value_type() const final {return EValueType::U8StringValueType;} + // member functions virtual TypedSpan data(void *) const = 0; virtual TypedSpan data(const void *) const = 0; - EValueType e_value_type() const final {return EValueType::U8StringValueType;} + virtual void assign(void * obj, const char * s, size_t count) const = 0; }; class U16StringValueType : public AnyValueType { public: using char_traits = std::char_traits; + EValueType e_value_type() const final {return EValueType::U16StringValueType;} + // member functions virtual TypedSpan data(void *) const = 0; virtual TypedSpan data(const void *) const = 0; - EValueType e_value_type() const final {return EValueType::U16StringValueType;} + virtual void assign(void * obj, const uint16_t * s, size_t count) const = 0; }; struct ROSIDLC_StringValueType : public U8StringValueType @@ -414,6 +449,10 @@ struct ROSIDLC_StringValueType : public U8StringValueType return {str->data, str->size}; } size_t sizeof_type() const override {return sizeof(type);} + void assign(void * obj, const char * s, size_t count) const final + { + rosidl_generator_c__String__assignn(static_cast(obj), s, count); + } }; class ROSIDLC_WStringValueType : public U16StringValueType @@ -432,6 +471,12 @@ class ROSIDLC_WStringValueType : public U16StringValueType return {reinterpret_cast(str->data), str->size}; } size_t sizeof_type() const override {return sizeof(type);} + void assign(void * obj, const uint16_t * s, size_t count) const final + { + rosidl_generator_c__U16String__assignn( + static_cast(obj), + static_cast(s), count); + } }; class ROSIDLCPP_StringValueType : public U8StringValueType @@ -450,6 +495,11 @@ class ROSIDLCPP_StringValueType : public U8StringValueType return {str->data(), str->size()}; } size_t sizeof_type() const override {return sizeof(type);} + + void assign(void * obj, const char * s, size_t count) const final + { + static_cast(obj)->assign(s, count); + } }; class ROSIDLCPP_U16StringValueType : public U16StringValueType @@ -468,6 +518,13 @@ class ROSIDLCPP_U16StringValueType : public U16StringValueType return {str->data(), str->size()}; } size_t sizeof_type() const override {return sizeof(type);} + void assign(void * obj, const uint16_t * s, size_t count) const final + { +// std::u16string x; +// uint16_t * y; +// x.assign(y,y+4); + static_cast(obj)->assign(s, s + count); + } }; template diff --git a/rmw_cyclonedds_cpp/src/serdata.hpp b/rmw_cyclonedds_cpp/src/serdata.hpp index cc506054..a3b97d3f 100644 --- a/rmw_cyclonedds_cpp/src/serdata.hpp +++ b/rmw_cyclonedds_cpp/src/serdata.hpp @@ -24,7 +24,7 @@ namespace rmw_cyclonedds_cpp { -class BaseCDRWriter; +class AbstractCDRWriter; } struct CddsTypeSupport @@ -42,7 +42,7 @@ struct sertopic_rmw : ddsi_sertopic std::string cpp_type_name; std::string cpp_name_type_name; #endif - std::unique_ptr cdr_writer; + std::unique_ptr cdr_writer; }; class serdata_rmw : public ddsi_serdata