niklas/rmw_cyclonedds
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Rework serialization (#42)

Browse source 
* Rework serialization and typesupport

Signed-off-by: Dan Rose <dan@digilabs.io>
This commit is contained in:
Dan Rose 2019-12-05 14:42:11 -06:00 • committed by GitHub
parent 3e7d33ba2b
commit 88e74c322f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
13 changed files with 1366 additions and 475 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rmw_cyclonedds_cpp/CMakeLists.txt
View file

@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>

4
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp
View file

@ -112,9 +112,6 @@ template<typename MembersType>
class TypeSupport
{
public:
bool serializeROSmessage(
const void * ros_message, cycser & ser,
std::function<void(cycser &)> prefix = nullptr);
bool deserializeROSmessage(
cycdeser & deser, void * ros_message,
std::function<void(cycdeser &)> 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);

246
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp
View file

@ -204,136 +204,6 @@ static size_t calculateMaxAlign(const MembersType * members)
return max_align;
}
// C++ specialization
template<typename T>
void serialize_field(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
void * field,
cycser & ser)
{
if (!member->is_array_) {
ser << *static_cast<T *>(field);
} else if (member->array_size_ && !member->is_upper_bound_) {
ser.serializeA(static_cast<T *>(field), member->array_size_);
} else {
std::vector<T> & data = *reinterpret_cast<std::vector<T> *>(field);
ser << data;
}
}
template<>
inline
void serialize_field<std::wstring>(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
void * field,
cycser & ser)
{
std::wstring wstr;
if (!member->is_array_) {
auto u16str = static_cast<std::u16string *>(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<uint32_t>(size);
}
for (size_t i = 0; i < size; ++i) {
const void * element = member->get_const_function(field, i);
auto u16str = static_cast<const std::u16string *>(element);
u16string_to_wstring(*u16str, wstr);
ser << wstr;
}
}
}
// C specialization
template<typename T>
void serialize_field(
const rosidl_typesupport_introspection_c__MessageMember * member,
void * field,
cycser & ser)
{
if (!member->is_array_) {
ser << *static_cast<T *>(field);
} else if (member->array_size_ && !member->is_upper_bound_) {
ser.serializeA(static_cast<T *>(field), member->array_size_);
} else {
auto & data = *reinterpret_cast<typename GenericCSequence<T>::type *>(field);
ser.serializeS(reinterpret_cast<T *>(data.data), data.size);
}
}
template<>
inline
void serialize_field<std::string>(
const rosidl_typesupport_introspection_c__MessageMember * member,
void * field,
cycser & ser)
{
using CStringHelper = StringHelper<rosidl_typesupport_introspection_c__MessageMembers>;
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<rosidl_generator_c__String *>(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<rosidl_generator_c__String__Sequence *>(field);
std::vector<std::string> 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<std::wstring>(
const rosidl_typesupport_introspection_c__MessageMember * member,
void * field,
cycser & ser)
{
std::wstring wstr;
if (!member->is_array_) {
auto u16str = static_cast<rosidl_generator_c__U16String *>(field);
u16string_to_wstring(*u16str, wstr);
ser << wstr;
} else if (member->array_size_ && !member->is_upper_bound_) {
auto array = static_cast<rosidl_generator_c__U16String *>(field);
for (size_t i = 0; i < member->array_size_; ++i) {
u16string_to_wstring(array[i], wstr);
ser << wstr;
}
} else {
auto sequence = static_cast<rosidl_generator_c__U16String__Sequence *>(field);
ser << static_cast<uint32_t>(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<typename MembersType>
bool TypeSupport<MembersType>::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<char *>(static_cast<const char *>(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<uint8_t *>(field) ? true : false);
} else {
serialize_field<bool>(member, field, ser);
}
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE:
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
serialize_field<uint8_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
serialize_field<char>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32:
serialize_field<float>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
serialize_field<double>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
serialize_field<int16_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
serialize_field<uint16_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
serialize_field<int32_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
serialize_field<uint32_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
serialize_field<int64_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
serialize_field<uint64_t>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
serialize_field<std::string>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING:
serialize_field<std::wstring>(member, field, ser);
break;
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
{
auto sub_members = static_cast<const MembersType *>(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<char *>(subros_message) + sub_members_size;
subros_message = align_(max_align, subros_message);
}
}
}
break;
default:
throw std::runtime_error("unknown type");
}
}
return true;
}
template<typename T>
void deserialize_field(
const rosidl_typesupport_introspection_cpp::MessageMember * member,
@ -872,26 +646,6 @@ bool TypeSupport<MembersType>::printROSmessage(
return true;
}
template<typename MembersType>
bool TypeSupport<MembersType>::serializeROSmessage(
const void * ros_message, cycser & ser,
std::function<void(cycser &)> 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<typename MembersType>
bool TypeSupport<MembersType>::deserializeROSmessage(
cycdeser & deser, void * ros_message,

9
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp
View file

@ -14,11 +14,12 @@
#ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_
#define RMW_CYCLONEDDS_CPP__SERDATA_HPP_
#include <memory>
#include <string>
#include <vector>
#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<const rmw_cyclonedds_cpp::StructValueType> 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<rmw_cyclonedds_cpp::StructValueType> message_type_support);
struct ddsi_serdata * serdata_rmw_from_serialized_message(
const struct ddsi_sertopic * topiccmn,

123
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp
View file

@ -29,129 +29,6 @@
using rmw_cyclonedds_cpp::DeserializationException;
class cycser
{
public:
explicit cycser(std::vector<unsigned char> & 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<class T>
inline cycser & operator<<(const std::vector<T> & x) {serialize(x); return *this;}
template<class T, size_t S>
inline cycser & operator<<(const std::array<T, S> & 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<T *>(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<unsigned char>(x));
}
inline void serialize(const std::string & x)
{
size_t sz = x.size() + 1;
serialize(static_cast<uint32_t>(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<uint32_t>(sz));
resize(off + sz * sizeof(wchar_t));
memcpy(data() + off, reinterpret_cast<const char *>(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<const void *>(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<class T>
inline void serializeA(const T * x, size_t cnt)
{
for (size_t i = 0; i < cnt; i++) {serialize(x[i]);}
}
template<class T>
inline void serialize(const std::vector<T> & x)
{
serialize(static_cast<uint32_t>(x.size()));
serializeA(x.data(), x.size());
}
inline void serialize(const std::vector<bool> & x)
{
serialize(static_cast<uint32_t>(x.size()));
for (auto && i : x) {serialize(i);}
}
template<class T>
inline void serializeS(const T * x, size_t cnt)
{
serialize(static_cast<uint32_t>(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<unsigned char> & dst;
size_t off;
};
class cycdeserbase
{
public:

508
rmw_cyclonedds_cpp/src/Serialization.cpp Normal file
View file

@ -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 <algorithm>
#include <limits>
#include <unordered_map>
#include <vector>
#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<ptrdiff_t>(offset()) - static_cast<ptrdiff_t>(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<decltype(align)>{} (k.align) ^
((std::hash<decltype(value_type)>{} (k.value_type)) << 1U);
}
};
};
std::unordered_map<CacheKey, bool, CacheKey::Hash> 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<char, 4> 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<uint32_t>::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<const PrimitiveValueType *>(p));
break;
case EValueType::StructValueType:
result = false;
result = is_trivially_serialized(align, *static_cast<const StructValueType *>(p));
break;
case EValueType::ArrayValueType:
result = is_trivially_serialized(align, *static_cast<const ArrayValueType *>(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<float>::is_iec559);
cursor->put_bytes(data, n_bytes);
return;
case ROSIDL_TypeKind::DOUBLE:
assert(std::numeric_limits<double>::is_iec559);
cursor->put_bytes(data, n_bytes);
return;
case ROSIDL_TypeKind::LONG_DOUBLE:
assert(std::numeric_limits<long double>::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<const PrimitiveValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const U8StringValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const U16StringValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const StructValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const ArrayValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const SpanSequenceValueType *>(value_type)) {
return serialize(cursor, data, *s);
}
if (auto s = dynamic_cast<const BoolVectorValueType *>(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<const PrimitiveValueType *>(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

32
rmw_cyclonedds_cpp/src/Serialization.hpp Normal file
View file

@ -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_

201
rmw_cyclonedds_cpp/src/TypeSupport2.cpp Normal file
View file

@ -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 <memory>
#include <string>
#include <utility>
#include <vector>
namespace rmw_cyclonedds_cpp
{
class ROSIDLC_StructValueType : public StructValueType
{
const rosidl_typesupport_introspection_c__MessageMembers * impl;
std::vector<Member> m_members;
std::vector<std::unique_ptr<const AnyValueType>> m_inner_value_types;
template<typename ConstructedType, typename ... Args>
ConstructedType * make_value_type(Args && ... args)
{
auto unique_ptr = std::make_unique<ConstructedType>(std::forward<Args>(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<Member> m_members;
std::vector<std::unique_ptr<const AnyValueType>> m_inner_value_types;
template<typename ConstructedType, typename ... Args>
ConstructedType * make_value_type(Args && ... args)
{
auto unique_ptr = std::make_unique<ConstructedType>(std::forward<Args>(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<StructValueType> make_message_value_type(const rosidl_message_type_support_t * mts)
{
if (auto ts_c = mts->func(mts, TypeGeneratorInfo<TypeGenerator::ROSIDL_C>::identifier)) {
auto members = static_cast<const MetaMessage<TypeGenerator::ROSIDL_C> *>(ts_c->data);
return std::make_unique<ROSIDLC_StructValueType>(members);
}
if (auto ts_cpp = mts->func(mts, TypeGeneratorInfo<TypeGenerator::ROSIDL_Cpp>::identifier)) {
auto members = static_cast<const MetaMessage<TypeGenerator::ROSIDL_Cpp> *>(ts_cpp->data);
return std::make_unique<ROSIDLCPP_StructValueType>(members);
}
throw std::runtime_error(
"could not identify message typesupport " + std::string(mts->typesupport_identifier));
}
std::pair<std::unique_ptr<StructValueType>, std::unique_ptr<StructValueType>>
make_request_response_value_types(const rosidl_service_type_support_t * svc_ts)
{
if (auto tsc = svc_ts->func(svc_ts, TypeGeneratorInfo<TypeGenerator::ROSIDL_C>::identifier)) {
auto typed =
static_cast<const TypeGeneratorInfo<TypeGenerator::ROSIDL_C>::MetaService *>(tsc->data);
return {
std::make_unique<ROSIDLC_StructValueType>(typed->request_members_),
std::make_unique<ROSIDLC_StructValueType>(typed->response_members_)
};
}
if (auto tscpp =
svc_ts->func(svc_ts, TypeGeneratorInfo<TypeGenerator::ROSIDL_Cpp>::identifier))
{
auto typed =
static_cast<const TypeGeneratorInfo<TypeGenerator::ROSIDL_Cpp>::MetaService *>(tscpp->data);
return {
std::make_unique<ROSIDLCPP_StructValueType>(typed->request_members_),
std::make_unique<ROSIDLCPP_StructValueType>(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<ROSIDLC_StringValueType>();
break;
case ROSIDL_TypeKind::WSTRING:
element_value_type = make_value_type<ROSIDLC_WStringValueType>();
break;
default:
element_value_type =
make_value_type<PrimitiveValueType>(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<ArrayValueType>(element_value_type,
member_impl.array_size_);
} else if (member_impl.size_function) {
member_value_type = make_value_type<CallbackSpanSequenceValueType>(element_value_type,
member_impl.size_function,
member_impl.get_const_function);
} else {
member_value_type = make_value_type<ROSIDLC_SpanSequenceValueType>(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<ROSIDLCPP_StringValueType>();
break;
case ROSIDL_TypeKind::WSTRING:
element_value_type = make_value_type<ROSIDLCPP_U16StringValueType>();
break;
default:
element_value_type =
make_value_type<PrimitiveValueType>(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<ArrayValueType>(element_value_type,
member_impl.array_size_);
} else if (ROSIDL_TypeKind(member_impl.type_id_) == ROSIDL_TypeKind::BOOLEAN) {
member_value_type = make_value_type<BoolVectorValueType>();
} else {
member_value_type = make_value_type<CallbackSpanSequenceValueType>(
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

510
rmw_cyclonedds_cpp/src/TypeSupport2.hpp Normal file
View file

@ -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 <cassert>
#include <memory>
#include <regex>
#include <string>
#include <utility>
#include <vector>
#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<typename T>
class TypedSpan;
template<typename T>
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<typename NativeType>
auto make_typed_span(const NativeType * m_data, size_t size)
{
return TypedSpan<NativeType>{m_data, size};
}
enum class TypeGenerator
{
ROSIDL_C,
ROSIDL_Cpp,
};
template<TypeGenerator>
struct TypeGeneratorInfo;
template<>
struct TypeGeneratorInfo<TypeGenerator::ROSIDL_C>
{
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<TypeGenerator::ROSIDL_Cpp>
{
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<TypeGenerator g>
using MetaMessage = typename TypeGeneratorInfo<g>::MetaMessage;
template<TypeGenerator g>
using MetaMember = typename TypeGeneratorInfo<g>::MetaMember;
template<TypeGenerator g>
using MetaService = typename TypeGeneratorInfo<g>::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<StructValueType> make_message_value_type(const rosidl_message_type_support_t * mts);
std::pair<std::unique_ptr<StructValueType>, std::unique_ptr<StructValueType>>
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<typename UnaryFunction>
auto apply(UnaryFunction f) const;
template<typename UnaryFunction>
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<size_t(const void *)> m_size_function;
std::function<const void * (const void *, size_t index)> 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<const ROSIDLC_SequenceObject *>(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<ROSIDL_TypeKind>(m_type_kind)));
}
}
EValueType e_value_type() const override {return EValueType::PrimitiveValueType;}
};
class BoolVectorValueType : public AnyValueType
{
protected:
const std::vector<bool> * get_value(const void * ptr_to_sequence) const
{
return static_cast<const std::vector<bool> *>(ptr_to_sequence);
}
static std::unique_ptr<PrimitiveValueType> s_element_value_type;
public:
size_t sizeof_type() const override {return sizeof(std::vector<bool>);}
static const AnyValueType * element_value_type()
{
if (!s_element_value_type) {
s_element_value_type = std::make_unique<PrimitiveValueType>(ROSIDL_TypeKind::BOOLEAN);
}
return s_element_value_type.get();
}
std::vector<bool>::const_iterator begin(const void * ptr_to_sequence) const
{
return get_value(ptr_to_sequence)->begin();
}
std::vector<bool>::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<char>;
virtual TypedSpan<char_traits::char_type> data(void *) const = 0;
virtual TypedSpan<const char_traits::char_type> 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<char16_t>;
virtual TypedSpan<char_traits::char_type> data(void *) const = 0;
virtual TypedSpan<const char_traits::char_type> 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<const char_traits::char_type> data(const void * ptr) const override
{
auto str = static_cast<const type *>(ptr);
assert(str->capacity == str->size + 1);
assert(str->data[str->size] == '\0');
return {str->data, str->size};
}
TypedSpan<char_traits::char_type> data(void * ptr) const override
{
auto str = static_cast<type *>(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<const char_traits::char_type> data(const void * ptr) const override
{
auto str = static_cast<const type *>(ptr);
return {reinterpret_cast<const char_traits::char_type *>(str->data), str->size};
}
TypedSpan<char_traits::char_type> data(void * ptr) const override
{
auto str = static_cast<type *>(ptr);
return {reinterpret_cast<char_traits::char_type *>(str->data), str->size};
}
size_t sizeof_type() const override {return sizeof(type);}
};
class ROSIDLCPP_StringValueType : public U8StringValueType
{
public:
using type = std::string;
TypedSpan<const char_traits::char_type> data(const void * ptr) const override
{
auto str = static_cast<const type *>(ptr);
return {str->data(), str->size()};
}
TypedSpan<char_traits::char_type> data(void * ptr) const override
{
auto str = static_cast<type *>(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<const char_traits::char_type> data(const void * ptr) const override
{
auto str = static_cast<const type *>(ptr);
return {str->data(), str->size()};
}
TypedSpan<char_traits::char_type> data(void * ptr) const override
{
auto str = static_cast<type *>(ptr);
return {str->data(), str->size()};
}
size_t sizeof_type() const override {return sizeof(type);}
};
template<typename UnaryFunction>
auto AnyValueType::apply(UnaryFunction f) const
{
switch (e_value_type()) {
case EValueType::PrimitiveValueType:
return f(*static_cast<const PrimitiveValueType *>(this));
case EValueType::U8StringValueType:
return f(*static_cast<const U8StringValueType *>(this));
case EValueType::U16StringValueType:
return f(*static_cast<const U16StringValueType *>(this));
case EValueType::StructValueType:
return f(*static_cast<const StructValueType *>(this));
case EValueType::ArrayValueType:
return f(*static_cast<const ArrayValueType *>(this));
case EValueType::SpanSequenceValueType:
return f(*static_cast<const SpanSequenceValueType *>(this));
case EValueType::BoolVectorValueType:
return f(*static_cast<const BoolVectorValueType *>(this));
}
}
template<typename UnaryFunction>
auto AnyValueType::apply(UnaryFunction f)
{
switch (e_value_type()) {
case EValueType::PrimitiveValueType:
return f(*static_cast<PrimitiveValueType *>(this));
case EValueType::U8StringValueType:
return f(*static_cast<U8StringValueType *>(this));
case EValueType::U16StringValueType:
return f(*static_cast<U16StringValueType *>(this));
case EValueType::StructValueType:
return f(*static_cast<StructValueType *>(this));
case EValueType::ArrayValueType:
return f(*static_cast<ArrayValueType *>(this));
case EValueType::SpanSequenceValueType:
return f(*static_cast<SpanSequenceValueType *>(this));
case EValueType::BoolVectorValueType:
return f(*static_cast<BoolVectorValueType *>(this));
}
}
} // namespace rmw_cyclonedds_cpp
#endif // TYPESUPPORT2_HPP_

41
rmw_cyclonedds_cpp/src/bytewise.hpp Normal file
View file

@ -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 <cstdint>
#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<void *>(static_cast<byte *>(ptr) + n);
}
static inline auto byte_offset(const void * ptr, ptrdiff_t n)
{
return static_cast<const void *>(static_cast<const byte *>(ptr) + n);
}
#endif // BYTEWISE_HPP_

69
rmw_cyclonedds_cpp/src/rmw_node.cpp
View file

@ -19,6 +19,7 @@
#include <set>
#include <functional>
#include <atomic>
#include <memory>
#include <vector>
#include <string>
#include <utility>
@ -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<void>(type_support);
static_cast<void>(message_bounds);
static_cast<void>(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<unsigned char> 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<const rosidl_typesupport_introspection_c__MessageMembers *>(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<const rosidl_typesupport_introspection_cpp::MessageMembers *>(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<rmw_cyclonedds_cpp::StructValueType> 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 =

79
rmw_cyclonedds_cpp/src/serdata.cpp
View file

@ -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 <string.h>
#include "rmw_cyclonedds_cpp/serdata.hpp"
#include <vector>
#include <cstring>
#include <memory>
#include <regex>
#include <sstream>
#include <string>
#include <utility>
#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<rosidl_typesupport_introspection_c__MessageMembers>;
@ -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<MessageTypeSupport_c *>(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<MessageTypeSupport_cpp *>(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<const cdds_request_wrapper_t *>(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<MessageTypeSupport_c *>(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<MessageTypeSupport_cpp *>(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<const cdds_request_wrapper_t *>(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<const struct sertopic_rmw *>(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<rmw_cyclonedds_cpp::StructValueType> 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;
}

16
rmw_cyclonedds_cpp/src/serdes.cpp
View file

@ -12,26 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <exception>
#include <vector>
#include "dds/ddsrt/endian.h"
#include "rmw_cyclonedds_cpp/serdes.hpp"
cycser::cycser(std::vector<unsigned char> & 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),