diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt index 7fe2187..66daf8d 100644 --- a/rmw_cyclonedds_cpp/CMakeLists.txt +++ b/rmw_cyclonedds_cpp/CMakeLists.txt @@ -57,7 +57,8 @@ add_library(rmw_cyclonedds_cpp src/u16string.cpp src/exception.cpp src/deserialization_exception.cpp -) + src/Serialization.cpp + src/TypeSupport2.cpp) target_include_directories(rmw_cyclonedds_cpp PUBLIC $ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp index 2e817e4..206f52f 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp @@ -112,9 +112,6 @@ template class TypeSupport { public: - bool serializeROSmessage( - const void * ros_message, cycser & ser, - std::function prefix = nullptr); bool deserializeROSmessage( cycdeser & deser, void * ros_message, std::function prefix = nullptr); @@ -132,7 +129,6 @@ protected: std::string name; private: - bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message); bool deserializeROSmessage( cycdeser & deser, const MembersType * members, void * ros_message, bool call_new); diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp index 1135aff..b8888c9 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -204,136 +204,6 @@ static size_t calculateMaxAlign(const MembersType * members) return max_align; } -// C++ specialization -template -void serialize_field( - const rosidl_typesupport_introspection_cpp::MessageMember * member, - void * field, - cycser & ser) -{ - if (!member->is_array_) { - ser << *static_cast(field); - } else if (member->array_size_ && !member->is_upper_bound_) { - ser.serializeA(static_cast(field), member->array_size_); - } else { - std::vector & data = *reinterpret_cast *>(field); - ser << data; - } -} - -template<> -inline -void serialize_field( - const rosidl_typesupport_introspection_cpp::MessageMember * member, - void * field, - cycser & ser) -{ - std::wstring wstr; - if (!member->is_array_) { - auto u16str = static_cast(field); - u16string_to_wstring(*u16str, wstr); - ser << wstr; - } else { - size_t size; - if (member->array_size_ && !member->is_upper_bound_) { - size = member->array_size_; - } else { - size = member->size_function(field); - ser << static_cast(size); - } - for (size_t i = 0; i < size; ++i) { - const void * element = member->get_const_function(field, i); - auto u16str = static_cast(element); - u16string_to_wstring(*u16str, wstr); - ser << wstr; - } - } -} - -// C specialization -template -void serialize_field( - const rosidl_typesupport_introspection_c__MessageMember * member, - void * field, - cycser & ser) -{ - if (!member->is_array_) { - ser << *static_cast(field); - } else if (member->array_size_ && !member->is_upper_bound_) { - ser.serializeA(static_cast(field), member->array_size_); - } else { - auto & data = *reinterpret_cast::type *>(field); - ser.serializeS(reinterpret_cast(data.data), data.size); - } -} - -template<> -inline -void serialize_field( - const rosidl_typesupport_introspection_c__MessageMember * member, - void * field, - cycser & ser) -{ - using CStringHelper = StringHelper; - if (!member->is_array_) { - auto && str = CStringHelper::convert_to_std_string(field); - // Control maximum length. - if (member->string_upper_bound_ && str.length() > member->string_upper_bound_ + 1) { - throw std::runtime_error("string overcomes the maximum length"); - } - ser << str; - } else { - // First, cast field to rosidl_generator_c - // Then convert to a std::string using StringHelper and serialize the std::string - if (member->array_size_ && !member->is_upper_bound_) { - // tmpstring is defined here and not below to avoid - // memory allocation in every iteration of the for loop - std::string tmpstring; - auto string_field = static_cast(field); - for (size_t i = 0; i < member->array_size_; ++i) { - tmpstring = string_field[i].data; - ser.serialize(tmpstring); - } - } else { - auto & string_array_field = *reinterpret_cast(field); - std::vector cpp_string_vector; - for (size_t i = 0; i < string_array_field.size; ++i) { - cpp_string_vector.push_back( - CStringHelper::convert_to_std_string(string_array_field.data[i])); - } - ser << cpp_string_vector; - } - } -} - -template<> -inline -void serialize_field( - const rosidl_typesupport_introspection_c__MessageMember * member, - void * field, - cycser & ser) -{ - std::wstring wstr; - if (!member->is_array_) { - auto u16str = static_cast(field); - u16string_to_wstring(*u16str, wstr); - ser << wstr; - } else if (member->array_size_ && !member->is_upper_bound_) { - auto array = static_cast(field); - for (size_t i = 0; i < member->array_size_; ++i) { - u16string_to_wstring(array[i], wstr); - ser << wstr; - } - } else { - auto sequence = static_cast(field); - ser << static_cast(sequence->size); - for (size_t i = 0; i < sequence->size; ++i) { - u16string_to_wstring(sequence->data[i], wstr); - ser << wstr; - } - } -} - inline size_t get_array_size_and_assign_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, @@ -367,102 +237,6 @@ size_t get_array_size_and_assign_field( return tmpsequence->size; } -template -bool TypeSupport::serializeROSmessage( - cycser & ser, const MembersType * members, const void * ros_message) -{ - assert(members); - assert(ros_message); - - for (uint32_t i = 0; i < members->member_count_; ++i) { - const auto member = members->members_ + i; - void * field = const_cast(static_cast(ros_message)) + member->offset_; - switch (member->type_id_) { - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: - if (!member->is_array_) { - // don't cast to bool here because if the bool is - // uninitialized the random value can't be deserialized - ser << (*static_cast(field) ? true : false); - } else { - serialize_field(member, field, ser); - } - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: - serialize_field(member, field, ser); - break; - case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: - { - auto sub_members = static_cast(member->members_->data); - if (!member->is_array_) { - serializeROSmessage(ser, sub_members, field); - } else { - void * subros_message = nullptr; - size_t array_size = 0; - size_t sub_members_size = sub_members->size_of_; - size_t max_align = calculateMaxAlign(sub_members); - - if (member->array_size_ && !member->is_upper_bound_) { - subros_message = field; - array_size = member->array_size_; - } else { - array_size = get_array_size_and_assign_field( - member, field, subros_message, sub_members_size, max_align); - - // Serialize length - ser << (uint32_t)array_size; - } - - for (size_t index = 0; index < array_size; ++index) { - serializeROSmessage(ser, sub_members, subros_message); - subros_message = static_cast(subros_message) + sub_members_size; - subros_message = align_(max_align, subros_message); - } - } - } - break; - default: - throw std::runtime_error("unknown type"); - } - } - - return true; -} - template void deserialize_field( const rosidl_typesupport_introspection_cpp::MessageMember * member, @@ -872,26 +646,6 @@ bool TypeSupport::printROSmessage( return true; } -template -bool TypeSupport::serializeROSmessage( - const void * ros_message, cycser & ser, - std::function prefix) -{ - assert(ros_message); - - if (prefix) { - prefix(ser); - } - - if (members_->member_count_ != 0) { - TypeSupport::serializeROSmessage(ser, members_, ros_message); - } else { - ser << (uint8_t)0; - } - - return true; -} - template bool TypeSupport::deserializeROSmessage( cycdeser & deser, void * ros_message, diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp index 2eda68f..53ae249 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp @@ -14,11 +14,12 @@ #ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_ #define RMW_CYCLONEDDS_CPP__SERDATA_HPP_ +#include #include #include -#include "dds/ddsi/ddsi_sertopic.h" #include "dds/ddsi/ddsi_serdata.h" +#include "dds/ddsi/ddsi_sertopic.h" struct CddsTypeSupport { @@ -26,6 +27,8 @@ struct CddsTypeSupport const char * typesupport_identifier_; }; +namespace rmw_cyclonedds_cpp {class StructValueType;} + struct sertopic_rmw : ddsi_sertopic { CddsTypeSupport type_support; @@ -35,6 +38,7 @@ struct sertopic_rmw : ddsi_sertopic std::string cpp_type_name; std::string cpp_name_type_name; #endif + std::unique_ptr value_type; }; struct serdata_rmw : ddsi_serdata @@ -68,7 +72,8 @@ void * create_response_type_support( struct sertopic_rmw * create_sertopic( const char * topicname, const char * type_support_identifier, - void * type_support, bool is_request_header); + void * type_support, bool is_request_header, + std::unique_ptr message_type_support); struct ddsi_serdata * serdata_rmw_from_serialized_message( const struct ddsi_sertopic * topiccmn, diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp index 4cdb678..00e9b66 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -29,129 +29,6 @@ using rmw_cyclonedds_cpp::DeserializationException; -class cycser -{ -public: - explicit cycser(std::vector & dst_); - cycser() = delete; - - inline cycser & operator<<(bool x) {serialize(x); return *this;} - inline cycser & operator<<(char x) {serialize(x); return *this;} - inline cycser & operator<<(int8_t x) {serialize(x); return *this;} - inline cycser & operator<<(uint8_t x) {serialize(x); return *this;} - inline cycser & operator<<(int16_t x) {serialize(x); return *this;} - inline cycser & operator<<(uint16_t x) {serialize(x); return *this;} - inline cycser & operator<<(int32_t x) {serialize(x); return *this;} - inline cycser & operator<<(uint32_t x) {serialize(x); return *this;} - inline cycser & operator<<(int64_t x) {serialize(x); return *this;} - inline cycser & operator<<(uint64_t x) {serialize(x); return *this;} - inline cycser & operator<<(float x) {serialize(x); return *this;} - inline cycser & operator<<(double x) {serialize(x); return *this;} - inline cycser & operator<<(const std::string & x) {serialize(x); return *this;} - inline cycser & operator<<(const std::wstring & x) {serialize(x); return *this;} - template - inline cycser & operator<<(const std::vector & x) {serialize(x); return *this;} - template - inline cycser & operator<<(const std::array & x) {serialize(x); return *this;} - -#define SER(T) inline void serialize(T x) { \ - if ((off % sizeof(T)) != 0) { \ - off += sizeof(T) - (off % sizeof(T)); \ - } \ - resize(off + sizeof(T)); \ - *(reinterpret_cast(data() + off)) = x; \ - off += sizeof(T); \ -} - SER(char); - SER(int8_t); - SER(uint8_t); - SER(int16_t); - SER(uint16_t); - SER(int32_t); - SER(uint32_t); - SER(int64_t); - SER(uint64_t); - SER(float); - SER(double); -#undef SER - - inline void serialize(bool x) - { - serialize(static_cast(x)); - } - inline void serialize(const std::string & x) - { - size_t sz = x.size() + 1; - serialize(static_cast(sz)); - resize(off + sz); - memcpy(data() + off, x.c_str(), sz); - off += sz; - } - inline void serialize(const std::wstring & x) - { - size_t sz = x.size(); - serialize(static_cast(sz)); - resize(off + sz * sizeof(wchar_t)); - memcpy(data() + off, reinterpret_cast(x.c_str()), sz * sizeof(wchar_t)); - off += sz * sizeof(wchar_t); - } - -#define SER_A(T) inline void serializeA(const T * x, size_t cnt) { \ - if (cnt > 0) { \ - if ((off % sizeof(T)) != 0) { \ - off += sizeof(T) - (off % sizeof(T)); \ - } \ - resize(off + cnt * sizeof(T)); \ - memcpy(data() + off, reinterpret_cast(x), cnt * sizeof(T)); \ - off += cnt * sizeof(T); \ - } \ -} - SER_A(char); - SER_A(int8_t); - SER_A(uint8_t); - SER_A(int16_t); - SER_A(uint16_t); - SER_A(int32_t); - SER_A(uint32_t); - SER_A(int64_t); - SER_A(uint64_t); - SER_A(float); - SER_A(double); -#undef SER_A - - template - inline void serializeA(const T * x, size_t cnt) - { - for (size_t i = 0; i < cnt; i++) {serialize(x[i]);} - } - - template - inline void serialize(const std::vector & x) - { - serialize(static_cast(x.size())); - serializeA(x.data(), x.size()); - } - inline void serialize(const std::vector & x) - { - serialize(static_cast(x.size())); - for (auto && i : x) {serialize(i);} - } - - template - inline void serializeS(const T * x, size_t cnt) - { - serialize(static_cast(cnt)); - serializeA(x, cnt); - } - -private: - inline void resize(size_t n) {dst.resize(n + 4);} - inline unsigned char * data() {return dst.data() + 4;} - - std::vector & dst; - size_t off; -}; - class cycdeserbase { public: diff --git a/rmw_cyclonedds_cpp/src/Serialization.cpp b/rmw_cyclonedds_cpp/src/Serialization.cpp new file mode 100644 index 0000000..e1b5d08 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/Serialization.cpp @@ -0,0 +1,508 @@ +// 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 "Serialization.hpp" + +#include +#include +#include +#include + +#include "TypeSupport2.hpp" +#include "bytewise.hpp" +#include "rmw_cyclonedds_cpp/TypeSupport_impl.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((-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 : 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 {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); + advance(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: + const EncodingVersion eversion; + const size_t max_align; + + struct CacheKey + { + size_t align; + const AnyValueType * value_type; + bool operator==(const CacheKey & other) const + { + return align == other.align && value_type == other.value_type; + } + + struct Hash + { + size_t operator()(const CacheKey & k) const + { + return std::hash{} (k.align) ^ + ((std::hash{} (k.value_type)) << 1U); + } + }; + }; + + std::unordered_map trivially_serialized_cache; + +public: + CDRWriter() + : eversion{EncodingVersion::CDR_Legacy}, max_align{8}, trivially_serialized_cache{} {} + + void serialize_top_level(CDRCursor * cursor, const void * data, const StructValueType * support) + { + put_rtps_header(cursor); + + if (eversion == EncodingVersion::CDR_Legacy) { + cursor->rebase(+4); + } + + if (support->n_members() == 0 && eversion == EncodingVersion::CDR_Legacy) { + char dummy = '\0'; + cursor->put_bytes(&dummy, 1); + } else { + serialize(cursor, data, support); + } + + if (eversion == EncodingVersion::CDR_Legacy) { + cursor->rebase(-4); + } + } + + void serialize_top_level( + CDRCursor * cursor, const cdds_request_wrapper_t & request, const StructValueType * support) + { + put_rtps_header(cursor); + if (eversion == EncodingVersion::CDR_Legacy) { + cursor->rebase(+4); + } + cursor->put_bytes(&request.header.guid, sizeof(request.header.guid)); + cursor->put_bytes(&request.header.seq, sizeof(request.header.seq)); + + serialize(cursor, request.data, support); + + if (eversion == EncodingVersion::CDR_Legacy) { + cursor->rebase(-4); + } + } + +protected: + void put_rtps_header(CDRCursor * cursor) + { + // beginning of message + char eversion_byte; + switch (eversion) { + case EncodingVersion::CDR_Legacy: + eversion_byte = '\0'; + break; + case EncodingVersion::CDR1: + eversion_byte = '\1'; + break; + } + std::array rtps_header{eversion_byte, + // encoding format = PLAIN_CDR + (native_endian() == endian::little) ? '\1' : '\0', + // options + '\0', '\0'}; + cursor->put_bytes(rtps_header.data(), rtps_header.size()); + } + + void serialize_u32(CDRCursor * cursor, size_t value) + { + assert(value <= std::numeric_limits::max()); + cursor->align(4); + cursor->put_bytes(&value, 4); + } + + static size_t get_cdr_size_of_primitive(ROSIDL_TypeKind tk) + { + /// 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; + } + } + + bool is_trivially_serialized(size_t align, const StructValueType & p) + { + align %= max_align; + + size_t offset = align; + for (size_t i = 0; i < p.n_members(); i++) { + auto m = p.get_member(i); + if (m->member_offset != offset - align) { + return false; + } + if (!is_trivially_serialized(offset % max_align, m->value_type)) { + return false; + } + offset += m->value_type->sizeof_type(); + } + + return offset == align + p.sizeof_struct(); + } + + bool is_trivially_serialized(size_t align, const PrimitiveValueType & v) + { + align %= max_align; + + if (align % get_cdr_alignof_primitive(v.type_kind()) != 0) { + return false; + } + return v.sizeof_type() == get_cdr_size_of_primitive(v.type_kind()); + } + + bool is_trivially_serialized(size_t align, const ArrayValueType & v) + { + align %= max_align; + // if the first element is aligned, we take advantage of the foreknowledge that all future + // elements will be aligned as well + return is_trivially_serialized(align, v.element_value_type()); + } + + /// Returns true if a memcpy is all it takes to serialize this value + bool is_trivially_serialized(size_t align, const AnyValueType * p) + { + align %= max_align; + + CacheKey key{align, p}; + auto iter = trivially_serialized_cache.find(key); + bool result; + if (iter != trivially_serialized_cache.end()) { + result = iter->second; + } else { + switch (p->e_value_type()) { + case EValueType::PrimitiveValueType: + result = is_trivially_serialized(align, *static_cast(p)); + break; + case EValueType::StructValueType: + result = false; + result = is_trivially_serialized(align, *static_cast(p)); + break; + case EValueType::ArrayValueType: + result = is_trivially_serialized(align, *static_cast(p)); + break; + case EValueType::U8StringValueType: + case EValueType::U16StringValueType: + case EValueType::SpanSequenceValueType: + case EValueType::BoolVectorValueType: + result = false; + } + trivially_serialized_cache.emplace(key, result); + } + return result; + } + + size_t get_cdr_alignof_primitive(ROSIDL_TypeKind vt) + { + /// return 0 if the value type is not primitive + /// else returns the number of bytes it should align to + return std::min(get_cdr_size_of_primitive(vt), max_align); + } + + void serialize(CDRCursor * cursor, const void * data, const PrimitiveValueType & value_type) + { + cursor->align(get_cdr_alignof_primitive(value_type.type_kind())); + size_t n_bytes = get_cdr_size_of_primitive(value_type.type_kind()); + + switch (value_type.type_kind()) { + case ROSIDL_TypeKind::FLOAT: + assert(std::numeric_limits::is_iec559); + cursor->put_bytes(data, n_bytes); + return; + case ROSIDL_TypeKind::DOUBLE: + assert(std::numeric_limits::is_iec559); + cursor->put_bytes(data, n_bytes); + return; + case ROSIDL_TypeKind::LONG_DOUBLE: + assert(std::numeric_limits::is_iec559); + cursor->put_bytes(data, n_bytes); + return; + case ROSIDL_TypeKind::CHAR: + case ROSIDL_TypeKind::WCHAR: + case ROSIDL_TypeKind::BOOLEAN: + case ROSIDL_TypeKind::OCTET: + case ROSIDL_TypeKind::UINT8: + case ROSIDL_TypeKind::INT8: + case ROSIDL_TypeKind::UINT16: + case ROSIDL_TypeKind::INT16: + case ROSIDL_TypeKind::UINT32: + case ROSIDL_TypeKind::INT32: + case ROSIDL_TypeKind::UINT64: + case ROSIDL_TypeKind::INT64: + if (value_type.sizeof_type() == n_bytes || native_endian() == endian::little) { + cursor->put_bytes(data, n_bytes); + } else { + const void * offset_data = byte_offset(data, value_type.sizeof_type() - n_bytes); + cursor->put_bytes(offset_data, n_bytes); + } + return; + case ROSIDL_TypeKind::STRING: + case ROSIDL_TypeKind::WSTRING: + case ROSIDL_TypeKind::MESSAGE: + throw std::logic_error("not a primitive"); + } + } + + void serialize(CDRCursor * cursor, const void * data, const U8StringValueType & value_type) + { + auto str = value_type.data(data); + serialize_u32(cursor, str.size() + 1); + cursor->put_bytes(str.data(), str.size()); + char terminator = '\0'; + cursor->put_bytes(&terminator, 1); + } + + void serialize(CDRCursor * cursor, const void * data, const U16StringValueType & value_type) + { + auto str = value_type.data(data); + if (eversion == EncodingVersion::CDR_Legacy) { + serialize_u32(cursor, str.size()); + if (cursor->ignores_data()) { + cursor->advance(sizeof(wchar_t) * str.size()); + } else { + for (wchar_t c : str) { + cursor->put_bytes(&c, sizeof(wchar_t)); + } + } + } else { + serialize_u32(cursor, str.size_bytes()); + cursor->put_bytes(str.data(), str.size_bytes()); + } + } + + void serialize(CDRCursor * cursor, const void * data, const ArrayValueType & value_type) + { + serialize_many( + cursor, value_type.get_data(data), value_type.array_size(), value_type.element_value_type()); + } + + void serialize(CDRCursor * cursor, const void * data, const SpanSequenceValueType & value_type) + { + size_t count = value_type.sequence_size(data); + serialize_u32(cursor, count); + serialize_many( + cursor, value_type.sequence_contents(data), count, value_type.element_value_type()); + } + + void serialize(CDRCursor * cursor, const void * data, const BoolVectorValueType & value_type) + { + size_t count = value_type.size(data); + serialize_u32(cursor, count); + if (cursor->ignores_data()) { + cursor->advance(count); + } else { + for (auto iter = value_type.begin(data); iter != value_type.end(data); ++iter) { + bool b = *iter; + cursor->put_bytes(&b, 1); + } + } + } + + void serialize(CDRCursor * cursor, const void * data, const AnyValueType * value_type) + { + if (is_trivially_serialized(cursor->offset(), value_type)) { + cursor->put_bytes(data, value_type->sizeof_type()); + } else { +// value_type->apply([&](const auto & vt) {return serialize(cursor, data, vt);}); + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + if (auto s = dynamic_cast(value_type)) { + return serialize(cursor, data, *s); + } + throw std::logic_error("Unhandled case"); + } + } + + void serialize_many(CDRCursor * cursor, const void * data, size_t count, const AnyValueType * vt) + { + // nothing to do; not even alignment + if (count == 0) { + return; + } + + if (auto p = dynamic_cast(vt)) { + cursor->align(get_cdr_alignof_primitive(p->type_kind())); + size_t value_size = get_cdr_size_of_primitive(p->type_kind()); + assert(value_size); + if (cursor->ignores_data()) { + cursor->advance(count * value_size); + return; + } + if (is_trivially_serialized(cursor->offset(), p)) { + cursor->put_bytes(data, count * value_size); + return; + } + } + for (size_t i = 0; i < count; i++) { + auto element = byte_offset(data, i * vt->sizeof_type()); + serialize(cursor, element, vt); + } + } + + void serialize(CDRCursor * cursor, const void * struct_data, const StructValueType & struct_info) + { + for (size_t i = 0; i < struct_info.n_members(); i++) { + auto member_info = struct_info.get_member(i); + auto value_type = member_info->value_type; + auto member_data = byte_offset(struct_data, member_info->member_offset); + serialize(cursor, member_data, value_type); + } + } +}; + +size_t get_serialized_size(const void * data, const StructValueType * ts) +{ + SizeCursor cursor; + CDRWriter().serialize_top_level(&cursor, data, ts); + return cursor.offset(); +} + +void serialize(void * dest, const void * data, const StructValueType * ts) +{ + DataCursor cursor(dest); + CDRWriter().serialize_top_level(&cursor, data, ts); +} + +size_t get_serialized_size(const cdds_request_wrapper_t & request, const StructValueType * ts) +{ + SizeCursor cursor; + CDRWriter().serialize_top_level(&cursor, request, ts); + return cursor.offset(); +} + +void serialize(void * dest, const cdds_request_wrapper_t & request, const StructValueType * ts) +{ + DataCursor cursor(dest); + CDRWriter().serialize_top_level(&cursor, request, ts); +} +} // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/src/Serialization.hpp b/rmw_cyclonedds_cpp/src/Serialization.hpp new file mode 100644 index 0000000..2fcf2ed --- /dev/null +++ b/rmw_cyclonedds_cpp/src/Serialization.hpp @@ -0,0 +1,32 @@ +// 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 SERIALIZATION_HPP_ +#define SERIALIZATION_HPP_ + +#include "TypeSupport2.hpp" +#include "rmw_cyclonedds_cpp/serdata.hpp" +#include "rosidl_generator_c/service_type_support_struct.h" + +namespace rmw_cyclonedds_cpp +{ +size_t get_serialized_size(const void * data, const StructValueType * ts); + +void serialize(void * dest, const void * data, const StructValueType * ts); + +size_t get_serialized_size(const cdds_request_wrapper_t & request, const StructValueType * ts); + +void serialize(void * dest, const cdds_request_wrapper_t & request, const StructValueType * ts); +} // namespace rmw_cyclonedds_cpp + +#endif // SERIALIZATION_HPP_ diff --git a/rmw_cyclonedds_cpp/src/TypeSupport2.cpp b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp new file mode 100644 index 0000000..3374b0f --- /dev/null +++ b/rmw_cyclonedds_cpp/src/TypeSupport2.cpp @@ -0,0 +1,201 @@ +// 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 "TypeSupport2.hpp" + +#include +#include +#include +#include + +namespace rmw_cyclonedds_cpp +{ +class ROSIDLC_StructValueType : public StructValueType +{ + const rosidl_typesupport_introspection_c__MessageMembers * impl; + std::vector m_members; + std::vector> m_inner_value_types; + + template + ConstructedType * make_value_type(Args && ... args) + { + auto unique_ptr = std::make_unique(std::forward(args)...); + auto ptr = unique_ptr.get(); + m_inner_value_types.push_back(std::move(unique_ptr)); + return ptr; + } + +public: + static constexpr TypeGenerator gen = TypeGenerator::ROSIDL_C; + explicit ROSIDLC_StructValueType(const rosidl_typesupport_introspection_c__MessageMembers * impl); + 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);} +}; + +class ROSIDLCPP_StructValueType : public StructValueType +{ + const rosidl_typesupport_introspection_cpp::MessageMembers * impl; + std::vector m_members; + std::vector> m_inner_value_types; + template + ConstructedType * make_value_type(Args && ... args) + { + auto unique_ptr = std::make_unique(std::forward(args)...); + auto ptr = unique_ptr.get(); + m_inner_value_types.push_back(std::move(unique_ptr)); + return ptr; + } + +public: + static constexpr TypeGenerator gen = TypeGenerator::ROSIDL_Cpp; + explicit ROSIDLCPP_StructValueType( + const rosidl_typesupport_introspection_cpp::MessageMembers * impl); + 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);} +}; + +std::unique_ptr make_message_value_type(const rosidl_message_type_support_t * mts) +{ + if (auto ts_c = mts->func(mts, TypeGeneratorInfo::identifier)) { + auto members = static_cast *>(ts_c->data); + return std::make_unique(members); + } + if (auto ts_cpp = mts->func(mts, TypeGeneratorInfo::identifier)) { + auto members = static_cast *>(ts_cpp->data); + return std::make_unique(members); + } + throw std::runtime_error( + "could not identify message typesupport " + std::string(mts->typesupport_identifier)); +} + +std::pair, std::unique_ptr> +make_request_response_value_types(const rosidl_service_type_support_t * svc_ts) +{ + if (auto tsc = svc_ts->func(svc_ts, TypeGeneratorInfo::identifier)) { + auto typed = + static_cast::MetaService *>(tsc->data); + return { + std::make_unique(typed->request_members_), + std::make_unique(typed->response_members_) + }; + } + + if (auto tscpp = + svc_ts->func(svc_ts, TypeGeneratorInfo::identifier)) + { + auto typed = + static_cast::MetaService *>(tscpp->data); + return { + std::make_unique(typed->request_members_), + std::make_unique(typed->response_members_) + }; + } + + throw std::runtime_error( + "Unidentified service type support: " + std::string(svc_ts->typesupport_identifier)); +} + +ROSIDLC_StructValueType::ROSIDLC_StructValueType( + const rosidl_typesupport_introspection_c__MessageMembers * impl) +: impl{impl}, m_members{}, m_inner_value_types{} +{ + for (size_t index = 0; index < impl->member_count_; index++) { + auto member_impl = impl->members_[index]; + + const AnyValueType * element_value_type; + switch (ROSIDL_TypeKind(member_impl.type_id_)) { + case ROSIDL_TypeKind::MESSAGE: + 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: + element_value_type = make_value_type(); + break; + case ROSIDL_TypeKind::WSTRING: + element_value_type = make_value_type(); + break; + default: + element_value_type = + make_value_type(ROSIDL_TypeKind(member_impl.type_id_)); + break; + } + + const AnyValueType * member_value_type; + if (!member_impl.is_array_) { + member_value_type = element_value_type; + } 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); + } else { + member_value_type = make_value_type(element_value_type); + } + m_members.push_back(Member{ + member_impl.name_, + member_value_type, + member_impl.offset_, + }); + } +} + +ROSIDLCPP_StructValueType::ROSIDLCPP_StructValueType( + const rosidl_typesupport_introspection_cpp::MessageMembers * impl) +: impl(impl) +{ + for (size_t index = 0; index < impl->member_count_; index++) { + auto member_impl = impl->members_[index]; + + const AnyValueType * element_value_type; + switch (ROSIDL_TypeKind(member_impl.type_id_)) { + case ROSIDL_TypeKind::MESSAGE: + 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: + element_value_type = make_value_type(); + break; + case ROSIDL_TypeKind::WSTRING: + element_value_type = make_value_type(); + break; + default: + element_value_type = + make_value_type(ROSIDL_TypeKind(member_impl.type_id_)); + break; + } + + const AnyValueType * member_value_type; + if (!member_impl.is_array_) { + member_value_type = element_value_type; + } 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) { + 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); + } + m_members.push_back( + Member { + member_impl.name_, + member_value_type, + member_impl.offset_, + }); + } +} +} // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/src/TypeSupport2.hpp b/rmw_cyclonedds_cpp/src/TypeSupport2.hpp new file mode 100644 index 0000000..030cf8a --- /dev/null +++ b/rmw_cyclonedds_cpp/src/TypeSupport2.hpp @@ -0,0 +1,510 @@ +// 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 TYPESUPPORT2_HPP_ +#define TYPESUPPORT2_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "bytewise.hpp" +#include "rosidl_generator_c/string_functions.h" +#include "rosidl_generator_c/u16string_functions.h" +#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +namespace rmw_cyclonedds_cpp +{ +struct AnyValueType; + +/// contiguous storage objects +template +class TypedSpan; + +template +class TypedSpan +{ + const T * m_data; + const size_t m_size; + +public: + TypedSpan(const T * data, size_t size) + : m_data(data), m_size(size) + { + } + + size_t size() const {return m_size;} + size_t size_bytes() const {return size() * sizeof(T);} + const T * data() const {return m_data;} + + auto begin() {return m_data;} + auto end() {return m_data + size();} +}; + +template +auto make_typed_span(const NativeType * m_data, size_t size) +{ + return TypedSpan{m_data, size}; +} + +enum class TypeGenerator +{ + ROSIDL_C, + ROSIDL_Cpp, +}; + +template +struct TypeGeneratorInfo; + +template<> +struct TypeGeneratorInfo +{ + static constexpr auto enum_value = TypeGenerator::ROSIDL_C; + + static constexpr auto & identifier = rosidl_typesupport_introspection_c__identifier; + using MetaMessage = rosidl_typesupport_introspection_c__MessageMembers; + using MetaMember = rosidl_typesupport_introspection_c__MessageMember; + using MetaService = rosidl_typesupport_introspection_c__ServiceMembers; +}; + +template<> +struct TypeGeneratorInfo +{ + static constexpr auto enum_value = TypeGenerator::ROSIDL_Cpp; + static constexpr auto & identifier = rosidl_typesupport_introspection_cpp::typesupport_identifier; + using MetaMessage = rosidl_typesupport_introspection_cpp::MessageMembers; + using MetaMember = rosidl_typesupport_introspection_cpp::MessageMember; + using MetaService = rosidl_typesupport_introspection_cpp::ServiceMembers; +}; + +template +using MetaMessage = typename TypeGeneratorInfo::MetaMessage; +template +using MetaMember = typename TypeGeneratorInfo::MetaMember; +template +using MetaService = typename TypeGeneratorInfo::MetaService; + +namespace tsi_enum = rosidl_typesupport_introspection_cpp; + +// these are shared between c and cpp +enum class ROSIDL_TypeKind : uint8_t +{ + FLOAT = tsi_enum::ROS_TYPE_FLOAT, + DOUBLE = tsi_enum::ROS_TYPE_DOUBLE, + LONG_DOUBLE = tsi_enum::ROS_TYPE_LONG_DOUBLE, + CHAR = tsi_enum::ROS_TYPE_CHAR, + WCHAR = tsi_enum::ROS_TYPE_WCHAR, + BOOLEAN = tsi_enum::ROS_TYPE_BOOLEAN, + OCTET = tsi_enum::ROS_TYPE_OCTET, + UINT8 = tsi_enum::ROS_TYPE_UINT8, + INT8 = tsi_enum::ROS_TYPE_INT8, + UINT16 = tsi_enum::ROS_TYPE_UINT16, + INT16 = tsi_enum::ROS_TYPE_INT16, + UINT32 = tsi_enum::ROS_TYPE_UINT32, + INT32 = tsi_enum::ROS_TYPE_INT32, + UINT64 = tsi_enum::ROS_TYPE_UINT64, + INT64 = tsi_enum::ROS_TYPE_INT64, + STRING = tsi_enum::ROS_TYPE_STRING, + WSTRING = tsi_enum::ROS_TYPE_WSTRING, + + MESSAGE = tsi_enum::ROS_TYPE_MESSAGE, +}; + + +class StructValueType; +std::unique_ptr make_message_value_type(const rosidl_message_type_support_t * mts); + +std::pair, std::unique_ptr> +make_request_response_value_types(const rosidl_service_type_support_t * svc); + +enum class EValueType +{ + // the logical value type + PrimitiveValueType, + U8StringValueType, + U16StringValueType, + StructValueType, + ArrayValueType, + SpanSequenceValueType, + BoolVectorValueType, +}; + +struct AnyValueType +{ + // represents not just the IDL value but also its physical representation + virtual ~AnyValueType() = default; + + // how many bytes this value type takes up + virtual size_t sizeof_type() const = 0; + + // represents the logical value type and supports the 'apply' function + virtual EValueType e_value_type() const = 0; + + // faster alternative to dynamic cast + template + auto apply(UnaryFunction f) const; + + template + auto apply(UnaryFunction f); +}; + +struct Member +{ + const char * name; + const AnyValueType * value_type; + size_t member_offset; +}; + +class StructValueType : public AnyValueType +{ +public: + ROSIDL_TypeKind type_kind() const {return ROSIDL_TypeKind::MESSAGE;} + size_t sizeof_type() const final {return sizeof_struct();} + virtual size_t sizeof_struct() const = 0; + 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;} +}; + +class ArrayValueType : public AnyValueType +{ +protected: + const AnyValueType * m_element_value_type; + size_t m_size; + +public: + ArrayValueType(const AnyValueType * element_value_type, size_t size) + : m_element_value_type(element_value_type), m_size(size) + { + } + const AnyValueType * element_value_type() const {return m_element_value_type;} + size_t sizeof_type() const final {return m_size * m_element_value_type->sizeof_type();} + size_t array_size() const {return m_size;} + const void * get_data(const void * ptr_to_array) const {return ptr_to_array;} + EValueType e_value_type() const final {return EValueType::ArrayValueType;} +}; + +class SpanSequenceValueType : public AnyValueType +{ +public: + using AnyValueType::sizeof_type; + 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; + EValueType e_value_type() const final {return EValueType::SpanSequenceValueType;} +}; + +class CallbackSpanSequenceValueType : public SpanSequenceValueType +{ +protected: + 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) + { + assert(m_element_value_type); + assert(size_function); + assert(get_const_function); + } + + 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); + } + const void * sequence_contents(const void * ptr_to_sequence) const override + { + return m_get_const_function(ptr_to_sequence, 0); + } +}; + +class ROSIDLC_SpanSequenceValueType : public SpanSequenceValueType +{ +protected: + const AnyValueType * m_element_value_type; + struct ROSIDLC_SequenceObject + { + void * data; + size_t size; /*!< The number of valid items in data */ + size_t capacity; /*!< The number of allocated items in data */ + }; + + const ROSIDLC_SequenceObject * get_value(const 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) + { + } + + size_t sizeof_type() const override {return sizeof(ROSIDLC_SequenceObject);} + const AnyValueType * element_value_type() const override {return m_element_value_type;} + size_t sequence_size(const void * ptr_to_sequence) const override + { + return get_value(ptr_to_sequence)->size; + } + const void * sequence_contents(const void * ptr_to_sequence) const final + { + return get_value(ptr_to_sequence)->data; + } +}; + +struct PrimitiveValueType : public AnyValueType +{ + const ROSIDL_TypeKind m_type_kind; + + explicit constexpr PrimitiveValueType(ROSIDL_TypeKind type_kind) + : m_type_kind(type_kind) + { + assert(type_kind != ROSIDL_TypeKind::STRING); + assert(type_kind != ROSIDL_TypeKind::WSTRING); + assert(type_kind != ROSIDL_TypeKind::MESSAGE); + } + ROSIDL_TypeKind type_kind() const {return m_type_kind;} + size_t sizeof_type() const final + { + switch (m_type_kind) { + case ROSIDL_TypeKind::FLOAT: + return sizeof(float); + case ROSIDL_TypeKind::DOUBLE: + return sizeof(double); + case ROSIDL_TypeKind::LONG_DOUBLE: + return sizeof(long double); + case ROSIDL_TypeKind::CHAR: + return sizeof(char); + case ROSIDL_TypeKind::WCHAR: + return sizeof(char16_t); + case ROSIDL_TypeKind::BOOLEAN: + return sizeof(bool); + case ROSIDL_TypeKind::OCTET: + return sizeof(unsigned char); + case ROSIDL_TypeKind::UINT8: + return sizeof(uint_least8_t); + case ROSIDL_TypeKind::INT8: + return sizeof(int_least8_t); + case ROSIDL_TypeKind::UINT16: + return sizeof(uint_least16_t); + case ROSIDL_TypeKind::INT16: + return sizeof(int_least16_t); + case ROSIDL_TypeKind::UINT32: + return sizeof(uint_least32_t); + case ROSIDL_TypeKind::INT32: + return sizeof(int_least32_t); + case ROSIDL_TypeKind::UINT64: + return sizeof(uint_least64_t); + case ROSIDL_TypeKind::INT64: + return sizeof(int_least64_t); + case ROSIDL_TypeKind::STRING: + case ROSIDL_TypeKind::WSTRING: + case ROSIDL_TypeKind::MESSAGE: + throw std::runtime_error( + "not a primitive value type: " + + std::to_string(std::underlying_type_t(m_type_kind))); + } + } + EValueType e_value_type() const override {return EValueType::PrimitiveValueType;} +}; + +class BoolVectorValueType : public AnyValueType +{ +protected: + const std::vector * get_value(const void * ptr_to_sequence) const + { + return static_cast *>(ptr_to_sequence); + } + + static std::unique_ptr s_element_value_type; + +public: + size_t sizeof_type() const override {return sizeof(std::vector);} + + static const AnyValueType * element_value_type() + { + if (!s_element_value_type) { + s_element_value_type = std::make_unique(ROSIDL_TypeKind::BOOLEAN); + } + return s_element_value_type.get(); + } + + std::vector::const_iterator begin(const void * ptr_to_sequence) const + { + return get_value(ptr_to_sequence)->begin(); + } + std::vector::const_iterator end(const void * ptr_to_sequence) const + { + return get_value(ptr_to_sequence)->end(); + } + size_t size(const void * ptr_to_sequence) const {return get_value(ptr_to_sequence)->size();} + EValueType e_value_type() const final {return EValueType::BoolVectorValueType;} +}; + +class ROSIDLC_StructValueType; + +class U8StringValueType : public AnyValueType +{ +public: + using char_traits = std::char_traits; + virtual TypedSpan data(void *) const = 0; + virtual TypedSpan data(const void *) const = 0; + EValueType e_value_type() const final {return EValueType::U8StringValueType;} +}; + +class U16StringValueType : public AnyValueType +{ +public: + using char_traits = std::char_traits; + virtual TypedSpan data(void *) const = 0; + virtual TypedSpan data(const void *) const = 0; + EValueType e_value_type() const final {return EValueType::U16StringValueType;} +}; + +struct ROSIDLC_StringValueType : public U8StringValueType +{ +public: + using type = rosidl_generator_c__String; + + TypedSpan data(const void * ptr) const override + { + auto str = static_cast(ptr); + assert(str->capacity == str->size + 1); + assert(str->data[str->size] == '\0'); + return {str->data, str->size}; + } + TypedSpan data(void * ptr) const override + { + auto str = static_cast(ptr); + assert(str->capacity == str->size + 1); + assert(str->data[str->size + 1] == 0); + return {str->data, str->size}; + } + size_t sizeof_type() const override {return sizeof(type);} +}; + +class ROSIDLC_WStringValueType : public U16StringValueType +{ +public: + using type = rosidl_generator_c__U16String; + + TypedSpan data(const void * ptr) const override + { + auto str = static_cast(ptr); + return {reinterpret_cast(str->data), str->size}; + } + TypedSpan data(void * ptr) const override + { + auto str = static_cast(ptr); + return {reinterpret_cast(str->data), str->size}; + } + size_t sizeof_type() const override {return sizeof(type);} +}; + +class ROSIDLCPP_StringValueType : public U8StringValueType +{ +public: + using type = std::string; + + TypedSpan data(const void * ptr) const override + { + auto str = static_cast(ptr); + return {str->data(), str->size()}; + } + TypedSpan data(void * ptr) const override + { + auto str = static_cast(ptr); + return {str->data(), str->size()}; + } + size_t sizeof_type() const override {return sizeof(type);} +}; + +class ROSIDLCPP_U16StringValueType : public U16StringValueType +{ +public: + using type = std::u16string; + + TypedSpan data(const void * ptr) const override + { + auto str = static_cast(ptr); + return {str->data(), str->size()}; + } + TypedSpan data(void * ptr) const override + { + auto str = static_cast(ptr); + return {str->data(), str->size()}; + } + size_t sizeof_type() const override {return sizeof(type);} +}; + +template +auto AnyValueType::apply(UnaryFunction f) const +{ + switch (e_value_type()) { + case EValueType::PrimitiveValueType: + return f(*static_cast(this)); + case EValueType::U8StringValueType: + return f(*static_cast(this)); + case EValueType::U16StringValueType: + return f(*static_cast(this)); + case EValueType::StructValueType: + return f(*static_cast(this)); + case EValueType::ArrayValueType: + return f(*static_cast(this)); + case EValueType::SpanSequenceValueType: + return f(*static_cast(this)); + case EValueType::BoolVectorValueType: + return f(*static_cast(this)); + } +} + +template +auto AnyValueType::apply(UnaryFunction f) +{ + switch (e_value_type()) { + case EValueType::PrimitiveValueType: + return f(*static_cast(this)); + case EValueType::U8StringValueType: + return f(*static_cast(this)); + case EValueType::U16StringValueType: + return f(*static_cast(this)); + case EValueType::StructValueType: + return f(*static_cast(this)); + case EValueType::ArrayValueType: + return f(*static_cast(this)); + case EValueType::SpanSequenceValueType: + return f(*static_cast(this)); + case EValueType::BoolVectorValueType: + return f(*static_cast(this)); + } +} + +} // namespace rmw_cyclonedds_cpp +#endif // TYPESUPPORT2_HPP_ diff --git a/rmw_cyclonedds_cpp/src/bytewise.hpp b/rmw_cyclonedds_cpp/src/bytewise.hpp new file mode 100644 index 0000000..fe5def9 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/bytewise.hpp @@ -0,0 +1,41 @@ +// 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 BYTEWISE_HPP_ +#define BYTEWISE_HPP_ + +#include + +#include "dds/ddsrt/endian.h" + +enum class endian +{ + little = DDSRT_LITTLE_ENDIAN, + big = DDSRT_BIG_ENDIAN, +}; + +constexpr endian native_endian() {return endian(DDSRT_ENDIAN);} + +enum class byte : unsigned char {}; + +static inline auto byte_offset(void * ptr, ptrdiff_t n) +{ + return static_cast(static_cast(ptr) + n); +} + +static inline auto byte_offset(const void * ptr, ptrdiff_t n) +{ + return static_cast(static_cast(ptr) + n); +} + +#endif // BYTEWISE_HPP_ diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index f99810d..e6487e9 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -40,8 +41,11 @@ #include "rmw/rmw.h" #include "rmw/sanity_checks.h" +#include "Serialization.hpp" #include "rmw/impl/cpp/macros.hpp" +#include "TypeSupport2.hpp" + #include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp" #include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp" @@ -846,6 +850,7 @@ extern "C" rmw_ret_t rmw_get_serialized_message_size( static_cast(type_support); static_cast(message_bounds); static_cast(size); + RMW_SET_ERROR_MSG("rmw_get_serialized_message_size: unimplemented"); return RMW_RET_ERROR; } @@ -855,39 +860,22 @@ extern "C" rmw_ret_t rmw_serialize( const rosidl_message_type_support_t * type_support, rmw_serialized_message_t * serialized_message) { - std::vector data; - cycser sd(data); rmw_ret_t ret; - const rosidl_message_type_support_t * ts; - if ((ts = - get_message_typesupport_handle(type_support, - rosidl_typesupport_introspection_c__identifier)) != nullptr) - { - auto members = - static_cast(ts->data); - MessageTypeSupport_c msgts(members); - msgts.serializeROSmessage(ros_message, sd, nullptr); - } else { - if ((ts = - get_message_typesupport_handle(type_support, - rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) - { - auto members = - static_cast(ts->data); - MessageTypeSupport_cpp msgts(members); - msgts.serializeROSmessage(ros_message, sd, nullptr); - } else { - RMW_SET_ERROR_MSG("rmw_serialize: type support trouble"); - return RMW_RET_ERROR; - } - } + try { + auto ts = rmw_cyclonedds_cpp::make_message_value_type(type_support); - if ((ret = rmw_serialized_message_resize(serialized_message, data.size())) != RMW_RET_OK) { - return ret; + auto size = rmw_cyclonedds_cpp::get_serialized_size(ros_message, ts.get()); + if ((ret = rmw_serialized_message_resize(serialized_message, size) != RMW_RET_OK)) { + RMW_SET_ERROR_MSG("rmw_serialize: failed to allocate space for message"); + return ret; + } + rmw_cyclonedds_cpp::serialize(serialized_message->buffer, ros_message, ts.get()); + serialized_message->buffer_length = size; + return RMW_RET_OK; + } catch (std::exception & e) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("rmw_serialize: failed to serialize: %s", e.what()); + return RMW_RET_ERROR; } - memcpy(serialized_message->buffer, data.data(), data.size()); - serialized_message->buffer_length = data.size(); - return RMW_RET_OK; } extern "C" rmw_ret_t rmw_deserialize( @@ -1226,7 +1214,8 @@ static CddsPublisher * create_cdds_publisher( auto sertopic = create_sertopic( fqtopic_name.c_str(), type_support->typesupport_identifier, - create_message_type_support(type_support->data, type_support->typesupport_identifier), false); + create_message_type_support(type_support->data, type_support->typesupport_identifier), false, + rmw_cyclonedds_cpp::make_message_value_type(type_supports)); if ((topic = dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) { @@ -1447,7 +1436,8 @@ static CddsSubscription * create_cdds_subscription( auto sertopic = create_sertopic( fqtopic_name.c_str(), type_support->typesupport_identifier, - create_message_type_support(type_support->data, type_support->typesupport_identifier), false); + create_message_type_support(type_support->data, type_support->typesupport_identifier), false, + rmw_cyclonedds_cpp::make_message_value_type(type_supports)); if ((topic = dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) { @@ -2331,7 +2321,13 @@ static rmw_ret_t rmw_init_cs( auto sub = new CddsSubscription(); std::string subtopic_name, pubtopic_name; void * pub_type_support, * sub_type_support; + + std::unique_ptr pub_msg_ts, sub_msg_ts; + if (is_service) { + std::tie(sub_msg_ts, pub_msg_ts) = + rmw_cyclonedds_cpp::make_request_response_value_types(type_supports); + sub_type_support = create_request_type_support(type_support->data, type_support->typesupport_identifier); pub_type_support = create_response_type_support(type_support->data, @@ -2340,6 +2336,9 @@ static rmw_ret_t rmw_init_cs( make_fqtopic(ros_service_requester_prefix, service_name, "Request", qos_policies); pubtopic_name = make_fqtopic(ros_service_response_prefix, service_name, "Reply", qos_policies); } else { + std::tie(pub_msg_ts, sub_msg_ts) = + rmw_cyclonedds_cpp::make_request_response_value_types(type_supports); + pub_type_support = create_request_type_support(type_support->data, type_support->typesupport_identifier); sub_type_support = create_response_type_support(type_support->data, @@ -2358,9 +2357,11 @@ static rmw_ret_t rmw_init_cs( dds_entity_t pubtopic, subtopic; auto pub_st = create_sertopic( - pubtopic_name.c_str(), type_support->typesupport_identifier, pub_type_support, true); + pubtopic_name.c_str(), type_support->typesupport_identifier, pub_type_support, true, + std::move(pub_msg_ts)); auto sub_st = create_sertopic( - subtopic_name.c_str(), type_support->typesupport_identifier, sub_type_support, true); + subtopic_name.c_str(), type_support->typesupport_identifier, sub_type_support, true, + std::move(sub_msg_ts)); dds_qos_t * qos; if ((pubtopic = diff --git a/rmw_cyclonedds_cpp/src/serdata.cpp b/rmw_cyclonedds_cpp/src/serdata.cpp index ff7c8ea..52a929d 100644 --- a/rmw_cyclonedds_cpp/src/serdata.cpp +++ b/rmw_cyclonedds_cpp/src/serdata.cpp @@ -11,23 +11,22 @@ // 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 +#include "rmw_cyclonedds_cpp/serdata.hpp" -#include +#include +#include #include #include #include +#include +#include "TypeSupport2.hpp" +#include "Serialization.hpp" +#include "dds/ddsi/q_radmin.h" #include "rmw/error_handling.h" - #include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp" #include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp" - #include "rmw_cyclonedds_cpp/serdes.hpp" -#include "rmw_cyclonedds_cpp/serdata.hpp" - -#include "dds/ddsi/ddsi_iid.h" -#include "dds/ddsi/q_radmin.h" using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport; @@ -161,6 +160,13 @@ static struct ddsi_serdata * serdata_rmw_from_keyhash( return rmw_serdata_new(topic, SDK_KEY); } +size_t next_multiple_of_4(size_t n_bytes) +{ + /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector + when copying data to network. Should fix Cyclone to handle that more elegantly. */ + return n_bytes + (-n_bytes) % 4; +} + static struct ddsi_serdata * serdata_rmw_from_sample( const struct ddsi_sertopic * topiccmn, enum ddsi_serdata_kind kind, @@ -172,43 +178,21 @@ static struct ddsi_serdata * serdata_rmw_from_sample( if (kind != SDK_DATA) { /* ROS2 doesn't do keys, so SDK_KEY is trivial */ } else if (!topic->is_request_header) { - cycser sd(d->data); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd); - } + size_t sz = rmw_cyclonedds_cpp::get_serialized_size(sample, topic->value_type.get()); + d->data.resize(next_multiple_of_4(sz)); + rmw_cyclonedds_cpp::serialize(d->data.data(), sample, topic->value_type.get()); } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - const cdds_request_wrapper_t * wrap = static_cast(sample); - auto prefix = [wrap](cycser & ser) {ser << wrap->header.guid; ser << wrap->header.seq;}; - cycser sd(d->data); - if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); - } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = - static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); - } - } - /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector - when copying data to network. Should fix Cyclone to handle that more elegantly. */ - while (d->data.size() % 4) { - d->data.push_back(0); + /* inject the service invocation header data into the CDR stream -- + * I haven't checked how it is done in the official RMW implementations, so it is + * probably incompatible. */ + auto wrap = *static_cast(sample); + + size_t sz = rmw_cyclonedds_cpp::get_serialized_size(wrap, topic->value_type.get()); + d->data.resize(next_multiple_of_4(sz)); + rmw_cyclonedds_cpp::serialize(d->data.data(), wrap, topic->value_type.get()); } return d; - } catch (rmw_cyclonedds_cpp::Exception & e) { - RMW_SET_ERROR_MSG(e.what()); - return nullptr; - } catch (std::runtime_error & e) { + } catch (std::exception & e) { RMW_SET_ERROR_MSG(e.what()); return nullptr; } @@ -220,13 +204,8 @@ struct ddsi_serdata * serdata_rmw_from_serialized_message( { const struct sertopic_rmw * topic = static_cast(topiccmn); struct serdata_rmw * d = rmw_serdata_new(topic, SDK_DATA); - /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector - when copying data to network. Should fix Cyclone to handle that more elegantly. */ - d->data.resize(size); + d->data.resize(next_multiple_of_4(size)); memcpy(d->data.data(), raw, size); - while (d->data.size() % 4) { - d->data.push_back(0); - } return d; } @@ -493,7 +472,8 @@ static std::string get_type_name(const char * type_support_identifier, void * ty struct sertopic_rmw * create_sertopic( const char * topicname, const char * type_support_identifier, - void * type_support, bool is_request_header) + void * type_support, bool is_request_header, + std::unique_ptr message_type) { struct sertopic_rmw * st = new struct sertopic_rmw; #if DDSI_SERTOPIC_HAS_TOPICKIND_NO_KEY @@ -517,5 +497,6 @@ struct sertopic_rmw * create_sertopic( st->type_support.typesupport_identifier_ = type_support_identifier; st->type_support.type_support_ = type_support; st->is_request_header = is_request_header; + st->value_type = std::move(message_type); return st; } diff --git a/rmw_cyclonedds_cpp/src/serdes.cpp b/rmw_cyclonedds_cpp/src/serdes.cpp index d5857aa..25e468b 100644 --- a/rmw_cyclonedds_cpp/src/serdes.cpp +++ b/rmw_cyclonedds_cpp/src/serdes.cpp @@ -12,26 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. #include -#include #include "dds/ddsrt/endian.h" #include "rmw_cyclonedds_cpp/serdes.hpp" -cycser::cycser(std::vector & dst_) -: dst(dst_), - off(0) -{ - dst.reserve(4); - - /* endianness: 0x0000 for BE, 0x0001 for LE */ - dst.push_back(0x00); - dst.push_back((DDSRT_ENDIAN == DDSRT_LITTLE_ENDIAN) ? 0x01 : 0x00); - - /* options: defaults to 0x0000 */ - dst.push_back(0); - dst.push_back(0); -} - cycdeserbase::cycdeserbase(const char * data_, size_t size_) : data(data_), pos(0),