diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt index f6c021b..4a9e215 100644 --- a/rmw_cyclonedds_cpp/CMakeLists.txt +++ b/rmw_cyclonedds_cpp/CMakeLists.txt @@ -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" diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport.hpp index b88c958..884ae6f 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport.hpp @@ -15,9 +15,6 @@ #ifndef RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_HPP_ #define RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_HPP_ -#include -#include - #include #include diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport_impl.hpp index a534c5e..5eaed34 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/MessageTypeSupport_impl.hpp @@ -15,9 +15,6 @@ #ifndef RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ #define RMW_CYCLONEDDS_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ -#include -#include - #include #include #include @@ -37,14 +34,6 @@ MessageTypeSupport::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(this->calculateMaxSerializedSize(members, 0)); - } else { - this->m_typeSize = 4; - } } } // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport.hpp index 3fcd4a4..3916cf8 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport.hpp @@ -15,8 +15,6 @@ #ifndef RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_HPP_ #define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_HPP_ -#include -#include #include #include "TypeSupport.hpp" diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport_impl.hpp index 8703639..e3b341a 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/ServiceTypeSupport_impl.hpp @@ -15,8 +15,6 @@ #ifndef RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ #define RMW_CYCLONEDDS_CPP__SERVICETYPESUPPORT_IMPL_HPP_ -#include -#include #include #include @@ -41,14 +39,6 @@ RequestTypeSupport::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(this->calculateMaxSerializedSize(this->members_, 0)); - } else { - this->m_typeSize = 4; - } } template @@ -61,14 +51,6 @@ ResponseTypeSupport::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(this->calculateMaxSerializedSize(this->members_, 0)); - } else { - this->m_typeSize = 4; - } } } // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp index 7fe587c..d5dd4d8 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp @@ -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 #include -#include -#include #include #include @@ -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 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 { @@ -75,7 +76,7 @@ struct StringHelper 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 return *(static_cast(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 class TypeSupport { public: - bool serializeROSmessage(const void * ros_message, eprosima::fastcdr::Cdr & ser, std::function prefix = nullptr); - - bool deserializeROSmessage(eprosima::fastcdr::FastBuffer * data, void * ros_message); - - bool deserializeROSmessage(eprosima::fastcdr::Cdr & deser, void * ros_message, std::function prefix = nullptr); + bool serializeROSmessage(const void * ros_message, cycser & ser, std::function prefix = nullptr); + bool deserializeROSmessage(cycdeser & deser, void * ros_message, std::function 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 diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp index f3b1bd2..9636449 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -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 -#include #include #include #include @@ -33,6 +32,8 @@ #include "rosidl_generator_c/primitives_array_functions.h" +#include "serdes.hpp" + namespace rmw_cyclonedds_cpp { @@ -109,7 +110,6 @@ template TypeSupport::TypeSupport() { name = ""; - m_typeSize = 0; } template @@ -207,12 +207,12 @@ template 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(field); } else if (member->array_size_ && !member->is_upper_bound_) { - ser.serializeArray(static_cast(field), member->array_size_); + ser.serializeA(static_cast(field), member->array_size_); } else { std::vector & data = *reinterpret_cast *>(field); ser << data; @@ -224,15 +224,15 @@ template 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(field); } else if (member->array_size_ && !member->is_upper_bound_) { - ser.serializeArray(static_cast(field), member->array_size_); + ser.serializeA(static_cast(field), member->array_size_); } else { auto & data = *reinterpret_cast::type *>(field); - ser.serializeSequence(reinterpret_cast(data.data), data.size); + ser.serializeS(reinterpret_cast(data.data), data.size); } } @@ -241,7 +241,7 @@ inline void serialize_field( const rosidl_typesupport_introspection_c__MessageMember * member, void * field, - eprosima::fastcdr::Cdr & ser) + cycser & ser) { using CStringHelper = StringHelper; if (!member->is_array_) { @@ -310,7 +310,7 @@ size_t get_array_size_and_assign_field( template bool TypeSupport::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 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(field); } else if (member->array_size_ && !member->is_upper_bound_) { - deser.deserializeArray(static_cast(field), member->array_size_); + deser.deserializeA(static_cast(field), member->array_size_); } else { auto & vector = *reinterpret_cast *>(field); if (call_new) { @@ -425,20 +425,20 @@ template 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(field); } else if (member->array_size_ && !member->is_upper_bound_) { - deser.deserializeArray(static_cast(field), member->array_size_); + deser.deserializeA(static_cast(field), member->array_size_); } else { auto & data = *reinterpret_cast::type *>(field); int32_t dsize = 0; deser >> dsize; GenericCArray::init(&data, dsize); - deser.deserializeArray(reinterpret_cast(data.data), dsize); + deser.deserializeA(reinterpret_cast(data.data), dsize); } } @@ -446,7 +446,7 @@ template<> inline void deserialize_field( 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( 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 bool TypeSupport::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::deserializeROSmessage( return true; } -template -size_t TypeSupport::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(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 bool TypeSupport::serializeROSmessage( - const void * ros_message, eprosima::fastcdr::Cdr & ser, - std::function prefix) + const void * ros_message, cycser & ser, + std::function prefix) { assert(ros_message); - // Serialize encapsulation - ser.serialize_encapsulation(); if (prefix) { prefix(ser); } @@ -713,13 +636,11 @@ bool TypeSupport::serializeROSmessage( template bool TypeSupport::deserializeROSmessage( - eprosima::fastcdr::Cdr & deser, void * ros_message, - std::function prefix) + cycdeser & deser, void * ros_message, + std::function prefix) { assert(ros_message); - // Deserialize encapsulation. - deser.read_encapsulation(); if (prefix) { prefix(deser); } @@ -735,17 +656,6 @@ bool TypeSupport::deserializeROSmessage( return true; } -#if 0 -template -std::function TypeSupport::getSerializedSizeProvider(void * data) -{ - assert(data); - - auto ser = static_cast(data); - return [ser]() -> uint32_t {return static_cast(ser->getSerializedDataLength());}; -} -#endif - } // namespace rmw_cyclonedds_cpp #endif // RMW_CYCLONEDDS_CPP__TYPESUPPORT_IMPL_HPP_ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp new file mode 100644 index 0000000..efc2b64 --- /dev/null +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -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 +#include +#include +#include + +#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 +struct simpletype : std::integral_constant< bool, std::is_arithmetic::value && !std::is_same::value > {}; + +template +struct complextype : std::integral_constant< bool, !(std::is_arithmetic::value && !std::is_same::value) > {}; + +template struct rank : rank {}; +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 inline cycser& operator<<(const std::vector& x) { serialize(x); return *this; } + template inline cycser& operator<<(const std::array& 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(x)); + } + inline void serialize(const char *x) + { + size_t sz = strlen(x) + 1; + serialize(static_cast(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(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 inline void serializeA(const T *x, size_t cnt) { + for (size_t i = 0; i < cnt; i++) serialize(x[i]); + } + + template inline void serialize(const std::vector& x) + { + serialize(static_cast(x.size())); + if (x.size() > 0) serializeA(x.data(), x.size()); + } + inline void serialize(const std::vector& x) { + serialize(static_cast(x.size())); + for (auto&& i : x) serialize(i); + } + + template inline void serializeS(const T *x, size_t cnt) + { + serialize(static_cast(cnt)); + 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 inline cycdeser& operator>>(std::vector& x) { deserialize(x); return *this; } + template inline cycdeser& operator>>(std::array& x) { deserialize(x); return *this; } + +#define SIMPLE(T) inline void deserialize(T& x) { \ + align(sizeof(x)); \ + x = *reinterpret_cast(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 inline void deserializeA(T *x, size_t cnt) { + for (size_t i = 0; i < cnt; i++) deserialize(x[i]); + } + + template inline void deserialize(std::vector& x) { + const uint32_t sz = deserialize32(); + x.resize(sz); + deserializeA(x.data(), sz); + } + inline void deserialize(std::vector& x) { + const uint32_t sz = deserialize32(); + x.resize(sz); + for (auto&& i : x) i = (data[i] != 0); + data += sz; + } + template inline void deserialize(std::array& x) { + deserializeA(x.data(), x.size()); + } + +private: + inline void align(size_t a) + { + const uintptr_t x = reinterpret_cast(data); + if ((x % a) != 0) { + const uintptr_t pad = a - (x % a); + data = reinterpret_cast(x + pad); + } + } + + const char *data; + size_t pos; + size_t lim; // ignored for now ... better provide correct input +}; + +#endif diff --git a/rmw_cyclonedds_cpp/package.xml b/rmw_cyclonedds_cpp/package.xml index 0ff0987..aaa9b5d 100644 --- a/rmw_cyclonedds_cpp/package.xml +++ b/rmw_cyclonedds_cpp/package.xml @@ -12,7 +12,6 @@ ament_cmake - fastcdr cyclonedds cyclonedds_cmake_module rcutils @@ -21,7 +20,6 @@ rosidl_typesupport_introspection_c rosidl_typesupport_introspection_cpp - fastcdr cyclonedds cyclonedds_cmake_module rcutils diff --git a/rmw_cyclonedds_cpp/rmw_cyclonedds_cpp-extras.cmake b/rmw_cyclonedds_cpp/rmw_cyclonedds_cpp-extras.cmake index 23fdd72..eeda675 100644 --- a/rmw_cyclonedds_cpp/rmw_cyclonedds_cpp-extras.cmake +++ b/rmw_cyclonedds_cpp/rmw_cyclonedds_cpp-extras.cmake @@ -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) diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index 51f4db4..6dd8b7d 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -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; using ResponseTypeSupport_cpp = rmw_cyclonedds_cpp::ResponseTypeSupport; +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 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 prefix, const CddsTypeSupport& ts) +static bool sermsg(const void *ros_message, cycser& ser, std::function prefix, const CddsTypeSupport& ts) { if (using_introspection_c_typesupport(ts.typesupport_identifier_)) { auto typed_typesupport = static_cast(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 prefix, const CddsTypeSupport& ts) +static bool desermsg(cycdeser& deser, void *ros_message, std::function prefix, const CddsTypeSupport& ts) { if (using_introspection_c_typesupport(ts.typesupport_identifier_)) { auto typed_typesupport = static_cast(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(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? */ @@ -430,17 +443,16 @@ static rmw_ret_t rmw_write_ser(dds_entity_t pubh, eprosima::fastcdr::Cdr& ser) return RMW_RET_OK; } } - + extern "C" rmw_ret_t rmw_publish(const rmw_publisher_t *publisher, const void *ros_message) { RET_WRONG_IMPLID(publisher); RET_NULL(ros_message); auto pub = static_cast(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(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(static_cast(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(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(static_cast(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"); diff --git a/rmw_cyclonedds_cpp/src/serdes.cpp b/rmw_cyclonedds_cpp/src/serdes.cpp new file mode 100644 index 0000000..a44e346 --- /dev/null +++ b/rmw_cyclonedds_cpp/src/serdes.cpp @@ -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(data_); + lim = size_; + pos = 0; +} + +cycdeser::~cycdeser() +{ +}