replace FastCDR and serialise straight into a serdata to avoid an extra copy

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"

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 <fastcdr/FastBuffer.h>
-#include <fastcdr/Cdr.h>
-
 #include <cassert>
 #include <memory>
 

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

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 <fastcdr/FastBuffer.h>
-#include <fastcdr/Cdr.h>
 #include <cassert>
 
 #include "TypeSupport.hpp"

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

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 <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 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);
-  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);
 
 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(
+  bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message);
-    eprosima::fastcdr::Cdr & ser, const MembersType * members, const void * ros_message);
+  bool deserializeROSmessage(cycdeser & deser, const MembersType * members, void * ros_message, bool call_new);
-
-  bool deserializeROSmessage(
-    eprosima::fastcdr::Cdr & deser, const MembersType * members, void * ros_message,
-    bool call_new);
 };
 
 }  // namespace rmw_cyclonedds_cpp

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 <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,
+  const void * ros_message, cycser & ser,
-  std::function<void(eprosima::fastcdr::Cdr&)> prefix)
+  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,
+  cycdeser & deser, void * ros_message,
-  std::function<void(eprosima::fastcdr::Cdr&)> prefix)
+  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_

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

rmw_cyclonedds_cpp/package.xml

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

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)

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<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();
+    if (dds_writecdr(pubh, sd.fix().ref().data()) >= 0) {
-    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) {
         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;
+    cycser sd(pub->sertopic);
-    eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+    if (sermsg(ros_message, sd, nullptr, pub->ts)) {
-    if (sermsg(ros_message, ser, nullptr, pub->ts)) {
+        return rmw_write_ser(pub->pubh, sd);
-        return rmw_write_ser(pub->pubh, ser);
     } 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);
+            /* FIXME: endianness (i.e., the "+ 4") */
-            eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            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);
+            /* FIXME: endianness (i.e., the "+ 4") */
-            eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+            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;
+    cycser sd(cs->pub->sertopic);
-    eprosima::fastcdr::Cdr ser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR);
+    if (sermsg(ros_data, sd, [&header](cycser& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) {
-    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, sd);
-        return rmw_write_ser(cs->pub->pubh, ser);
     } 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");

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<const char *>(data_);
+    lim = size_;
+    pos = 0;
+}
+
+cycdeser::~cycdeser()
+{
+}