replace FastCDR and serialise straight into a serdata to avoid an extra copy
This commit is contained in:
Erik Boasson
parent
e6b6ede709
commit
40a042c6dc
12 changed files with 391 additions and 210 deletions
|
|
@ -32,7 +32,6 @@ find_package(ament_cmake_ros REQUIRED)
|
|||
find_package(rcutils REQUIRED)
|
||||
|
||||
find_package(cyclonedds_cmake_module REQUIRED)
|
||||
find_package(fastcdr REQUIRED CONFIG)
|
||||
find_package(CycloneDDS REQUIRED CONFIG)
|
||||
|
||||
find_package(rmw REQUIRED)
|
||||
|
|
@ -53,11 +52,12 @@ include_directories(include)
|
|||
add_library(rmw_cyclonedds_cpp
|
||||
src/namespace_prefix.cpp
|
||||
src/rmw_node.cpp
|
||||
src/serdes.cpp
|
||||
)
|
||||
|
||||
idlc_generate(rmw_cyclonedds_topic_lib src/rmw_cyclonedds_topic.idl)
|
||||
target_link_libraries(rmw_cyclonedds_cpp
|
||||
fastcdr rmw_cyclonedds_topic_lib CycloneDDS::ddsc)
|
||||
rmw_cyclonedds_topic_lib CycloneDDS::ddsc)
|
||||
|
||||
ament_target_dependencies(rmw_cyclonedds_cpp
|
||||
"rcutils"
|
||||
|
|
|
|||
|
|
@ -15,9 +15,6 @@
|
|||
#ifndef RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_HPP_
|
||||
#define RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_HPP_
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <memory>
|
||||
|
||||
|
|
|
|||
|
|
@ -15,9 +15,6 @@
|
|||
#ifndef RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_
|
||||
#define RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
|
@ -37,14 +34,6 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(const MembersType * members)
|
|||
std::string name = std::string(members->package_name_) + "::msg::dds_::" +
|
||||
members->message_name_ + "_";
|
||||
this->setName(name.c_str());
|
||||
|
||||
// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
|
||||
// maximum serialized size of the message, when the message is a bounded one.
|
||||
if (members->member_count_ != 0) {
|
||||
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(members, 0));
|
||||
} else {
|
||||
this->m_typeSize = 4;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rmw_cyclonedds_cpp
|
||||
|
|
|
|||
|
|
@ -15,8 +15,6 @@
|
|||
#ifndef RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_HPP_
|
||||
#define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_HPP_
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
#include <cassert>
|
||||
|
||||
#include "TypeSupport.hpp"
|
||||
|
|
|
|||
|
|
@ -15,8 +15,6 @@
|
|||
#ifndef RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_
|
||||
#define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
#include <cassert>
|
||||
#include <string>
|
||||
|
||||
|
|
@ -41,14 +39,6 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
|
|||
std::string name = std::string(members->package_name_) + "::srv::dds_::" +
|
||||
members->service_name_ + "_Request_";
|
||||
this->setName(name.c_str());
|
||||
|
||||
// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
|
||||
// maximum serialized size of the message, when the message is a bounded one.
|
||||
if (this->members_->member_count_ != 0) {
|
||||
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
|
||||
} else {
|
||||
this->m_typeSize = 4;
|
||||
}
|
||||
}
|
||||
|
||||
template<typename ServiceMembersType, typename MessageMembersType>
|
||||
|
|
@ -61,14 +51,6 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
|
|||
std::string name = std::string(members->package_name_) + "::srv::dds_::" +
|
||||
members->service_name_ + "_Response_";
|
||||
this->setName(name.c_str());
|
||||
|
||||
// TODO(wjwwood): this could be more intelligent, setting m_typeSize to the
|
||||
// maximum serialized size of the message, when the message is a bounded one.
|
||||
if (this->members_->member_count_ != 0) {
|
||||
this->m_typeSize = static_cast<uint32_t>(this->calculateMaxSerializedSize(this->members_, 0));
|
||||
} else {
|
||||
this->m_typeSize = 4;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rmw_cyclonedds_cpp
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
// Copyright 2018 ADLINK Technology
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
|
|
@ -18,8 +19,6 @@
|
|||
#include <rosidl_generator_c/string.h>
|
||||
#include <rosidl_generator_c/string_functions.h>
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
#include <cassert>
|
||||
#include <string>
|
||||
|
||||
|
|
@ -37,16 +36,18 @@
|
|||
#include "rosidl_typesupport_introspection_c/service_introspection.h"
|
||||
#include "rosidl_typesupport_introspection_c/visibility_control.h"
|
||||
|
||||
#include "serdes.hpp"
|
||||
|
||||
namespace rmw_cyclonedds_cpp
|
||||
{
|
||||
|
||||
// Helper class that uses template specialization to read/write string types to/from a
|
||||
// eprosima::fastcdr::Cdr
|
||||
// cycser/cycdeser
|
||||
template<typename MembersType>
|
||||
struct StringHelper;
|
||||
|
||||
// For C introspection typesupport we create intermediate instances of std::string so that
|
||||
// eprosima::fastcdr::Cdr can handle the string properly.
|
||||
// cycser/cycdeser can handle the string properly.
|
||||
template<>
|
||||
struct StringHelper<rosidl_typesupport_introspection_c__MessageMembers>
|
||||
{
|
||||
|
|
@ -75,7 +76,7 @@ struct StringHelper<rosidl_typesupport_introspection_c__MessageMembers>
|
|||
return std::string(data.data);
|
||||
}
|
||||
|
||||
static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool)
|
||||
static void assign(cycdeser & deser, void * field, bool)
|
||||
{
|
||||
std::string str;
|
||||
deser >> str;
|
||||
|
|
@ -95,7 +96,7 @@ struct StringHelper<rosidl_typesupport_introspection_cpp::MessageMembers>
|
|||
return *(static_cast<std::string *>(data));
|
||||
}
|
||||
|
||||
static void assign(eprosima::fastcdr::Cdr & deser, void * field, bool call_new)
|
||||
static void assign(cycdeser & deser, void * field, bool call_new)
|
||||
{
|
||||
std::string & str = *(std::string *)field;
|
||||
if (call_new) {
|
||||
|
|
@ -109,29 +110,20 @@ template<typename MembersType>
|
|||
class TypeSupport
|
||||
{
|
||||
public:
|
||||
bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser, std::function<void(eprosima::fastcdr::Cdr&)> prefix = nullptr);
|
||||
|
||||
bool deserializeROSmessage(eprosima::fastcdr::FastBuffer * data, void * ros_message);
|
||||
|
||||
bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message, std::function<void(eprosima::fastcdr::Cdr&)> prefix = nullptr);
|
||||
bool serializeROSmessage(const void * ros_message, cycser & ser, std::function<void(cycser&)> prefix = nullptr);
|
||||
bool deserializeROSmessage(cycdeser & deser, void * ros_message, std::function<void(cycdeser&)> prefix = nullptr);
|
||||
|
||||
protected:
|
||||
TypeSupport();
|
||||
|
||||
size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment);
|
||||
void setName(const std::string& name);
|
||||
|
||||
const MembersType * members_;
|
||||
std::string name;
|
||||
size_t m_typeSize;
|
||||
|
||||
private:
|
||||
bool serializeROSmessage(
|
||||
eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message);
|
||||
|
||||
bool deserializeROSmessage(
|
||||
eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message,
|
||||
bool call_new);
|
||||
bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message);
|
||||
bool deserializeROSmessage(cycdeser & deser, const MembersType * members, void * ros_message, bool call_new);
|
||||
};
|
||||
|
||||
} // namespace rmw_cyclonedds_cpp
|
||||
|
|
|
|||
|
|
@ -1,4 +1,5 @@
|
|||
// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
// Copyright 2018 ADLINK Technology
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
|
|
@ -15,8 +16,6 @@
|
|||
#ifndef RMW_CYCLONEDDS_CPP__TYPESUPPORT_IMPL_HPP_
|
||||
#define RMW_CYCLONEDDS_CPP__TYPESUPPORT_IMPL_HPP_
|
||||
|
||||
#include <fastcdr/FastBuffer.h>
|
||||
#include <fastcdr/Cdr.h>
|
||||
#include <cassert>
|
||||
#include <functional>
|
||||
#include <string>
|
||||
|
|
@ -33,6 +32,8 @@
|
|||
|
||||
#include "rosidl_generator_c/primitives_array_functions.h"
|
||||
|
||||
#include "serdes.hpp"
|
||||
|
||||
namespace rmw_cyclonedds_cpp
|
||||
{
|
||||
|
||||
|
|
@ -109,7 +110,6 @@ template<typename MembersType>
|
|||
TypeSupport<MembersType>::TypeSupport()
|
||||
{
|
||||
name = "";
|
||||
m_typeSize = 0;
|
||||
}
|
||||
|
||||
template<typename MembersType>
|
||||
|
|
@ -207,12 +207,12 @@ template<typename T>
|
|||
void serialize_field(
|
||||
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & ser)
|
||||
cycser & ser)
|
||||
{
|
||||
if (!member->is_array_) {
|
||||
ser << *static_cast<T *>(field);
|
||||
} else if (member->array_size_ && !member->is_upper_bound_) {
|
||||
ser.serializeArray(static_cast<T *>(field), member->array_size_);
|
||||
ser.serializeA(static_cast<T *>(field), member->array_size_);
|
||||
} else {
|
||||
std::vector<T> & data = *reinterpret_cast<std::vector<T> *>(field);
|
||||
ser << data;
|
||||
|
|
@ -224,15 +224,15 @@ template<typename T>
|
|||
void serialize_field(
|
||||
const rosidl_typesupport_introspection_c__MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & ser)
|
||||
cycser & ser)
|
||||
{
|
||||
if (!member->is_array_) {
|
||||
ser << *static_cast<T *>(field);
|
||||
} else if (member->array_size_ && !member->is_upper_bound_) {
|
||||
ser.serializeArray(static_cast<T *>(field), member->array_size_);
|
||||
ser.serializeA(static_cast<T *>(field), member->array_size_);
|
||||
} else {
|
||||
auto & data = *reinterpret_cast<typename GenericCArray<T>::type *>(field);
|
||||
ser.serializeSequence(reinterpret_cast<T *>(data.data), data.size);
|
||||
ser.serializeS(reinterpret_cast<T *>(data.data), data.size);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -241,7 +241,7 @@ inline
|
|||
void serialize_field<std::string>(
|
||||
const rosidl_typesupport_introspection_c__MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & ser)
|
||||
cycser & ser)
|
||||
{
|
||||
using CStringHelper = StringHelper<rosidl_typesupport_introspection_c__MessageMembers>;
|
||||
if (!member->is_array_) {
|
||||
|
|
@ -310,7 +310,7 @@ size_t get_array_size_and_assign_field(
|
|||
|
||||
template<typename MembersType>
|
||||
bool TypeSupport<MembersType>::serializeROSmessage(
|
||||
eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message)
|
||||
cycser & ser, const MembersType * members, const void * ros_message)
|
||||
{
|
||||
assert(members);
|
||||
assert(ros_message);
|
||||
|
|
@ -405,13 +405,13 @@ template<typename T>
|
|||
void deserialize_field(
|
||||
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & deser,
|
||||
cycdeser & deser,
|
||||
bool call_new)
|
||||
{
|
||||
if (!member->is_array_) {
|
||||
deser >> *static_cast<T *>(field);
|
||||
} else if (member->array_size_ && !member->is_upper_bound_) {
|
||||
deser.deserializeArray(static_cast<T *>(field), member->array_size_);
|
||||
deser.deserializeA(static_cast<T *>(field), member->array_size_);
|
||||
} else {
|
||||
auto & vector = *reinterpret_cast<std::vector<T> *>(field);
|
||||
if (call_new) {
|
||||
|
|
@ -425,20 +425,20 @@ template<typename T>
|
|||
void deserialize_field(
|
||||
const rosidl_typesupport_introspection_c__MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & deser,
|
||||
cycdeser & deser,
|
||||
bool call_new)
|
||||
{
|
||||
(void)call_new;
|
||||
if (!member->is_array_) {
|
||||
deser >> *static_cast<T *>(field);
|
||||
} else if (member->array_size_ && !member->is_upper_bound_) {
|
||||
deser.deserializeArray(static_cast<T *>(field), member->array_size_);
|
||||
deser.deserializeA(static_cast<T *>(field), member->array_size_);
|
||||
} else {
|
||||
auto & data = *reinterpret_cast<typename GenericCArray<T>::type *>(field);
|
||||
int32_t dsize = 0;
|
||||
deser >> dsize;
|
||||
GenericCArray<T>::init(&data, dsize);
|
||||
deser.deserializeArray(reinterpret_cast<T *>(data.data), dsize);
|
||||
deser.deserializeA(reinterpret_cast<T *>(data.data), dsize);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -446,7 +446,7 @@ template<>
|
|||
inline void deserialize_field<std::string>(
|
||||
const rosidl_typesupport_introspection_c__MessageMember * member,
|
||||
void * field,
|
||||
eprosima::fastcdr::Cdr & deser,
|
||||
cycdeser & deser,
|
||||
bool call_new)
|
||||
{
|
||||
(void)call_new;
|
||||
|
|
@ -487,7 +487,7 @@ inline void deserialize_field<std::string>(
|
|||
|
||||
inline size_t get_submessage_array_deserialize(
|
||||
const rosidl_typesupport_introspection_cpp::MessageMember * member,
|
||||
eprosima::fastcdr::Cdr & deser,
|
||||
cycdeser & deser,
|
||||
void * field,
|
||||
void * & subros_message,
|
||||
bool call_new,
|
||||
|
|
@ -510,7 +510,7 @@ inline size_t get_submessage_array_deserialize(
|
|||
|
||||
inline size_t get_submessage_array_deserialize(
|
||||
const rosidl_typesupport_introspection_c__MessageMember * member,
|
||||
eprosima::fastcdr::Cdr & deser,
|
||||
cycdeser & deser,
|
||||
void * field,
|
||||
void * & subros_message,
|
||||
bool,
|
||||
|
|
@ -529,7 +529,7 @@ inline size_t get_submessage_array_deserialize(
|
|||
|
||||
template<typename MembersType>
|
||||
bool TypeSupport<MembersType>::deserializeROSmessage(
|
||||
eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message, bool call_new)
|
||||
cycdeser & deser, const MembersType * members, void * ros_message, bool call_new)
|
||||
{
|
||||
assert(members);
|
||||
assert(ros_message);
|
||||
|
|
@ -614,90 +614,13 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
|
|||
return true;
|
||||
}
|
||||
|
||||
template<typename MembersType>
|
||||
size_t TypeSupport<MembersType>::calculateMaxSerializedSize(
|
||||
const MembersType * members, size_t current_alignment)
|
||||
{
|
||||
assert(members);
|
||||
|
||||
size_t initial_alignment = current_alignment;
|
||||
|
||||
// Encapsulation
|
||||
const size_t padding = 4;
|
||||
current_alignment += padding;
|
||||
|
||||
for (uint32_t i = 0; i < members->member_count_; ++i) {
|
||||
const auto * member = members->members_ + i;
|
||||
|
||||
size_t array_size = 1;
|
||||
if (member->is_array_) {
|
||||
array_size = member->array_size_;
|
||||
// Whether it is a sequence.
|
||||
if (array_size == 0 || member->is_upper_bound_) {
|
||||
current_alignment += padding +
|
||||
eprosima::fastcdr::Cdr::alignment(current_alignment, padding);
|
||||
}
|
||||
}
|
||||
|
||||
switch (member->type_id_) {
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8:
|
||||
current_alignment += array_size * sizeof(int8_t);
|
||||
break;
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16:
|
||||
current_alignment += array_size * sizeof(uint16_t) +
|
||||
eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t));
|
||||
break;
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32:
|
||||
current_alignment += array_size * sizeof(uint32_t) +
|
||||
eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t));
|
||||
break;
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64:
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64:
|
||||
current_alignment += array_size * sizeof(uint64_t) +
|
||||
eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t));
|
||||
break;
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING:
|
||||
{
|
||||
for (size_t index = 0; index < array_size; ++index) {
|
||||
current_alignment += padding +
|
||||
eprosima::fastcdr::Cdr::alignment(current_alignment, padding) +
|
||||
member->string_upper_bound_ + 1;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE:
|
||||
{
|
||||
auto sub_members = static_cast<const MembersType *>(member->members_->data);
|
||||
for (size_t index = 0; index < array_size; ++index) {
|
||||
current_alignment += calculateMaxSerializedSize(sub_members, current_alignment);
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error("unknown type");
|
||||
}
|
||||
}
|
||||
|
||||
return current_alignment - initial_alignment;
|
||||
}
|
||||
|
||||
template<typename MembersType>
|
||||
bool TypeSupport<MembersType>::serializeROSmessage(
|
||||
const void * ros_message, eprosima::fastcdr::Cdr & ser,
|
||||
std::function<void(eprosima::fastcdr::Cdr&)> prefix)
|
||||
const void * ros_message, cycser & ser,
|
||||
std::function<void(cycser&)> prefix)
|
||||
{
|
||||
assert(ros_message);
|
||||
|
||||
// Serialize encapsulation
|
||||
ser.serialize_encapsulation();
|
||||
if (prefix) {
|
||||
prefix(ser);
|
||||
}
|
||||
|
|
@ -713,13 +636,11 @@ bool TypeSupport<MembersType>::serializeROSmessage(
|
|||
|
||||
template<typename MembersType>
|
||||
bool TypeSupport<MembersType>::deserializeROSmessage(
|
||||
eprosima::fastcdr::Cdr & deser, void * ros_message,
|
||||
std::function<void(eprosima::fastcdr::Cdr&)> prefix)
|
||||
cycdeser & deser, void * ros_message,
|
||||
std::function<void(cycdeser&)> prefix)
|
||||
{
|
||||
assert(ros_message);
|
||||
|
||||
// Deserialize encapsulation.
|
||||
deser.read_encapsulation();
|
||||
if (prefix) {
|
||||
prefix(deser);
|
||||
}
|
||||
|
|
@ -735,17 +656,6 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
|
|||
return true;
|
||||
}
|
||||
|
||||
#if 0
|
||||
template<typename MembersType>
|
||||
std::function<uint32_t()> TypeSupport<MembersType>::getSerializedSizeProvider(void * data)
|
||||
{
|
||||
assert(data);
|
||||
|
||||
auto ser = static_cast<eprosima::fastcdr::Cdr *>(data);
|
||||
return [ser]() -> uint32_t {return static_cast<uint32_t>(ser->getSerializedDataLength());};
|
||||
}
|
||||
#endif
|
||||
|
||||
} // namespace rmw_cyclonedds_cpp
|
||||
|
||||
#endif // RMW_CYCLONEDDS_CPP__TYPESUPPORT_IMPL_HPP_
|
||||
|
|
|
|||
254
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp
Normal file
254
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp
Normal file
|
|
@ -0,0 +1,254 @@
|
|||
// Copyright 2018 ADLINK Technology
|
||||
//
|
||||
// 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 SERDES_HPP
|
||||
#define SERDES_HPP
|
||||
|
||||
#include <array>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <type_traits>
|
||||
|
||||
#include "ddsc/dds.h"
|
||||
|
||||
extern "C" {
|
||||
struct serstate;
|
||||
struct serdata;
|
||||
struct sertopic;
|
||||
|
||||
struct serdata *ddsi_serdata_ref(struct serdata *serdata);
|
||||
void ddsi_serdata_unref(struct serdata *serdata);
|
||||
struct serstate *ddsi_serstate_new(const struct sertopic *topic);
|
||||
struct serdata *ddsi_serstate_fix(struct serstate *st);
|
||||
void ddsi_serstate_release(struct serstate *st);
|
||||
void *ddsi_serstate_append(struct serstate *st, size_t n);
|
||||
void *ddsi_serstate_append_align(struct serstate *st, size_t a);
|
||||
void *ddsi_serstate_append_aligned(struct serstate *st, size_t n, size_t a);
|
||||
void ddsi_serdata_getblob(void **raw, size_t *sz, struct serdata *serdata);
|
||||
}
|
||||
|
||||
template<class T>
|
||||
struct simpletype : std::integral_constant< bool, std::is_arithmetic<T>::value && !std::is_same<bool, T>::value > {};
|
||||
|
||||
template<class T>
|
||||
struct complextype : std::integral_constant< bool, !(std::is_arithmetic<T>::value && !std::is_same<bool, T>::value) > {};
|
||||
|
||||
template<unsigned N> struct rank : rank<N-1> {};
|
||||
template<> struct rank<0> {};
|
||||
|
||||
class cycser {
|
||||
public:
|
||||
cycser(struct sertopic *topic);
|
||||
cycser() = delete;
|
||||
~cycser();
|
||||
cycser& fix();
|
||||
cycser& ref();
|
||||
void unref();
|
||||
struct serdata *data();
|
||||
|
||||
inline cycser& operator<<(bool x) { serialize(x); return *this; }
|
||||
inline cycser& operator<<(char 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 char *x) { serialize(x); return *this; }
|
||||
inline cycser& operator<<(const std::string& 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 SIMPLE(T) inline void serialize(T x) { \
|
||||
T *p = (T *)ddsi_serstate_append_aligned(st, sizeof(T), sizeof(T)); \
|
||||
*p = x; \
|
||||
}
|
||||
SIMPLE(char);
|
||||
SIMPLE(unsigned char);
|
||||
SIMPLE(int16_t);
|
||||
SIMPLE(uint16_t);
|
||||
SIMPLE(int32_t);
|
||||
SIMPLE(uint32_t);
|
||||
SIMPLE(int64_t);
|
||||
SIMPLE(uint64_t);
|
||||
SIMPLE(float);
|
||||
SIMPLE(double);
|
||||
#undef SIMPLE
|
||||
|
||||
inline void serialize(bool x) {
|
||||
serialize(static_cast<unsigned char>(x));
|
||||
}
|
||||
inline void serialize(const char *x)
|
||||
{
|
||||
size_t sz = strlen(x) + 1;
|
||||
serialize(static_cast<uint32_t>(sz));
|
||||
char *p = (char *)ddsi_serstate_append(st, sz);
|
||||
memcpy(p, x, sz);
|
||||
}
|
||||
inline void serialize(const std::string& x)
|
||||
{
|
||||
size_t sz = x.size();
|
||||
serialize(static_cast<uint32_t>(sz+1));
|
||||
char *p = (char *)ddsi_serstate_append(st, sz+1);
|
||||
memcpy(p, x.data(), sz);
|
||||
p[sz] = 0;
|
||||
}
|
||||
|
||||
#define SIMPLEA(T) inline void serializeA(const T *x, size_t cnt) { \
|
||||
void *p = (void *)ddsi_serstate_append_aligned(st, (cnt) * sizeof(T), sizeof(T)); \
|
||||
memcpy(p, (void *)x, (cnt) * sizeof(T)); \
|
||||
}
|
||||
SIMPLEA(char);
|
||||
SIMPLEA(unsigned char);
|
||||
SIMPLEA(int16_t);
|
||||
SIMPLEA(uint16_t);
|
||||
SIMPLEA(int32_t);
|
||||
SIMPLEA(uint32_t);
|
||||
SIMPLEA(int64_t);
|
||||
SIMPLEA(uint64_t);
|
||||
SIMPLEA(float);
|
||||
SIMPLEA(double);
|
||||
#undef SIMPLEA
|
||||
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()));
|
||||
if (x.size() > 0) 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));
|
||||
if (cnt > 0) serializeA(x, cnt);
|
||||
}
|
||||
|
||||
private:
|
||||
struct serstate *st;
|
||||
struct serdata *sd;
|
||||
};
|
||||
|
||||
class cycdeser {
|
||||
public:
|
||||
// FIXME: byteswapping
|
||||
cycdeser(const void *data, size_t size);
|
||||
cycdeser() = delete;
|
||||
~cycdeser();
|
||||
|
||||
inline cycdeser& operator>>(bool& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(char& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(uint8_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(int16_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(uint16_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(int32_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(uint32_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(int64_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(uint64_t& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(float& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(double& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(char *& x) { deserialize(x); return *this; }
|
||||
inline cycdeser& operator>>(std::string& x) { deserialize(x); return *this; }
|
||||
template<class T> inline cycdeser& operator>>(std::vector<T>& x) { deserialize(x); return *this; }
|
||||
template<class T, size_t S> inline cycdeser& operator>>(std::array<T, S>& x) { deserialize(x); return *this; }
|
||||
|
||||
#define SIMPLE(T) inline void deserialize(T& x) { \
|
||||
align(sizeof(x)); \
|
||||
x = *reinterpret_cast<const T *>(data); \
|
||||
data += sizeof(x); \
|
||||
}
|
||||
SIMPLE(char);
|
||||
SIMPLE(unsigned char);
|
||||
SIMPLE(int16_t);
|
||||
SIMPLE(uint16_t);
|
||||
SIMPLE(int32_t);
|
||||
SIMPLE(uint32_t);
|
||||
SIMPLE(int64_t);
|
||||
SIMPLE(uint64_t);
|
||||
SIMPLE(float);
|
||||
SIMPLE(double);
|
||||
#undef SIMPLE
|
||||
|
||||
inline void deserialize(bool& x) {
|
||||
unsigned char z; deserialize(z); x = (z != 0);
|
||||
}
|
||||
inline uint32_t deserialize32() {
|
||||
uint32_t sz; deserialize(sz); return sz;
|
||||
}
|
||||
inline void deserialize(char *& x) {
|
||||
const uint32_t sz = deserialize32(); x = (char *)malloc(sz); memcpy(x, data, sz); data += sz;
|
||||
}
|
||||
inline void deserialize(std::string& x) {
|
||||
const uint32_t sz = deserialize32(); x = std::string(data, sz-1); data += sz;
|
||||
}
|
||||
|
||||
#define SIMPLEA(T) inline void deserializeA(T *x, size_t cnt) { \
|
||||
align(sizeof(T)); \
|
||||
memcpy((void *)x, (void *)data, (cnt) * sizeof(T)); \
|
||||
data += (cnt) * sizeof(T); \
|
||||
}
|
||||
SIMPLEA(char);
|
||||
SIMPLEA(unsigned char);
|
||||
SIMPLEA(int16_t);
|
||||
SIMPLEA(uint16_t);
|
||||
SIMPLEA(int32_t);
|
||||
SIMPLEA(uint32_t);
|
||||
SIMPLEA(int64_t);
|
||||
SIMPLEA(uint64_t);
|
||||
SIMPLEA(float);
|
||||
SIMPLEA(double);
|
||||
#undef SIMPLEA
|
||||
template<class T> inline void deserializeA(T *x, size_t cnt) {
|
||||
for (size_t i = 0; i < cnt; i++) deserialize(x[i]);
|
||||
}
|
||||
|
||||
template<class T> inline void deserialize(std::vector<T>& x) {
|
||||
const uint32_t sz = deserialize32();
|
||||
x.resize(sz);
|
||||
deserializeA(x.data(), sz);
|
||||
}
|
||||
inline void deserialize(std::vector<bool>& x) {
|
||||
const uint32_t sz = deserialize32();
|
||||
x.resize(sz);
|
||||
for (auto&& i : x) i = (data[i] != 0);
|
||||
data += sz;
|
||||
}
|
||||
template<class T, size_t S> inline void deserialize(std::array<T, S>& x) {
|
||||
deserializeA(x.data(), x.size());
|
||||
}
|
||||
|
||||
private:
|
||||
inline void align(size_t a)
|
||||
{
|
||||
const uintptr_t x = reinterpret_cast<uintptr_t>(data);
|
||||
if ((x % a) != 0) {
|
||||
const uintptr_t pad = a - (x % a);
|
||||
data = reinterpret_cast<const char *>(x + pad);
|
||||
}
|
||||
}
|
||||
|
||||
const char *data;
|
||||
size_t pos;
|
||||
size_t lim; // ignored for now ... better provide correct input
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -12,7 +12,6 @@
|
|||
|
||||
<buildtool_export_depend>ament_cmake</buildtool_export_depend>
|
||||
|
||||
<build_depend>fastcdr</build_depend>
|
||||
<build_depend>cyclonedds</build_depend>
|
||||
<build_depend>cyclonedds_cmake_module</build_depend>
|
||||
<build_depend>rcutils</build_depend>
|
||||
|
|
@ -21,7 +20,6 @@
|
|||
<build_depend>rosidl_typesupport_introspection_c</build_depend>
|
||||
<build_depend>rosidl_typesupport_introspection_cpp</build_depend>
|
||||
|
||||
<build_export_depend>fastcdr</build_export_depend>
|
||||
<build_export_depend>cyclonedds</build_export_depend>
|
||||
<build_export_depend>cyclonedds_cmake_module</build_export_depend>
|
||||
<build_export_depend>rcutils</build_export_depend>
|
||||
|
|
|
|||
|
|
@ -12,8 +12,3 @@
|
|||
# See the License for the specific language governing permissions and
|
||||
# limitations under the License.
|
||||
|
||||
# copied from rmw_fastrtps_cpp/rmw_fastrtps_cpp-extras.cmake
|
||||
|
||||
find_package(fastcdr REQUIRED CONFIG)
|
||||
|
||||
list(APPEND rmw_fastrtps_cpp_LIBRARIES fastcdr)
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@
|
|||
|
||||
- type names (make a copy of the generic type descriptor, modify the name and pass that)
|
||||
|
||||
- should serialize straight into serdata_t, instead of into a FastBuffer that then gets copied
|
||||
- need to handle endianness differences in deserialization
|
||||
|
||||
- topic creation: shouldn't leak topics
|
||||
|
||||
|
|
@ -72,18 +72,14 @@
|
|||
#include "rmw/impl/cpp/macros.hpp"
|
||||
|
||||
#include "namespace_prefix.hpp"
|
||||
#include "fastcdr/FastBuffer.h"
|
||||
|
||||
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
|
||||
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
|
||||
|
||||
#include "ddsc/dds.h"
|
||||
extern "C" {
|
||||
extern void ddsi_serdata_getblob (void **raw, size_t *sz, struct serdata *serdata);
|
||||
extern void ddsi_serdata_unref (struct serdata *serdata);
|
||||
}
|
||||
|
||||
#include "rmw_cyclonedds_topic.h"
|
||||
#include "rmw_cyclonedds_cpp/serdes.hpp"
|
||||
|
||||
#define RET_ERR_X(msg, code) do { RMW_SET_ERROR_MSG(msg); code; } while(0)
|
||||
#define RET_NULL_X(var, code) do { if (!var) RET_ERR_X(#var " is null", code); } while(0)
|
||||
|
|
@ -130,12 +126,11 @@ struct CddsNode {
|
|||
struct CddsPublisher {
|
||||
dds_entity_t pubh;
|
||||
dds_instance_handle_t pubiid;
|
||||
struct sertopic *sertopic;
|
||||
CddsTypeSupport ts;
|
||||
};
|
||||
|
||||
struct CddsSubscription {
|
||||
typedef rmw_subscription_t rmw_type;
|
||||
typedef rmw_subscriptions_t rmw_set_type;
|
||||
dds_entity_t subh;
|
||||
dds_entity_t rdcondh;
|
||||
CddsNode *node;
|
||||
|
|
@ -149,14 +144,10 @@ struct CddsCS {
|
|||
};
|
||||
|
||||
struct CddsClient {
|
||||
typedef rmw_client_t rmw_type;
|
||||
typedef rmw_clients_t rmw_set_type;
|
||||
CddsCS client;
|
||||
};
|
||||
|
||||
struct CddsService {
|
||||
typedef rmw_service_t rmw_type;
|
||||
typedef rmw_services_t rmw_set_type;
|
||||
CddsCS service;
|
||||
};
|
||||
|
||||
|
|
@ -199,8 +190,30 @@ using RequestTypeSupport_cpp = rmw_cyclonedds_cpp::RequestTypeSupport<rosidl_typ
|
|||
using ResponseTypeSupport_c = rmw_cyclonedds_cpp::ResponseTypeSupport<rosidl_typesupport_introspection_c__ServiceMembers, rosidl_typesupport_introspection_c__MessageMembers>;
|
||||
using ResponseTypeSupport_cpp = rmw_cyclonedds_cpp::ResponseTypeSupport<rosidl_typesupport_introspection_cpp::ServiceMembers, rosidl_typesupport_introspection_cpp::MessageMembers>;
|
||||
|
||||
extern "C" {
|
||||
struct dds_entity;
|
||||
struct dds_domain;
|
||||
int32_t dds_entity_lock(dds_entity_t hdl, dds_entity_kind_t kind, struct dds_entity **e);
|
||||
void dds_entity_unlock(struct dds_entity *e);
|
||||
struct sertopic *dds_topic_lookup (struct dds_domain *domain, const char *name);
|
||||
dds_domain *dds__entity_domain(dds_entity* e);
|
||||
void sertopic_free (struct sertopic * tp);
|
||||
}
|
||||
|
||||
static void clean_waitset_caches();
|
||||
|
||||
static struct sertopic *get_sertopic(dds_entity_t topic, const std::string& name)
|
||||
{
|
||||
struct dds_entity *x;
|
||||
struct sertopic *sertopic;
|
||||
if (dds_entity_lock(topic, DDS_KIND_TOPIC, &x) < 0) {
|
||||
abort();
|
||||
}
|
||||
sertopic = dds_topic_lookup(dds__entity_domain(x), name.c_str());
|
||||
dds_entity_unlock(x);
|
||||
return sertopic;
|
||||
}
|
||||
|
||||
static bool using_introspection_c_typesupport(const char *typesupport_identifier)
|
||||
{
|
||||
return typesupport_identifier == rosidl_typesupport_introspection_c__identifier;
|
||||
|
|
@ -264,7 +277,7 @@ template<typename ServiceType> const void *get_response_ptr(const void *untyped_
|
|||
return service_members->response_members_;
|
||||
}
|
||||
|
||||
static bool sermsg(const void *ros_message, eprosima::fastcdr::Cdr& ser, std::function<void(eprosima::fastcdr::Cdr&)> prefix, const CddsTypeSupport& ts)
|
||||
static bool sermsg(const void *ros_message, cycser& ser, std::function<void(cycser&)> prefix, const CddsTypeSupport& ts)
|
||||
{
|
||||
if (using_introspection_c_typesupport(ts.typesupport_identifier_)) {
|
||||
auto typed_typesupport = static_cast<MessageTypeSupport_c *>(ts.type_support_);
|
||||
|
|
@ -277,7 +290,7 @@ static bool sermsg(const void *ros_message, eprosima::fastcdr::Cdr& ser, std::fu
|
|||
return false;
|
||||
}
|
||||
|
||||
static bool desermsg(eprosima::fastcdr::Cdr& deser, void *ros_message, std::function<void(eprosima::fastcdr::Cdr&)> prefix, const CddsTypeSupport& ts)
|
||||
static bool desermsg(cycdeser& deser, void *ros_message, std::function<void(cycdeser&)> prefix, const CddsTypeSupport& ts)
|
||||
{
|
||||
if (using_introspection_c_typesupport(ts.typesupport_identifier_)) {
|
||||
auto typed_typesupport = static_cast<MessageTypeSupport_c *>(ts.type_support_);
|
||||
|
|
@ -415,13 +428,13 @@ extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition(const
|
|||
/////////// ///////////
|
||||
/////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
static rmw_ret_t rmw_write_ser(dds_entity_t pubh, eprosima::fastcdr::Cdr& ser)
|
||||
extern "C" {
|
||||
int dds_writecdr(dds_entity_t writer, struct serdata *serdata);
|
||||
}
|
||||
|
||||
static rmw_ret_t rmw_write_ser(dds_entity_t pubh, cycser& sd)
|
||||
{
|
||||
const size_t sz = ser.getSerializedDataLength();
|
||||
const void *raw = static_cast<void *>(ser.getBufferPointer());
|
||||
/* shifting by 4 bytes skips the CDR header -- it should be identical and the entire
|
||||
writecdr is a hack at the moment anyway */
|
||||
if (dds_writecdr(pubh, (char *)raw + 4, sz) >= 0) {
|
||||
if (dds_writecdr(pubh, sd.fix().ref().data()) >= 0) {
|
||||
return RMW_RET_OK;
|
||||
} else {
|
||||
/* FIXME: what is the expected behavior when it times out? */
|
||||
|
|
@ -437,10 +450,9 @@ extern "C" rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *r
|
|||
RET_NULL(ros_message);
|
||||
auto pub = static_cast<CddsPublisher *>(publisher->data);
|
||||
assert(pub);
|
||||
eprosima::fastcdr::FastBuffer buffer;
|
||||
eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
if (sermsg(ros_message, ser, nullptr, pub->ts)) {
|
||||
return rmw_write_ser(pub->pubh, ser);
|
||||
cycser sd(pub->sertopic);
|
||||
if (sermsg(ros_message, sd, nullptr, pub->ts)) {
|
||||
return rmw_write_ser(pub->pubh, sd);
|
||||
} else {
|
||||
RMW_SET_ERROR_MSG("cannot serialize data");
|
||||
return RMW_RET_ERROR;
|
||||
|
|
@ -540,11 +552,12 @@ static CddsPublisher *create_cdds_publisher(const rmw_node_t *node, const rosidl
|
|||
RMW_SET_ERROR_MSG("failed to create writer");
|
||||
goto fail_writer;
|
||||
}
|
||||
dds_qos_delete(qos);
|
||||
pub->sertopic = get_sertopic(topic, fqtopic_name);
|
||||
if (dds_get_instance_handle(pub->pubh, &pub->pubiid) < 0) {
|
||||
RMW_SET_ERROR_MSG("failed to get instance handle for writer");
|
||||
goto fail_instance_handle;
|
||||
}
|
||||
dds_qos_delete(qos);
|
||||
node_impl->own_writers.insert(pub->pubiid);
|
||||
/* FIXME: leak the topic for now */
|
||||
return pub;
|
||||
|
|
@ -753,8 +766,8 @@ static rmw_ret_t rmw_take_int(const rmw_subscription_t *subscription, void *ros_
|
|||
size_t sz;
|
||||
void *raw;
|
||||
ddsi_serdata_getblob(&raw, &sz, sd);
|
||||
eprosima::fastcdr::FastBuffer buffer(static_cast<char *>(raw), sz);
|
||||
eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
/* FIXME: endianness (i.e., the "+ 4") */
|
||||
cycdeser deser(static_cast<void *>(static_cast<char *>(raw) + 4), sz);
|
||||
desermsg(deser, ros_message, nullptr, sub->ts);
|
||||
ddsi_serdata_unref(sd);
|
||||
if (message_info) {
|
||||
|
|
@ -1026,10 +1039,10 @@ static rmw_ret_t rmw_take_response_request(CddsCS *cs, rmw_request_id_t *request
|
|||
size_t sz;
|
||||
void *raw;
|
||||
ddsi_serdata_getblob(&raw, &sz, sd);
|
||||
eprosima::fastcdr::FastBuffer buffer(static_cast<char *>(raw), sz);
|
||||
eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
/* FIXME: endianness (i.e., the "+ 4") */
|
||||
cycdeser deser(static_cast<void *>(static_cast<char *>(raw) + 4), sz);
|
||||
cdds_request_header_t header;
|
||||
desermsg(deser, ros_data, [&header](eprosima::fastcdr::Cdr& ser) { ser >> header.guid; ser >> header.seq; }, cs->sub->ts);
|
||||
desermsg(deser, ros_data, [&header](cycdeser& ser) { ser >> header.guid; ser >> header.seq; }, cs->sub->ts);
|
||||
ddsi_serdata_unref(sd);
|
||||
memset(request_header, 0, sizeof(*request_header));
|
||||
assert(sizeof(header.guid) < sizeof(request_header->writer_guid));
|
||||
|
|
@ -1063,10 +1076,9 @@ extern "C" rmw_ret_t rmw_take_request(const rmw_service_t *service, rmw_request_
|
|||
|
||||
static rmw_ret_t rmw_send_response_request(CddsCS *cs, cdds_request_header_t *header, const void *ros_data)
|
||||
{
|
||||
eprosima::fastcdr::FastBuffer buffer;
|
||||
eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
|
||||
if (sermsg(ros_data, ser, [&header](eprosima::fastcdr::Cdr& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) {
|
||||
return rmw_write_ser(cs->pub->pubh, ser);
|
||||
cycser sd(cs->pub->sertopic);
|
||||
if (sermsg(ros_data, sd, [&header](cycser& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) {
|
||||
return rmw_write_ser(cs->pub->pubh, sd);
|
||||
} else {
|
||||
RMW_SET_ERROR_MSG("cannot serialize data");
|
||||
return RMW_RET_ERROR;
|
||||
|
|
@ -1162,6 +1174,7 @@ static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_se
|
|||
RMW_SET_ERROR_MSG("failed to create writer");
|
||||
goto fail_writer;
|
||||
}
|
||||
pub->sertopic = get_sertopic(pubtopic, pubtopic_name);
|
||||
sub->node = node_impl;
|
||||
if ((sub->subh = dds_create_reader(gcdds.ppant, subtopic, qos, nullptr)) < 0) {
|
||||
RMW_SET_ERROR_MSG("failed to create reader");
|
||||
|
|
|
|||
53
rmw_cyclonedds_cpp/src/serdes.cpp
Normal file
53
rmw_cyclonedds_cpp/src/serdes.cpp
Normal file
|
|
@ -0,0 +1,53 @@
|
|||
#include "rmw_cyclonedds_cpp/serdes.hpp"
|
||||
|
||||
cycser::cycser(struct sertopic *topic)
|
||||
{
|
||||
st = ddsi_serstate_new(topic);
|
||||
sd = nullptr;
|
||||
}
|
||||
|
||||
cycser::~cycser()
|
||||
{
|
||||
if (sd == nullptr) {
|
||||
ddsi_serstate_release(st);
|
||||
} else {
|
||||
ddsi_serdata_unref(sd);
|
||||
}
|
||||
}
|
||||
|
||||
cycser& cycser::ref()
|
||||
{
|
||||
assert(sd != nullptr);
|
||||
ddsi_serdata_ref(sd);
|
||||
return *this;
|
||||
}
|
||||
|
||||
void cycser::unref()
|
||||
{
|
||||
assert(sd != nullptr);
|
||||
ddsi_serdata_unref(sd);
|
||||
}
|
||||
|
||||
cycser& cycser::fix()
|
||||
{
|
||||
assert(sd == nullptr);
|
||||
sd = ddsi_serstate_fix(st);
|
||||
return *this;
|
||||
}
|
||||
|
||||
struct serdata *cycser::data()
|
||||
{
|
||||
assert(sd != nullptr);
|
||||
return sd;
|
||||
}
|
||||
|
||||
cycdeser::cycdeser(const void *data_, size_t size_)
|
||||
{
|
||||
data = static_cast<const char *>(data_);
|
||||
lim = size_;
|
||||
pos = 0;
|
||||
}
|
||||
|
||||
cycdeser::~cycdeser()
|
||||
{
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue