Rework serialization (#42)
* Rework serialization and typesupport Signed-off-by: Dan Rose <dan@digilabs.io>
This commit is contained in:
Dan Rose
GitHub
parent
3e7d33ba2b
commit
88e74c322f
13 changed files with 1366 additions and 475 deletions
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue