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
|
|
@ -57,7 +57,8 @@ add_library(rmw_cyclonedds_cpp
|
||||||
src/u16string.cpp
|
src/u16string.cpp
|
||||||
src/exception.cpp
|
src/exception.cpp
|
||||||
src/deserialization_exception.cpp
|
src/deserialization_exception.cpp
|
||||||
)
|
src/Serialization.cpp
|
||||||
|
src/TypeSupport2.cpp)
|
||||||
|
|
||||||
target_include_directories(rmw_cyclonedds_cpp PUBLIC
|
target_include_directories(rmw_cyclonedds_cpp PUBLIC
|
||||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||||
|
|
|
||||||
|
|
@ -112,9 +112,6 @@ template<typename MembersType>
|
||||||
class TypeSupport
|
class TypeSupport
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
bool serializeROSmessage(
|
|
||||||
const void * ros_message, cycser & ser,
|
|
||||||
std::function<void(cycser &)> prefix = nullptr);
|
|
||||||
bool deserializeROSmessage(
|
bool deserializeROSmessage(
|
||||||
cycdeser & deser, void * ros_message,
|
cycdeser & deser, void * ros_message,
|
||||||
std::function<void(cycdeser &)> prefix = nullptr);
|
std::function<void(cycdeser &)> prefix = nullptr);
|
||||||
|
|
@ -132,7 +129,6 @@ protected:
|
||||||
std::string name;
|
std::string name;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message);
|
|
||||||
bool deserializeROSmessage(
|
bool deserializeROSmessage(
|
||||||
cycdeser & deser, const MembersType * members, void * ros_message,
|
cycdeser & deser, const MembersType * members, void * ros_message,
|
||||||
bool call_new);
|
bool call_new);
|
||||||
|
|
|
||||||
|
|
@ -204,136 +204,6 @@ static size_t calculateMaxAlign(const MembersType * members)
|
||||||
return max_align;
|
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
|
inline
|
||||||
size_t get_array_size_and_assign_field(
|
size_t get_array_size_and_assign_field(
|
||||||
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
||||||
|
|
@ -367,102 +237,6 @@ size_t get_array_size_and_assign_field(
|
||||||
return tmpsequence->size;
|
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>
|
template<typename T>
|
||||||
void deserialize_field(
|
void deserialize_field(
|
||||||
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
||||||
|
|
@ -872,26 +646,6 @@ bool TypeSupport<MembersType>::printROSmessage(
|
||||||
return true;
|
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>
|
template<typename MembersType>
|
||||||
bool TypeSupport<MembersType>::deserializeROSmessage(
|
bool TypeSupport<MembersType>::deserializeROSmessage(
|
||||||
cycdeser & deser, void * ros_message,
|
cycdeser & deser, void * ros_message,
|
||||||
|
|
|
||||||
|
|
@ -14,11 +14,12 @@
|
||||||
#ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_
|
#ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_
|
||||||
#define RMW_CYCLONEDDS_CPP__SERDATA_HPP_
|
#define RMW_CYCLONEDDS_CPP__SERDATA_HPP_
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
#include "dds/ddsi/ddsi_sertopic.h"
|
|
||||||
#include "dds/ddsi/ddsi_serdata.h"
|
#include "dds/ddsi/ddsi_serdata.h"
|
||||||
|
#include "dds/ddsi/ddsi_sertopic.h"
|
||||||
|
|
||||||
struct CddsTypeSupport
|
struct CddsTypeSupport
|
||||||
{
|
{
|
||||||
|
|
@ -26,6 +27,8 @@ struct CddsTypeSupport
|
||||||
const char * typesupport_identifier_;
|
const char * typesupport_identifier_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
namespace rmw_cyclonedds_cpp {class StructValueType;}
|
||||||
|
|
||||||
struct sertopic_rmw : ddsi_sertopic
|
struct sertopic_rmw : ddsi_sertopic
|
||||||
{
|
{
|
||||||
CddsTypeSupport type_support;
|
CddsTypeSupport type_support;
|
||||||
|
|
@ -35,6 +38,7 @@ struct sertopic_rmw : ddsi_sertopic
|
||||||
std::string cpp_type_name;
|
std::string cpp_type_name;
|
||||||
std::string cpp_name_type_name;
|
std::string cpp_name_type_name;
|
||||||
#endif
|
#endif
|
||||||
|
std::unique_ptr<const rmw_cyclonedds_cpp::StructValueType> value_type;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct serdata_rmw : ddsi_serdata
|
struct serdata_rmw : ddsi_serdata
|
||||||
|
|
@ -68,7 +72,8 @@ void * create_response_type_support(
|
||||||
|
|
||||||
struct sertopic_rmw * create_sertopic(
|
struct sertopic_rmw * create_sertopic(
|
||||||
const char * topicname, const char * type_support_identifier,
|
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(
|
struct ddsi_serdata * serdata_rmw_from_serialized_message(
|
||||||
const struct ddsi_sertopic * topiccmn,
|
const struct ddsi_sertopic * topiccmn,
|
||||||
|
|
|
||||||
|
|
@ -29,129 +29,6 @@
|
||||||
|
|
||||||
using rmw_cyclonedds_cpp::DeserializationException;
|
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
|
class cycdeserbase
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
|
|
||||||
508
rmw_cyclonedds_cpp/src/Serialization.cpp
Normal file
508
rmw_cyclonedds_cpp/src/Serialization.cpp
Normal 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
32
rmw_cyclonedds_cpp/src/Serialization.hpp
Normal 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
201
rmw_cyclonedds_cpp/src/TypeSupport2.cpp
Normal 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
510
rmw_cyclonedds_cpp/src/TypeSupport2.hpp
Normal 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
41
rmw_cyclonedds_cpp/src/bytewise.hpp
Normal 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_
|
||||||
|
|
@ -19,6 +19,7 @@
|
||||||
#include <set>
|
#include <set>
|
||||||
#include <functional>
|
#include <functional>
|
||||||
#include <atomic>
|
#include <atomic>
|
||||||
|
#include <memory>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <utility>
|
#include <utility>
|
||||||
|
|
@ -40,8 +41,11 @@
|
||||||
#include "rmw/rmw.h"
|
#include "rmw/rmw.h"
|
||||||
#include "rmw/sanity_checks.h"
|
#include "rmw/sanity_checks.h"
|
||||||
|
|
||||||
|
#include "Serialization.hpp"
|
||||||
#include "rmw/impl/cpp/macros.hpp"
|
#include "rmw/impl/cpp/macros.hpp"
|
||||||
|
|
||||||
|
#include "TypeSupport2.hpp"
|
||||||
|
|
||||||
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
|
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
|
||||||
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.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>(type_support);
|
||||||
static_cast<void>(message_bounds);
|
static_cast<void>(message_bounds);
|
||||||
static_cast<void>(size);
|
static_cast<void>(size);
|
||||||
|
|
||||||
RMW_SET_ERROR_MSG("rmw_get_serialized_message_size: unimplemented");
|
RMW_SET_ERROR_MSG("rmw_get_serialized_message_size: unimplemented");
|
||||||
return RMW_RET_ERROR;
|
return RMW_RET_ERROR;
|
||||||
}
|
}
|
||||||
|
|
@ -855,39 +860,22 @@ extern "C" rmw_ret_t rmw_serialize(
|
||||||
const rosidl_message_type_support_t * type_support,
|
const rosidl_message_type_support_t * type_support,
|
||||||
rmw_serialized_message_t * serialized_message)
|
rmw_serialized_message_t * serialized_message)
|
||||||
{
|
{
|
||||||
std::vector<unsigned char> data;
|
|
||||||
cycser sd(data);
|
|
||||||
rmw_ret_t ret;
|
rmw_ret_t ret;
|
||||||
const rosidl_message_type_support_t * ts;
|
try {
|
||||||
if ((ts =
|
auto ts = rmw_cyclonedds_cpp::make_message_value_type(type_support);
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if ((ret = rmw_serialized_message_resize(serialized_message, data.size())) != RMW_RET_OK) {
|
auto size = rmw_cyclonedds_cpp::get_serialized_size(ros_message, ts.get());
|
||||||
return ret;
|
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(
|
extern "C" rmw_ret_t rmw_deserialize(
|
||||||
|
|
@ -1226,7 +1214,8 @@ static CddsPublisher * create_cdds_publisher(
|
||||||
|
|
||||||
auto sertopic = create_sertopic(
|
auto sertopic = create_sertopic(
|
||||||
fqtopic_name.c_str(), type_support->typesupport_identifier,
|
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 =
|
if ((topic =
|
||||||
dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0)
|
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(
|
auto sertopic = create_sertopic(
|
||||||
fqtopic_name.c_str(), type_support->typesupport_identifier,
|
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 =
|
if ((topic =
|
||||||
dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0)
|
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();
|
auto sub = new CddsSubscription();
|
||||||
std::string subtopic_name, pubtopic_name;
|
std::string subtopic_name, pubtopic_name;
|
||||||
void * pub_type_support, * sub_type_support;
|
void * pub_type_support, * sub_type_support;
|
||||||
|
|
||||||
|
std::unique_ptr<rmw_cyclonedds_cpp::StructValueType> pub_msg_ts, sub_msg_ts;
|
||||||
|
|
||||||
if (is_service) {
|
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,
|
sub_type_support = create_request_type_support(type_support->data,
|
||||||
type_support->typesupport_identifier);
|
type_support->typesupport_identifier);
|
||||||
pub_type_support = create_response_type_support(type_support->data,
|
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);
|
make_fqtopic(ros_service_requester_prefix, service_name, "Request", qos_policies);
|
||||||
pubtopic_name = make_fqtopic(ros_service_response_prefix, service_name, "Reply", qos_policies);
|
pubtopic_name = make_fqtopic(ros_service_response_prefix, service_name, "Reply", qos_policies);
|
||||||
} else {
|
} 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,
|
pub_type_support = create_request_type_support(type_support->data,
|
||||||
type_support->typesupport_identifier);
|
type_support->typesupport_identifier);
|
||||||
sub_type_support = create_response_type_support(type_support->data,
|
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;
|
dds_entity_t pubtopic, subtopic;
|
||||||
|
|
||||||
auto pub_st = create_sertopic(
|
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(
|
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;
|
dds_qos_t * qos;
|
||||||
if ((pubtopic =
|
if ((pubtopic =
|
||||||
|
|
|
||||||
|
|
@ -11,23 +11,22 @@
|
||||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
#include <string.h>
|
#include "rmw_cyclonedds_cpp/serdata.hpp"
|
||||||
|
|
||||||
#include <vector>
|
#include <cstring>
|
||||||
|
#include <memory>
|
||||||
#include <regex>
|
#include <regex>
|
||||||
#include <sstream>
|
#include <sstream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <utility>
|
||||||
|
|
||||||
|
#include "TypeSupport2.hpp"
|
||||||
|
#include "Serialization.hpp"
|
||||||
|
#include "dds/ddsi/q_radmin.h"
|
||||||
#include "rmw/error_handling.h"
|
#include "rmw/error_handling.h"
|
||||||
|
|
||||||
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
|
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
|
||||||
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
|
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
|
||||||
|
|
||||||
#include "rmw_cyclonedds_cpp/serdes.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 =
|
using MessageTypeSupport_c =
|
||||||
rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>;
|
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);
|
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(
|
static struct ddsi_serdata * serdata_rmw_from_sample(
|
||||||
const struct ddsi_sertopic * topiccmn,
|
const struct ddsi_sertopic * topiccmn,
|
||||||
enum ddsi_serdata_kind kind,
|
enum ddsi_serdata_kind kind,
|
||||||
|
|
@ -172,43 +178,21 @@ static struct ddsi_serdata * serdata_rmw_from_sample(
|
||||||
if (kind != SDK_DATA) {
|
if (kind != SDK_DATA) {
|
||||||
/* ROS2 doesn't do keys, so SDK_KEY is trivial */
|
/* ROS2 doesn't do keys, so SDK_KEY is trivial */
|
||||||
} else if (!topic->is_request_header) {
|
} else if (!topic->is_request_header) {
|
||||||
cycser sd(d->data);
|
size_t sz = rmw_cyclonedds_cpp::get_serialized_size(sample, topic->value_type.get());
|
||||||
if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) {
|
d->data.resize(next_multiple_of_4(sz));
|
||||||
auto typed_typesupport =
|
rmw_cyclonedds_cpp::serialize(d->data.data(), sample, topic->value_type.get());
|
||||||
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);
|
|
||||||
}
|
|
||||||
} else {
|
} else {
|
||||||
/* The "prefix" lambda is there to inject the service invocation header data into the CDR
|
/* inject the service invocation header data into the CDR stream --
|
||||||
stream -- I haven't checked how it is done in the official RMW implementations, so it is
|
* I haven't checked how it is done in the official RMW implementations, so it is
|
||||||
probably incompatible. */
|
* probably incompatible. */
|
||||||
const cdds_request_wrapper_t * wrap = static_cast<const cdds_request_wrapper_t *>(sample);
|
auto 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);
|
size_t sz = rmw_cyclonedds_cpp::get_serialized_size(wrap, topic->value_type.get());
|
||||||
if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) {
|
d->data.resize(next_multiple_of_4(sz));
|
||||||
auto typed_typesupport =
|
rmw_cyclonedds_cpp::serialize(d->data.data(), wrap, topic->value_type.get());
|
||||||
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);
|
|
||||||
}
|
}
|
||||||
return d;
|
return d;
|
||||||
} catch (rmw_cyclonedds_cpp::Exception & e) {
|
} catch (std::exception & e) {
|
||||||
RMW_SET_ERROR_MSG(e.what());
|
|
||||||
return nullptr;
|
|
||||||
} catch (std::runtime_error & e) {
|
|
||||||
RMW_SET_ERROR_MSG(e.what());
|
RMW_SET_ERROR_MSG(e.what());
|
||||||
return nullptr;
|
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);
|
const struct sertopic_rmw * topic = static_cast<const struct sertopic_rmw *>(topiccmn);
|
||||||
struct serdata_rmw * d = rmw_serdata_new(topic, SDK_DATA);
|
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
|
d->data.resize(next_multiple_of_4(size));
|
||||||
when copying data to network. Should fix Cyclone to handle that more elegantly. */
|
|
||||||
d->data.resize(size);
|
|
||||||
memcpy(d->data.data(), raw, size);
|
memcpy(d->data.data(), raw, size);
|
||||||
while (d->data.size() % 4) {
|
|
||||||
d->data.push_back(0);
|
|
||||||
}
|
|
||||||
return d;
|
return d;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -493,7 +472,8 @@ static std::string get_type_name(const char * type_support_identifier, void * ty
|
||||||
|
|
||||||
struct sertopic_rmw * create_sertopic(
|
struct sertopic_rmw * create_sertopic(
|
||||||
const char * topicname, const char * type_support_identifier,
|
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;
|
struct sertopic_rmw * st = new struct sertopic_rmw;
|
||||||
#if DDSI_SERTOPIC_HAS_TOPICKIND_NO_KEY
|
#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.typesupport_identifier_ = type_support_identifier;
|
||||||
st->type_support.type_support_ = type_support;
|
st->type_support.type_support_ = type_support;
|
||||||
st->is_request_header = is_request_header;
|
st->is_request_header = is_request_header;
|
||||||
|
st->value_type = std::move(message_type);
|
||||||
return st;
|
return st;
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -12,26 +12,10 @@
|
||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
#include <exception>
|
#include <exception>
|
||||||
#include <vector>
|
|
||||||
|
|
||||||
#include "dds/ddsrt/endian.h"
|
#include "dds/ddsrt/endian.h"
|
||||||
#include "rmw_cyclonedds_cpp/serdes.hpp"
|
#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_)
|
cycdeserbase::cycdeserbase(const char * data_, size_t size_)
|
||||||
: data(data_),
|
: data(data_),
|
||||||
pos(0),
|
pos(0),
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue