diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp index a70ec05..6c8079f 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp @@ -111,21 +111,27 @@ template class TypeSupport { public: - bool serializeROSmessage(const void * ros_message, cycser & ser, std::function prefix = nullptr); - bool deserializeROSmessage(cycdeser & deser, void * ros_message, std::function prefix = nullptr); - std::string getName (); + bool serializeROSmessage( + const void * ros_message, cycser & ser, + std::function prefix = nullptr); + bool deserializeROSmessage( + cycdeser & deser, void * ros_message, + std::function prefix = nullptr); + std::string getName(); protected: TypeSupport(); - void setName(const std::string& name); + void setName(const std::string & name); const MembersType * members_; std::string name; private: bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message); - bool deserializeROSmessage(cycdeser & deser, const MembersType * members, void * ros_message, bool call_new); + 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 513ffdb..b1bb25b 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -109,13 +109,13 @@ rosidl_generator_c__void__Sequence__fini(rosidl_generator_c__void__Sequence * se template TypeSupport::TypeSupport() { - name = ""; + name = ""; } template -void TypeSupport::setName(const std::string& name) +void TypeSupport::setName(const std::string & name) { - this->name = std::string(name); + this->name = std::string(name); } static inline void * @@ -502,7 +502,9 @@ inline void deserialize_field( deser >> cpp_string_vector; auto & string_array_field = *reinterpret_cast(field); - if (!rosidl_generator_c__String__Sequence__init(&string_array_field, cpp_string_vector.size())) { + if (!rosidl_generator_c__String__Sequence__init(&string_array_field, + cpp_string_vector.size())) + { throw std::runtime_error("unable to initialize rosidl_generator_c__String array"); } @@ -649,7 +651,7 @@ bool TypeSupport::deserializeROSmessage( template bool TypeSupport::serializeROSmessage( const void * ros_message, cycser & ser, - std::function prefix) + std::function prefix) { assert(ros_message); @@ -669,7 +671,7 @@ bool TypeSupport::serializeROSmessage( template bool TypeSupport::deserializeROSmessage( cycdeser & deser, void * ros_message, - std::function prefix) + std::function prefix) { assert(ros_message); @@ -689,7 +691,7 @@ bool TypeSupport::deserializeROSmessage( } template -std::string TypeSupport::getName () +std::string TypeSupport::getName() { return name; } diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp index 8223a5e..d1a6262 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp @@ -11,47 +11,65 @@ // 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 SERDATA_HPP -#define SERDATA_HPP +#ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_ +#define RMW_CYCLONEDDS_CPP__SERDATA_HPP_ + +#include +#include #include "dds/ddsi/ddsi_sertopic.h" #include "dds/ddsi/ddsi_serdata.h" -struct CddsTypeSupport { - void *type_support_; - const char *typesupport_identifier_; +struct CddsTypeSupport +{ + void * type_support_; + const char * typesupport_identifier_; }; -struct sertopic_rmw : ddsi_sertopic { - CddsTypeSupport type_support; - bool is_request_header; - std::string cpp_name; - std::string cpp_type_name; - std::string cpp_name_type_name; +struct sertopic_rmw : ddsi_sertopic +{ + CddsTypeSupport type_support; + bool is_request_header; + std::string cpp_name; + std::string cpp_type_name; + std::string cpp_name_type_name; }; -struct serdata_rmw : ddsi_serdata { - /* first two bytes of data is CDR encoding - second two bytes are encoding options */ - std::vector data; +struct serdata_rmw : ddsi_serdata +{ + /* first two bytes of data is CDR encoding + second two bytes are encoding options */ + std::vector data; }; -typedef struct cdds_request_header { - uint64_t guid; - int64_t seq; +typedef struct cdds_request_header +{ + uint64_t guid; + int64_t seq; } cdds_request_header_t; -typedef struct cdds_request_wrapper { - cdds_request_header_t header; - void *data; +typedef struct cdds_request_wrapper +{ + cdds_request_header_t header; + void * data; } cdds_request_wrapper_t; -void *create_message_type_support (const void *untyped_members, const char *typesupport_identifier); -void *create_request_type_support (const void *untyped_members, const char *typesupport_identifier); -void *create_response_type_support (const void *untyped_members, const char *typesupport_identifier); +void * create_message_type_support( + const void * untyped_members, + const char * typesupport_identifier); +void * create_request_type_support( + const void * untyped_members, + const char * typesupport_identifier); +void * create_response_type_support( + const void * untyped_members, + const char * typesupport_identifier); -struct sertopic_rmw *create_sertopic (const char *topicname, const char *type_support_identifier, void *type_support, bool is_request_header); +struct sertopic_rmw * create_sertopic( + const char * topicname, const char * type_support_identifier, + void * type_support, bool is_request_header); -struct ddsi_serdata *serdata_rmw_from_serialized_message (const struct ddsi_sertopic *topiccmn, const void *raw, size_t size); +struct ddsi_serdata * serdata_rmw_from_serialized_message( + const struct ddsi_sertopic * topiccmn, + const void * raw, size_t size); -#endif +#endif // RMW_CYCLONEDDS_CPP__SERDATA_HPP_ diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp index add0a39..4abab01 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -1,4 +1,4 @@ -// Copyright 2018 ADLINK Technology +// Copyright 2018 to 2019 ADLINK Technology // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,238 +12,263 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SERDES_HPP -#define SERDES_HPP +#ifndef RMW_CYCLONEDDS_CPP__SERDES_HPP_ +#define RMW_CYCLONEDDS_CPP__SERDES_HPP_ + +#include #include #include #include -#include #include -class cycser { +class cycser +{ public: - cycser (std::vector& dst_); - cycser () = delete; + explicit cycser(std::vector & dst_); + cycser() = delete; - inline cycser& operator<< (bool x) { serialize (x); return *this; } - inline cycser& operator<< (char x) { serialize (x); return *this; } - inline cycser& operator<< (int8_t x) { serialize (x); return *this; } - inline cycser& operator<< (uint8_t x) { serialize (x); return *this; } - inline cycser& operator<< (int16_t x) { serialize (x); return *this; } - inline cycser& operator<< (uint16_t x) { serialize (x); return *this; } - inline cycser& operator<< (int32_t x) { serialize (x); return *this; } - inline cycser& operator<< (uint32_t x) { serialize (x); return *this; } - inline cycser& operator<< (int64_t x) { serialize (x); return *this; } - inline cycser& operator<< (uint64_t x) { serialize (x); return *this; } - inline cycser& operator<< (float x) { serialize (x); return *this; } - inline cycser& operator<< (double x) { serialize (x); return *this; } - inline cycser& operator<< (const 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; } + inline cycser & operator<<(bool x) {serialize(x); return *this;} + inline cycser & operator<<(char x) {serialize(x); return *this;} + inline cycser & operator<<(int8_t x) {serialize(x); return *this;} + inline cycser & operator<<(uint8_t x) {serialize(x); return *this;} + inline cycser & operator<<(int16_t x) {serialize(x); return *this;} + inline cycser & operator<<(uint16_t x) {serialize(x); return *this;} + inline cycser & operator<<(int32_t x) {serialize(x); return *this;} + inline cycser & operator<<(uint32_t x) {serialize(x); return *this;} + inline cycser & operator<<(int64_t x) {serialize(x); return *this;} + inline cycser & operator<<(uint64_t x) {serialize(x); return *this;} + inline cycser & operator<<(float x) {serialize(x); return *this;} + inline cycser & operator<<(double x) {serialize(x); return *this;} + inline cycser & operator<<(const 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) { \ - if ((off % sizeof (T)) != 0) { \ - off += sizeof (T) - (off % sizeof (T)); \ - } \ - resize (off + sizeof (T)); \ - *(T *) (data () + off) = x; \ - off += sizeof (T); \ - } - SIMPLE (char); - SIMPLE (int8_t); - SIMPLE (uint8_t); - SIMPLE (int16_t); - SIMPLE (uint16_t); - SIMPLE (int32_t); - SIMPLE (uint32_t); - SIMPLE (int64_t); - SIMPLE (uint64_t); - SIMPLE (float); - SIMPLE (double); +#define SIMPLE(T) inline void serialize(T x) { \ + if ((off % sizeof(T)) != 0) { \ + off += sizeof(T) - (off % sizeof(T)); \ + } \ + resize(off + sizeof(T)); \ + *(reinterpret_cast(data() + off)) = x; \ + off += sizeof(T); \ +} + SIMPLE(char); + SIMPLE(int8_t); + SIMPLE(uint8_t); + 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)); - resize (off + sz); - memcpy (data () + off, x, sz); - off += sz; - } - inline void serialize (const std::string& x) - { - size_t sz = x.size () + 1; - serialize (static_cast (sz)); - resize (off + sz); - memcpy (data () + off, x.c_str (), sz); - off += sz; - } + 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)); + resize(off + sz); + memcpy(data() + off, x, sz); + off += sz; + } + inline void serialize(const std::string & x) + { + size_t sz = x.size() + 1; + serialize(static_cast(sz)); + resize(off + sz); + memcpy(data() + off, x.c_str(), sz); + off += sz; + } -#define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \ - if (cnt > 0) { \ - if ((off % sizeof (T)) != 0) { \ - off += sizeof (T) - (off % sizeof (T)); \ - } \ - resize (off + cnt * sizeof (T)); \ - memcpy (data () + off, (void *) x, cnt * sizeof (T)); \ - off += cnt * sizeof (T); \ - } \ - } - SIMPLEA (char); - SIMPLEA (int8_t); - SIMPLEA (uint8_t); - SIMPLEA (int16_t); - SIMPLEA (uint16_t); - SIMPLEA (int32_t); - SIMPLEA (uint32_t); - SIMPLEA (int64_t); - SIMPLEA (uint64_t); - SIMPLEA (float); - SIMPLEA (double); +#define SIMPLEA(T) inline void serializeA(const T * x, size_t cnt) { \ + if (cnt > 0) { \ + if ((off % sizeof(T)) != 0) { \ + off += sizeof(T) - (off % sizeof(T)); \ + } \ + resize(off + cnt * sizeof(T)); \ + memcpy(data() + off, reinterpret_cast(x), cnt * sizeof(T)); \ + off += cnt * sizeof(T); \ + } \ +} + SIMPLEA(char); + SIMPLEA(int8_t); + SIMPLEA(uint8_t); + 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 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 ())); - 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 serialize(const std::vector & x) + { + serialize(static_cast(x.size())); + 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)); - serializeA (x, cnt); - } + template + inline void serializeS(const T * x, size_t cnt) + { + serialize(static_cast(cnt)); + serializeA(x, cnt); + } private: - inline void resize (size_t n) { dst.resize (n + 4); } - inline unsigned char *data () { return dst.data () + 4; } + inline void resize(size_t n) {dst.resize(n + 4);} + inline unsigned char * data() {return dst.data() + 4;} - std::vector& dst; - size_t off; + std::vector & dst; + size_t off; }; -class cycdeser { +class cycdeser +{ public: - // FIXME: byteswapping - cycdeser (const void *data, size_t size); - cycdeser () = delete; + // FIXME: byteswapping + cycdeser(const void * data, size_t size); + cycdeser() = delete; - inline cycdeser& operator>> (bool& x) { deserialize (x); return *this; } - inline cycdeser& operator>> (char& x) { deserialize (x); return *this; } - inline cycdeser& operator>> (int8_t& 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; } + inline cycdeser & operator>>(bool & x) {deserialize(x); return *this;} + inline cycdeser & operator>>(char & x) {deserialize(x); return *this;} + inline cycdeser & operator>>(int8_t & 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 + pos); \ - pos += sizeof (x); \ - } - SIMPLE (char); - SIMPLE (int8_t); - SIMPLE (uint8_t); - SIMPLE (int16_t); - SIMPLE (uint16_t); - SIMPLE (int32_t); - SIMPLE (uint32_t); - SIMPLE (int64_t); - SIMPLE (uint64_t); - SIMPLE (float); - SIMPLE (double); +#define SIMPLE(T) inline void deserialize(T & x) { \ + align(sizeof(x)); \ + x = *reinterpret_cast(data + pos); \ + pos += sizeof(x); \ +} + SIMPLE(char); + SIMPLE(int8_t); + SIMPLE(uint8_t); + 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 + pos, sz); - pos += sz; - } - inline void deserialize (std::string& x) { - const uint32_t sz = deserialize32 (); - x = std::string (data + pos, sz-1); - pos += sz; - } + 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 = reinterpret_cast(malloc(sz)); + memcpy(x, data + pos, sz); + pos += sz; + } + inline void deserialize(std::string & x) + { + const uint32_t sz = deserialize32(); + x = std::string(data + pos, sz - 1); + pos += sz; + } -#define SIMPLEA(T) inline void deserializeA (T *x, size_t cnt) { \ - if (cnt > 0) { \ - align (sizeof (T)); \ - memcpy ((void *) x, (void *) (data + pos), (cnt) * sizeof (T)); \ - pos += (cnt) * sizeof (T); \ - } \ - } - SIMPLEA (char); - SIMPLEA (int8_t); - SIMPLEA (uint8_t); - SIMPLEA (int16_t); - SIMPLEA (uint16_t); - SIMPLEA (int32_t); - SIMPLEA (uint32_t); - SIMPLEA (int64_t); - SIMPLEA (uint64_t); - SIMPLEA (float); - SIMPLEA (double); +#define SIMPLEA(T) inline void deserializeA(T * x, size_t cnt) { \ + if (cnt > 0) { \ + align(sizeof(T)); \ + memcpy(reinterpret_cast(x), reinterpret_cast(data + pos), \ + (cnt) * sizeof(T)); \ + pos += (cnt) * sizeof(T); \ + } \ +} + SIMPLEA(char); + SIMPLEA(int8_t); + SIMPLEA(uint8_t); + 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 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 (size_t i = 0; i < sz; i++) { - x[i] = ((data + pos)[i] != 0); - } - pos += sz; - } - template inline void deserialize (std::array& x) { - deserializeA (x.data (), x.size ()); + 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 (size_t i = 0; i < sz; i++) { + x[i] = ((data + pos)[i] != 0); } + pos += sz; + } + template + inline void deserialize(std::array & x) + { + deserializeA(x.data(), x.size()); + } private: - inline void align (size_t a) - { - if ((pos % a) != 0) { - pos += a - (pos % a); - } + inline void align(size_t a) + { + if ((pos % a) != 0) { + pos += a - (pos % a); } + } - const char *data; - size_t pos; - size_t lim; // ignored for now ... better provide correct input + const char * data; + size_t pos; + size_t lim; // ignored for now ... better provide correct input }; -#endif +#endif // RMW_CYCLONEDDS_CPP__SERDES_HPP_ diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp index c859d3b..de78e86 100644 --- a/rmw_cyclonedds_cpp/src/rmw_node.cpp +++ b/rmw_cyclonedds_cpp/src/rmw_node.cpp @@ -19,6 +19,9 @@ #include #include #include +#include +#include +#include #include #include "rcutils/logging_macros.h" @@ -47,297 +50,314 @@ #include "rmw_cyclonedds_cpp/serdes.hpp" #include "rmw_cyclonedds_cpp/serdata.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) -#define RET_ALLOC_X(var, code) do { if (!var) RET_ERR_X ("failed to allocate " #var, code); } while (0) -#define RET_WRONG_IMPLID_X(var, code) do { \ - RET_NULL_X (var, code); \ - if ((var)->implementation_identifier != eclipse_cyclonedds_identifier) { \ - RET_ERR_X (#var " not from this implementation", code); \ - } \ - } while (0) -#define RET_NULL_OR_EMPTYSTR_X(var, code) do { \ - if (!var || strlen (var) == 0) { \ - RET_ERR_X (#var " is null or empty string", code); \ - } \ - } while (0) -#define RET_ERR(msg) RET_ERR_X (msg, return RMW_RET_ERROR) -#define RET_NULL(var) RET_NULL_X (var, return RMW_RET_ERROR) -#define RET_ALLOC(var) RET_ALLOC_X (var, return RMW_RET_ERROR) -#define RET_WRONG_IMPLID(var) RET_WRONG_IMPLID_X (var, return RMW_RET_ERROR) -#define RET_NULL_OR_EMPTYSTR(var) RET_NULL_OR_EMPTYSTR_X (var, return RMW_RET_ERROR) +#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) +#define RET_ALLOC_X(var, code) do {if (!var) RET_ERR_X ("failed to allocate " #var, code); \ +} while (0) +#define RET_WRONG_IMPLID_X(var, code) do { \ + RET_NULL_X(var, code); \ + if ((var)->implementation_identifier != eclipse_cyclonedds_identifier) { \ + RET_ERR_X(#var " not from this implementation", code); \ + } \ +} while (0) +#define RET_NULL_OR_EMPTYSTR_X(var, code) do { \ + if (!var || strlen(var) == 0) { \ + RET_ERR_X(#var " is null or empty string", code); \ + } \ +} while (0) +#define RET_ERR(msg) RET_ERR_X(msg, return RMW_RET_ERROR) +#define RET_NULL(var) RET_NULL_X(var, return RMW_RET_ERROR) +#define RET_ALLOC(var) RET_ALLOC_X(var, return RMW_RET_ERROR) +#define RET_WRONG_IMPLID(var) RET_WRONG_IMPLID_X(var, return RMW_RET_ERROR) +#define RET_NULL_OR_EMPTYSTR(var) RET_NULL_OR_EMPTYSTR_X(var, return RMW_RET_ERROR) -const char *const eclipse_cyclonedds_identifier = "rmw_cyclonedds_cpp"; +const char * const eclipse_cyclonedds_identifier = "rmw_cyclonedds_cpp"; const char * const eclipse_cyclonedds_serialization_format = "cdr"; /* instance handles are unsigned 64-bit integers carefully constructed to be as close to uniformly distributed as possible for no other reason than making them near-perfect hash keys, hence we can improve over the default hash function */ -struct dds_instance_handle_hash { +struct dds_instance_handle_hash +{ public: - std::size_t operator() (dds_instance_handle_t const& x) const noexcept { - return static_cast (x); - } + std::size_t operator()(dds_instance_handle_t const & x) const noexcept + { + return static_cast(x); + } }; -bool operator< (dds_builtintopic_guid_t const& a, dds_builtintopic_guid_t const& b) { - return (memcmp (&a, &b, sizeof (dds_builtintopic_guid_t)) < 0); +bool operator<(dds_builtintopic_guid_t const & a, dds_builtintopic_guid_t const & b) +{ + return memcmp(&a, &b, sizeof(dds_builtintopic_guid_t)) < 0; } static const dds_entity_t builtin_topics[] = { - DDS_BUILTIN_TOPIC_DCPSPARTICIPANT, - DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, - DDS_BUILTIN_TOPIC_DCPSPUBLICATION + DDS_BUILTIN_TOPIC_DCPSPARTICIPANT, + DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, + DDS_BUILTIN_TOPIC_DCPSPUBLICATION }; -struct CddsNode { - dds_entity_t pp; - dds_entity_t pub; - dds_entity_t sub; - rmw_guard_condition_t *graph_guard_condition; +struct CddsNode +{ + dds_entity_t pp; + dds_entity_t pub; + dds_entity_t sub; + rmw_guard_condition_t * graph_guard_condition; }; -struct CddsPublisher { - dds_entity_t pubh; - dds_instance_handle_t pubiid; - struct ddsi_sertopic *sertopic; +struct CddsPublisher +{ + dds_entity_t pubh; + dds_instance_handle_t pubiid; + struct ddsi_sertopic * sertopic; }; -struct CddsSubscription { - dds_entity_t subh; - dds_entity_t rdcondh; - struct ddsi_sertopic *sertopic; +struct CddsSubscription +{ + dds_entity_t subh; + dds_entity_t rdcondh; + struct ddsi_sertopic * sertopic; }; -struct CddsCS { - CddsPublisher *pub; - CddsSubscription *sub; +struct CddsCS +{ + CddsPublisher * pub; + CddsSubscription * sub; }; -struct CddsClient { - CddsCS client; +struct CddsClient +{ + CddsCS client; }; -struct CddsService { - CddsCS service; +struct CddsService +{ + CddsCS service; }; -struct CddsGuardCondition { - dds_entity_t gcondh; +struct CddsGuardCondition +{ + dds_entity_t gcondh; }; -struct CddsWaitset { - dds_entity_t waitseth; +struct CddsWaitset +{ + dds_entity_t waitseth; - std::vector trigs; + std::vector trigs; - std::mutex lock; - bool inuse; - std::vector subs; - std::vector gcs; - std::vector cls; - std::vector srvs; + std::mutex lock; + bool inuse; + std::vector subs; + std::vector gcs; + std::vector cls; + std::vector srvs; }; -struct Cdds { - std::mutex lock; - uint32_t refcount; - dds_entity_t ppant; - dds_entity_t builtin_readers[sizeof (builtin_topics) / sizeof (builtin_topics[0])]; +struct Cdds +{ + std::mutex lock; + uint32_t refcount; + dds_entity_t ppant; + dds_entity_t builtin_readers[sizeof(builtin_topics) / sizeof(builtin_topics[0])]; - /* set of waitsets protected by lock, used to invalidate all waitsets caches when an entity is - deleted */ - std::unordered_set waitsets; + /* set of waitsets protected by lock, used to invalidate all waitsets caches when an entity is + deleted */ + std::unordered_set waitsets; - /* set of nodes is used to trigger graph guard conditions when something changes, but that - something also changes when creating the built-in readers when Cdds::lock is already held */ - std::mutex nodes_lock; - std::unordered_set nodes; + /* set of nodes is used to trigger graph guard conditions when something changes, but that + something also changes when creating the built-in readers when Cdds::lock is already held */ + std::mutex nodes_lock; + std::unordered_set nodes; - Cdds () : refcount (0), ppant (0) {} + Cdds() + : refcount(0), ppant(0) {} }; static Cdds gcdds; -static void clean_waitset_caches (); +static void clean_waitset_caches(); #ifndef WIN32 /* TODO(allenh1): check for Clang */ #pragma GCC visibility push (default) #endif -extern "C" const char *rmw_get_implementation_identifier () +extern "C" const char * rmw_get_implementation_identifier() { - return eclipse_cyclonedds_identifier; + return eclipse_cyclonedds_identifier; } -extern "C" const char *rmw_get_serialization_format() +extern "C" const char * rmw_get_serialization_format() { - return eclipse_cyclonedds_serialization_format; + return eclipse_cyclonedds_serialization_format; } -extern "C" rmw_ret_t rmw_set_log_severity (rmw_log_severity_t severity) +extern "C" rmw_ret_t rmw_set_log_severity(rmw_log_severity_t severity) { - static_cast(severity); - RMW_SET_ERROR_MSG ("unimplemented"); - return RMW_RET_ERROR; + static_cast(severity); + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_ERROR; } -extern "C" rmw_ret_t rmw_context_fini (rmw_context_t *context) +extern "C" rmw_ret_t rmw_context_fini(rmw_context_t * context) { - RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH ( - context, - context->implementation_identifier, - eclipse_cyclonedds_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - // context impl is explicitly supposed to be nullptr for now, see rmw_init's code - // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); - *context = rmw_get_zero_initialized_context (); - return RMW_RET_OK; + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + // context impl is explicitly supposed to be nullptr for now, see rmw_init's code + // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); + *context = rmw_get_zero_initialized_context(); + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_init_options_init (rmw_init_options_t *init_options, rcutils_allocator_t allocator) +extern "C" rmw_ret_t rmw_init_options_init( + rmw_init_options_t * init_options, + rcutils_allocator_t allocator) { - RMW_CHECK_ARGUMENT_FOR_NULL (init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR (&allocator, return RMW_RET_INVALID_ARGUMENT); - if (NULL != init_options->implementation_identifier) { - RMW_SET_ERROR_MSG ("expected zero-initialized init_options"); - return RMW_RET_INVALID_ARGUMENT; + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = eclipse_cyclonedds_identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + *dst = *src; + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&init_options->allocator, return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + *init_options = rmw_get_zero_initialized_init_options(); + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) +{ + static_cast(options); + static_cast(context); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + options, + options->implementation_identifier, + eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + context->instance_id = options->instance_id; + context->implementation_identifier = eclipse_cyclonedds_identifier; + context->impl = nullptr; + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_node_assert_liveliness(const rmw_node_t * node) +{ + RET_WRONG_IMPLID(node); + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_shutdown(rmw_context_t * context) +{ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eclipse_cyclonedds_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + // context impl is explicitly supposed to be nullptr for now, see rmw_init's code + // RCUTILS_CHECK_ARGUMENT_FOR_NULL (context->impl, RMW_RET_INVALID_ARGUMENT); + *context = rmw_get_zero_initialized_context(); + return RMW_RET_OK; +} + +static void ggcallback(dds_entity_t rd, void * varg) +{ + static_cast(varg); + void * msg = 0; + dds_sample_info_t info; + while (dds_take(rd, &msg, &info, 1, 1) > 0) { + dds_return_loan(rd, &msg, 1); + } + + { + std::lock_guard lock(gcdds.nodes_lock); + for (auto && node_impl : gcdds.nodes) { + if (rmw_trigger_guard_condition(node_impl->graph_guard_condition) != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "failed to trigger graph guard condition"); + } } - init_options->instance_id = 0; - init_options->implementation_identifier = eclipse_cyclonedds_identifier; - init_options->allocator = allocator; - init_options->impl = nullptr; - return RMW_RET_OK; + } } -extern "C" rmw_ret_t rmw_init_options_copy (const rmw_init_options_t *src, rmw_init_options_t *dst) +static dds_entity_t ref_ppant() { - RMW_CHECK_ARGUMENT_FOR_NULL (src, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL (dst, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH ( - src, - src->implementation_identifier, - eclipse_cyclonedds_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (NULL != dst->implementation_identifier) { - RMW_SET_ERROR_MSG ("expected zero-initialized dst"); - return RMW_RET_INVALID_ARGUMENT; - } - *dst = *src; - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_init_options_fini (rmw_init_options_t *init_options) -{ - RMW_CHECK_ARGUMENT_FOR_NULL (init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR (&init_options->allocator, return RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH ( - init_options, - init_options->implementation_identifier, - eclipse_cyclonedds_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - *init_options = rmw_get_zero_initialized_init_options (); - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_init (const rmw_init_options_t *options, rmw_context_t *context) -{ - static_cast(options); - static_cast(context); - RCUTILS_CHECK_ARGUMENT_FOR_NULL (options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH ( - options, - options->implementation_identifier, - eclipse_cyclonedds_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - context->instance_id = options->instance_id; - context->implementation_identifier = eclipse_cyclonedds_identifier; - context->impl = nullptr; - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_node_assert_liveliness (const rmw_node_t *node) -{ - RET_WRONG_IMPLID (node); - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_shutdown (rmw_context_t *context) -{ - RCUTILS_CHECK_ARGUMENT_FOR_NULL (context, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH ( - context, - context->implementation_identifier, - eclipse_cyclonedds_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - // context impl is explicitly supposed to be nullptr for now, see rmw_init's code - // RCUTILS_CHECK_ARGUMENT_FOR_NULL (context->impl, RMW_RET_INVALID_ARGUMENT); - *context = rmw_get_zero_initialized_context (); - return RMW_RET_OK; -} - -static void ggcallback (dds_entity_t rd, void *varg) -{ - static_cast(varg); - void *msg = 0; - dds_sample_info_t info; - while (dds_take (rd, &msg, &info, 1, 1) > 0) { - dds_return_loan (rd, &msg, 1); + std::lock_guard lock(gcdds.lock); + if (gcdds.refcount == 0) { + if ((gcdds.ppant = dds_create_participant(DDS_DOMAIN_DEFAULT, nullptr, nullptr)) < 0) { + RMW_SET_ERROR_MSG("failed to create participant"); + return gcdds.ppant; } - { - std::lock_guard lock (gcdds.nodes_lock); - for (auto&& node_impl : gcdds.nodes) { - if (rmw_trigger_guard_condition (node_impl->graph_guard_condition) != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to trigger graph guard condition"); - } + static_assert(sizeof(gcdds.builtin_readers) / sizeof(gcdds.builtin_readers[0]) == + sizeof(builtin_topics) / sizeof(builtin_topics[0]), + "mismatch between array of built-in topics and array of built-in readers"); + dds_listener_t * gglistener = dds_create_listener(nullptr); + dds_lset_data_available(gglistener, ggcallback); + for (size_t i = 0; i < sizeof(builtin_topics) / sizeof(builtin_topics[0]); i++) { + dds_entity_t rd = dds_create_reader(gcdds.ppant, builtin_topics[i], nullptr, gglistener); + if (rd < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "rmw_create_node: failed to create DDS built-in reader"); + while (i--) { + dds_delete(gcdds.builtin_readers[i]); } - } -} - -static dds_entity_t ref_ppant () -{ - std::lock_guard lock (gcdds.lock); - if (gcdds.refcount == 0) { - if ((gcdds.ppant = dds_create_participant (DDS_DOMAIN_DEFAULT, nullptr, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create participant"); - return gcdds.ppant; - } - - static_assert (sizeof (gcdds.builtin_readers) / sizeof (gcdds.builtin_readers[0]) == - sizeof (builtin_topics) / sizeof (builtin_topics[0]), - "mismatch between array of built-in topics and array of built-in readers"); - dds_listener_t *gglistener = dds_create_listener (nullptr); - dds_lset_data_available (gglistener, ggcallback); - for (size_t i = 0; i < sizeof (builtin_topics) / sizeof (builtin_topics[0]); i++) { - dds_entity_t rd = dds_create_reader (gcdds.ppant, builtin_topics[i], nullptr, gglistener); - if (rd < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "rmw_create_node: failed to create DDS built-in reader"); - while (i--) { - dds_delete (gcdds.builtin_readers[i]); - } - dds_delete (gcdds.ppant); - gcdds.ppant = 0; - return 0; - } - gcdds.builtin_readers[i] = rd; - } - dds_delete_listener (gglistener); - } - gcdds.refcount++; - return gcdds.ppant; -} - -static void unref_ppant () -{ - std::lock_guard lock (gcdds.lock); - if (--gcdds.refcount == 0) { - dds_delete (gcdds.ppant); + dds_delete(gcdds.ppant); gcdds.ppant = 0; + return 0; + } + gcdds.builtin_readers[i] = rd; } + dds_delete_listener(gglistener); + } + gcdds.refcount++; + return gcdds.ppant; +} + +static void unref_ppant() +{ + std::lock_guard lock(gcdds.lock); + if (--gcdds.refcount == 0) { + dds_delete(gcdds.ppant); + gcdds.ppant = 0; + } } ///////////////////////////////////////////////////////////////////////////////////////// @@ -346,133 +366,143 @@ static void unref_ppant () /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -static std::string get_node_user_data (const char *node_name, const char *node_namespace) +static std::string get_node_user_data(const char * node_name, const char * node_namespace) { - return (std::string ("name=") + std::string (node_name) + - std::string (";namespace=") + std::string (node_namespace) + - std::string (";")); + return std::string("name=") + std::string(node_name) + + std::string(";namespace=") + std::string(node_namespace) + + std::string(";"); } -extern "C" rmw_node_t *rmw_create_node (rmw_context_t *context, const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options) +extern "C" rmw_node_t * rmw_create_node( + rmw_context_t * context, const char * name, + const char * namespace_, size_t domain_id, + const rmw_node_security_options_t * security_options) { - static_cast(context); - RET_NULL_X (name, return nullptr); - RET_NULL_X (namespace_, return nullptr); - (void) domain_id; - (void) security_options; - rmw_ret_t ret; - int dummy_validation_result; - size_t dummy_invalid_index; - if ((ret = rmw_validate_node_name (name, &dummy_validation_result, &dummy_invalid_index)) != RMW_RET_OK) { - return nullptr; - } - dds_qos_t *qos = dds_create_qos (); - std::string user_data = get_node_user_data (name, namespace_); - dds_qset_userdata (qos, user_data.c_str (), user_data.size ()); - dds_entity_t pp = dds_create_participant (DDS_DOMAIN_DEFAULT, qos, nullptr); - dds_delete_qos (qos); - if (pp < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "rmw_create_node: failed to create DDS participant"); - return nullptr; - } - dds_entity_t pub, sub; - if ((pub = dds_create_publisher (pp, nullptr, nullptr)) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "rmw_create_node: failed to create DDS publisher"); - dds_delete (pp); - return nullptr; - } - if ((sub = dds_create_subscriber (pp, nullptr, nullptr)) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "rmw_create_node: failed to create DDS subscriber"); - dds_delete (pp); - return nullptr; - } - auto *node_impl = new CddsNode (); - rmw_node_t *node_handle = nullptr; - RET_ALLOC_X (node_impl, goto fail_node_impl); - rmw_guard_condition_t *graph_guard_condition; - if (!(graph_guard_condition = rmw_create_guard_condition (context))) { - goto fail_ggc; - } - node_impl->pp = pp; - node_impl->pub = pub; - node_impl->sub = sub; - node_impl->graph_guard_condition = graph_guard_condition; + static_cast(context); + RET_NULL_X(name, return nullptr); + RET_NULL_X(namespace_, return nullptr); + (void) domain_id; + (void) security_options; + rmw_ret_t ret; + int dummy_validation_result; + size_t dummy_invalid_index; + if ((ret = + rmw_validate_node_name(name, &dummy_validation_result, &dummy_invalid_index)) != RMW_RET_OK) + { + return nullptr; + } + dds_qos_t * qos = dds_create_qos(); + std::string user_data = get_node_user_data(name, namespace_); + dds_qset_userdata(qos, user_data.c_str(), user_data.size()); + dds_entity_t pp = dds_create_participant(DDS_DOMAIN_DEFAULT, qos, nullptr); + dds_delete_qos(qos); + if (pp < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "rmw_create_node: failed to create DDS participant"); + return nullptr; + } + dds_entity_t pub, sub; + if ((pub = dds_create_publisher(pp, nullptr, nullptr)) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "rmw_create_node: failed to create DDS publisher"); + dds_delete(pp); + return nullptr; + } + if ((sub = dds_create_subscriber(pp, nullptr, nullptr)) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "rmw_create_node: failed to create DDS subscriber"); + dds_delete(pp); + return nullptr; + } + auto * node_impl = new CddsNode(); + rmw_node_t * node_handle = nullptr; + RET_ALLOC_X(node_impl, goto fail_node_impl); + rmw_guard_condition_t * graph_guard_condition; + if (!(graph_guard_condition = rmw_create_guard_condition(context))) { + goto fail_ggc; + } + node_impl->pp = pp; + node_impl->pub = pub; + node_impl->sub = sub; + node_impl->graph_guard_condition = graph_guard_condition; - { - std::lock_guard lock (gcdds.nodes_lock); - gcdds.nodes.insert (node_impl); - } + { + std::lock_guard lock(gcdds.nodes_lock); + gcdds.nodes.insert(node_impl); + } - /* FIXME: should there be a (potentially spurious) trigger of the graph guard condition here? */ + /* FIXME: should there be a (potentially spurious) trigger of the graph guard condition here? */ - node_handle = rmw_node_allocate (); - RET_ALLOC_X (node_handle, goto fail_node_handle); - node_handle->implementation_identifier = eclipse_cyclonedds_identifier; - node_handle->data = node_impl; + node_handle = rmw_node_allocate(); + RET_ALLOC_X(node_handle, goto fail_node_handle); + node_handle->implementation_identifier = eclipse_cyclonedds_identifier; + node_handle->data = node_impl; - node_handle->name = static_cast (rmw_allocate (sizeof (char) * strlen (name) + 1)); - RET_ALLOC_X (node_handle->name, goto fail_node_handle_name); - memcpy (const_cast (node_handle->name), name, strlen (name) + 1); + node_handle->name = static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); + RET_ALLOC_X(node_handle->name, goto fail_node_handle_name); + memcpy(const_cast(node_handle->name), name, strlen(name) + 1); - node_handle->namespace_ = static_cast (rmw_allocate (sizeof (char) * strlen (namespace_) + 1)); - RET_ALLOC_X (node_handle->namespace_, goto fail_node_handle_namespace); - memcpy (const_cast (node_handle->namespace_), namespace_, strlen (namespace_) + 1); - return node_handle; + node_handle->namespace_ = + static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); + RET_ALLOC_X(node_handle->namespace_, goto fail_node_handle_namespace); + memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); + return node_handle; #if 0 - fail_add_node: - rmw_free (const_cast (node_handle->namespace_)); +fail_add_node: + rmw_free(const_cast(node_handle->namespace_)); #endif - fail_node_handle_namespace: - rmw_free (const_cast (node_handle->name)); - fail_node_handle_name: - rmw_node_free (node_handle); - fail_node_handle: - { - std::lock_guard lock (gcdds.nodes_lock); - gcdds.nodes.erase (node_impl); - } - if (RMW_RET_OK != rmw_destroy_guard_condition (graph_guard_condition)) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to destroy guard condition during error handling"); - } - fail_ggc: - delete node_impl; - fail_node_impl: - dds_delete (pp); - return nullptr; +fail_node_handle_namespace: + rmw_free(const_cast(node_handle->name)); +fail_node_handle_name: + rmw_node_free(node_handle); +fail_node_handle: + { + std::lock_guard lock(gcdds.nodes_lock); + gcdds.nodes.erase(node_impl); + } + if (RMW_RET_OK != rmw_destroy_guard_condition(graph_guard_condition)) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "failed to destroy guard condition during error handling"); + } +fail_ggc: + delete node_impl; +fail_node_impl: + dds_delete(pp); + return nullptr; } -extern "C" rmw_ret_t rmw_destroy_node (rmw_node_t *node) +extern "C" rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - rmw_ret_t result_ret = RMW_RET_OK; - RET_WRONG_IMPLID (node); - auto node_impl = static_cast (node->data); - RET_NULL (node_impl); - rmw_free (const_cast (node->name)); - rmw_free (const_cast (node->namespace_)); - rmw_node_free (node); - { - std::lock_guard lock (gcdds.nodes_lock); - gcdds.nodes.erase (node_impl); - } - if (RMW_RET_OK != rmw_destroy_guard_condition (node_impl->graph_guard_condition)) { - RMW_SET_ERROR_MSG ("failed to destroy graph guard condition"); - result_ret = RMW_RET_ERROR; - } - if (dds_delete (node_impl->pp) < 0) { - RMW_SET_ERROR_MSG ("failed to destroy DDS participant"); - result_ret = RMW_RET_ERROR; - } - delete node_impl; - return result_ret; + rmw_ret_t result_ret = RMW_RET_OK; + RET_WRONG_IMPLID(node); + auto node_impl = static_cast(node->data); + RET_NULL(node_impl); + rmw_free(const_cast(node->name)); + rmw_free(const_cast(node->namespace_)); + rmw_node_free(node); + { + std::lock_guard lock(gcdds.nodes_lock); + gcdds.nodes.erase(node_impl); + } + if (RMW_RET_OK != rmw_destroy_guard_condition(node_impl->graph_guard_condition)) { + RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); + result_ret = RMW_RET_ERROR; + } + if (dds_delete(node_impl->pp) < 0) { + RMW_SET_ERROR_MSG("failed to destroy DDS participant"); + result_ret = RMW_RET_ERROR; + } + delete node_impl; + return result_ret; } -extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition (const rmw_node_t *node) +extern "C" const rmw_guard_condition_t * rmw_node_get_graph_guard_condition(const rmw_node_t * node) { - RET_WRONG_IMPLID_X (node, return nullptr); - auto node_impl = static_cast (node->data); - RET_NULL_X (node_impl, return nullptr); - return node_impl->graph_guard_condition; + RET_WRONG_IMPLID_X(node, return nullptr); + auto node_impl = static_cast(node->data); + RET_NULL_X(node_impl, return nullptr); + return node_impl->graph_guard_condition; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -481,63 +511,93 @@ extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition (cons /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport; -using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport; +using MessageTypeSupport_c = + rmw_cyclonedds_cpp::MessageTypeSupport; +using MessageTypeSupport_cpp = + rmw_cyclonedds_cpp::MessageTypeSupport; -extern "C" rmw_ret_t rmw_get_serialized_message_size (const rosidl_message_type_support_t *type_support, const rosidl_message_bounds_t *message_bounds, size_t *size) +extern "C" rmw_ret_t rmw_get_serialized_message_size( + const rosidl_message_type_support_t * type_support, + const rosidl_message_bounds_t * message_bounds, size_t * size) { - static_cast(type_support); - static_cast(message_bounds); - static_cast(size); - RMW_SET_ERROR_MSG ("rmw_get_serialized_message_size: unimplemented"); - return RMW_RET_ERROR; + static_cast(type_support); + static_cast(message_bounds); + static_cast(size); + RMW_SET_ERROR_MSG("rmw_get_serialized_message_size: unimplemented"); + return RMW_RET_ERROR; } -extern "C" rmw_ret_t rmw_serialize (const void *ros_message, const rosidl_message_type_support_t *type_support, rmw_serialized_message_t *serialized_message) +extern "C" rmw_ret_t rmw_serialize( + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_serialized_message_t * serialized_message) { - std::vector data; - cycser sd (data); - rmw_ret_t ret; - const rosidl_message_type_support_t *ts; - if ((ts = get_message_typesupport_handle (type_support, rosidl_typesupport_introspection_c__identifier)) != nullptr) { - auto members = static_cast (ts->data); - MessageTypeSupport_c msgts (members); - msgts.serializeROSmessage (ros_message, sd, nullptr); - } else if ((ts = get_message_typesupport_handle (type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) { - auto members = static_cast (ts->data); - MessageTypeSupport_cpp msgts (members); - msgts.serializeROSmessage (ros_message, sd, nullptr); + std::vector data; + cycser sd(data); + rmw_ret_t ret; + const rosidl_message_type_support_t * ts; + if ((ts = + get_message_typesupport_handle(type_support, + rosidl_typesupport_introspection_c__identifier)) != nullptr) + { + auto members = + static_cast(ts->data); + MessageTypeSupport_c msgts(members); + msgts.serializeROSmessage(ros_message, sd, nullptr); + } else { + if ((ts = + get_message_typesupport_handle(type_support, + rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + { + auto members = + static_cast(ts->data); + MessageTypeSupport_cpp msgts(members); + msgts.serializeROSmessage(ros_message, sd, nullptr); } else { - RMW_SET_ERROR_MSG ("rmw_serialize: type support trouble"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("rmw_serialize: type support trouble"); + return RMW_RET_ERROR; } - /* FIXME: what about the header - should be included or not? */ - if ((ret = rmw_serialized_message_resize (serialized_message, data.size ())) != RMW_RET_OK) { - return ret; - } - memcpy (serialized_message->buffer, data.data (), data.size ()); - serialized_message->buffer_length = data.size (); - return RMW_RET_OK; + } + /* FIXME: what about the header - should be included or not? */ + if ((ret = rmw_serialized_message_resize(serialized_message, data.size())) != RMW_RET_OK) { + return ret; + } + memcpy(serialized_message->buffer, data.data(), data.size()); + serialized_message->buffer_length = data.size(); + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_deserialize (const rmw_serialized_message_t *serialized_message, const rosidl_message_type_support_t *type_support, void *ros_message) +extern "C" rmw_ret_t rmw_deserialize( + const rmw_serialized_message_t * serialized_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) { - cycdeser sd (serialized_message->buffer, serialized_message->buffer_length); - bool ok; - const rosidl_message_type_support_t *ts; - if ((ts = get_message_typesupport_handle (type_support, rosidl_typesupport_introspection_c__identifier)) != nullptr) { - auto members = static_cast (ts->data); - MessageTypeSupport_c msgts (members); - ok = msgts.deserializeROSmessage (sd, ros_message, nullptr); - } else if ((ts = get_message_typesupport_handle (type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) { - auto members = static_cast (ts->data); - MessageTypeSupport_cpp msgts (members); - ok = msgts.deserializeROSmessage (sd, ros_message, nullptr); + cycdeser sd(serialized_message->buffer, serialized_message->buffer_length); + bool ok; + const rosidl_message_type_support_t * ts; + if ((ts = + get_message_typesupport_handle(type_support, + rosidl_typesupport_introspection_c__identifier)) != nullptr) + { + auto members = + static_cast(ts->data); + MessageTypeSupport_c msgts(members); + ok = msgts.deserializeROSmessage(sd, ros_message, nullptr); + } else { + if ((ts = + get_message_typesupport_handle(type_support, + rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + { + auto members = + static_cast(ts->data); + MessageTypeSupport_cpp msgts(members); + ok = msgts.deserializeROSmessage(sd, ros_message, nullptr); } else { - RMW_SET_ERROR_MSG ("rmw_serialize: type support trouble"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("rmw_serialize: type support trouble"); + return RMW_RET_ERROR; } - return ok ? RMW_RET_OK : RMW_RET_ERROR; + } + return ok ? RMW_RET_OK : RMW_RET_ERROR; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -546,393 +606,431 @@ extern "C" rmw_ret_t rmw_deserialize (const rmw_serialized_message_t *serialized /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *ros_message, rmw_publisher_allocation_t *allocation) +extern "C" rmw_ret_t rmw_publish( + const rmw_publisher_t * publisher, const void * ros_message, + rmw_publisher_allocation_t * allocation) { - static_cast(allocation); // unused - RET_WRONG_IMPLID (publisher); - RET_NULL (ros_message); - auto pub = static_cast (publisher->data); - assert (pub); - if (dds_write (pub->pubh, ros_message) >= 0) { - return RMW_RET_OK; + static_cast(allocation); // unused + RET_WRONG_IMPLID(publisher); + RET_NULL(ros_message); + auto pub = static_cast(publisher->data); + assert(pub); + if (dds_write(pub->pubh, ros_message) >= 0) { + return RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("failed to publish data"); + return RMW_RET_ERROR; + } +} + +extern "C" rmw_ret_t rmw_publish_serialized_message( + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message, rmw_publisher_allocation_t * allocation) +{ + static_cast(allocation); // unused + RET_WRONG_IMPLID(publisher); + RET_NULL(serialized_message); + auto pub = static_cast(publisher->data); + struct ddsi_serdata * d = serdata_rmw_from_serialized_message(pub->sertopic, + serialized_message->buffer, + serialized_message->buffer_length); + const bool ok = (dds_writecdr(pub->pubh, d) >= 0); + return ok ? RMW_RET_OK : RMW_RET_ERROR; +} + +static const rosidl_message_type_support_t * get_typesupport( + const rosidl_message_type_support_t * type_supports) +{ + const rosidl_message_type_support_t * ts; + if ((ts = + get_message_typesupport_handle(type_supports, + rosidl_typesupport_introspection_c__identifier)) != nullptr) + { + return ts; + } else { + if ((ts = + get_message_typesupport_handle(type_supports, + rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + { + return ts; } else { - RMW_SET_ERROR_MSG ("failed to publish data"); - return RMW_RET_ERROR; + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; } + } } -extern "C" rmw_ret_t rmw_publish_serialized_message (const rmw_publisher_t *publisher, const rmw_serialized_message_t *serialized_message, rmw_publisher_allocation_t *allocation) +static std::string make_fqtopic( + const char * prefix, const char * topic_name, const char * suffix, + bool avoid_ros_namespace_conventions) { - static_cast(allocation); // unused - RET_WRONG_IMPLID (publisher); - RET_NULL (serialized_message); - auto pub = static_cast (publisher->data); - struct ddsi_serdata *d = serdata_rmw_from_serialized_message (pub->sertopic, serialized_message->buffer, serialized_message->buffer_length); - const bool ok = (dds_writecdr (pub->pubh, d) >= 0); - return ok ? RMW_RET_OK : RMW_RET_ERROR; + if (avoid_ros_namespace_conventions) { + return std::string(topic_name) + "__" + std::string(suffix); + } else { + return std::string(prefix) + std::string(topic_name) + std::string(suffix); + } } -static const rosidl_message_type_support_t *get_typesupport (const rosidl_message_type_support_t *type_supports) +static std::string make_fqtopic( + const char * prefix, const char * topic_name, const char * suffix, + const rmw_qos_profile_t * qos_policies) { - const rosidl_message_type_support_t *ts; - if ((ts = get_message_typesupport_handle (type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) { - return ts; - } else if ((ts = get_message_typesupport_handle (type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) { - return ts; - } else { - RMW_SET_ERROR_MSG ("type support not from this implementation"); + return make_fqtopic(prefix, topic_name, suffix, qos_policies->avoid_ros_namespace_conventions); +} + +static dds_qos_t * create_readwrite_qos( + const rmw_qos_profile_t * qos_policies, + bool ignore_local_publications) +{ + dds_qos_t * qos = dds_create_qos(); + switch (qos_policies->history) { + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_HISTORY_UNKNOWN: + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + if (qos_policies->depth > INT32_MAX) { + RMW_SET_ERROR_MSG("unsupported history depth"); + dds_delete_qos(qos); return nullptr; - } + } + dds_qset_history(qos, DDS_HISTORY_KEEP_LAST, static_cast(qos_policies->depth)); + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + dds_qset_history(qos, DDS_HISTORY_KEEP_ALL, DDS_LENGTH_UNLIMITED); + break; + } + switch (qos_policies->reliability) { + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_INFINITY); + break; + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + dds_qset_reliability(qos, DDS_RELIABILITY_BEST_EFFORT, 0); + break; + } + switch (qos_policies->durability) { + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + case RMW_QOS_POLICY_DURABILITY_UNKNOWN: + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + dds_qset_durability(qos, DDS_DURABILITY_VOLATILE); + break; + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + dds_qset_durability(qos, DDS_DURABILITY_TRANSIENT_LOCAL); + break; + } + /* deadline, lifespan, liveliness are not yet supported */ + if (ignore_local_publications) { + dds_qset_ignorelocal(qos, DDS_IGNORELOCAL_PARTICIPANT); + } + return qos; } -static std::string make_fqtopic (const char *prefix, const char *topic_name, const char *suffix, bool avoid_ros_namespace_conventions) +static bool get_readwrite_qos(dds_entity_t handle, rmw_qos_profile_t * qos_policies) { - if (avoid_ros_namespace_conventions) { - return std::string (topic_name) + "__" + std::string (suffix); + dds_qos_t * qos = dds_create_qos(); + if (dds_get_qos(handle, qos) < 0) { + RMW_SET_ERROR_MSG("get_readwrite_qos: invalid handle"); + goto error; + } + + { + dds_history_kind_t kind; + int32_t depth; + if (!dds_qget_history(qos, &kind, &depth)) { + RMW_SET_ERROR_MSG("get_readwrite_qos: history not set"); + goto error; + } + switch (kind) { + case DDS_HISTORY_KEEP_LAST: + qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_policies->depth = (uint32_t) depth; + break; + case DDS_HISTORY_KEEP_ALL: + qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + qos_policies->depth = (uint32_t) depth; + break; + } + } + + { + dds_reliability_kind_t kind; + dds_duration_t max_blocking_time; + if (!dds_qget_reliability(qos, &kind, &max_blocking_time)) { + RMW_SET_ERROR_MSG("get_readwrite_qos: history not set"); + goto error; + } + switch (kind) { + case DDS_RELIABILITY_BEST_EFFORT: + qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case DDS_RELIABILITY_RELIABLE: + qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + } + } + + { + dds_durability_kind_t kind; + if (!dds_qget_durability(qos, &kind)) { + RMW_SET_ERROR_MSG("get_readwrite_qos: durability not set"); + goto error; + } + switch (kind) { + case DDS_DURABILITY_VOLATILE: + qos_policies->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + case DDS_DURABILITY_TRANSIENT_LOCAL: + qos_policies->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case DDS_DURABILITY_TRANSIENT: + case DDS_DURABILITY_PERSISTENT: + qos_policies->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + } + + { + dds_duration_t deadline; + if (!dds_qget_deadline(qos, &deadline)) { + RMW_SET_ERROR_MSG("get_readwrite_qos: deadline not set"); + goto error; + } + if (deadline == DDS_INFINITY) { + qos_policies->deadline.sec = qos_policies->deadline.nsec = 0; } else { - return std::string (prefix) + std::string (topic_name) + std::string (suffix); + qos_policies->deadline.sec = (uint64_t) deadline / 1000000000; + qos_policies->deadline.nsec = (uint64_t) deadline % 1000000000; } + } + + { + dds_duration_t lifespan; + if (!dds_qget_lifespan(qos, &lifespan)) { + lifespan = DDS_INFINITY; + } + if (lifespan == DDS_INFINITY) { + qos_policies->lifespan.sec = qos_policies->lifespan.nsec = 0; + } else { + qos_policies->lifespan.sec = (uint64_t) lifespan / 1000000000; + qos_policies->lifespan.nsec = (uint64_t) lifespan % 1000000000; + } + } + + { + dds_liveliness_kind_t kind; + dds_duration_t lease_duration; + if (!dds_qget_liveliness(qos, &kind, &lease_duration)) { + RMW_SET_ERROR_MSG("get_readwrite_qos: liveliness not set"); + goto error; + } + switch (kind) { + case DDS_LIVELINESS_AUTOMATIC: + qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case DDS_LIVELINESS_MANUAL_BY_PARTICIPANT: + qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; + break; + case DDS_LIVELINESS_MANUAL_BY_TOPIC: + qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + } + if (lease_duration == DDS_INFINITY) { + qos_policies->liveliness_lease_duration.sec = qos_policies->liveliness_lease_duration.nsec = + 0; + } else { + qos_policies->liveliness_lease_duration.sec = (uint64_t) lease_duration / 1000000000; + qos_policies->liveliness_lease_duration.nsec = (uint64_t) lease_duration % 1000000000; + } + } + + dds_delete_qos(qos); + return true; +error: + dds_delete_qos(qos); + return false; } -static std::string make_fqtopic (const char *prefix, const char *topic_name, const char *suffix, const rmw_qos_profile_t *qos_policies) +static CddsPublisher * create_cdds_publisher( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies) { - return make_fqtopic (prefix, topic_name, suffix, qos_policies->avoid_ros_namespace_conventions); + RET_WRONG_IMPLID_X(node, return nullptr); + RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr); + RET_NULL_X(qos_policies, return nullptr); + auto node_impl = static_cast(node->data); + RET_NULL_X(node_impl, return nullptr); + const rosidl_message_type_support_t * type_support = get_typesupport(type_supports); + RET_NULL_X(type_support, return nullptr); + CddsPublisher * pub = new CddsPublisher(); + dds_entity_t topic; + dds_qos_t * qos; + + std::string fqtopic_name = make_fqtopic(ros_topic_prefix, topic_name, "", qos_policies); + + auto sertopic = create_sertopic( + fqtopic_name.c_str(), type_support->typesupport_identifier, + create_message_type_support(type_support->data, type_support->typesupport_identifier), false); + if ((topic = + dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) + { + RMW_SET_ERROR_MSG("failed to create topic"); + goto fail_topic; + } + if ((qos = create_readwrite_qos(qos_policies, false)) == nullptr) { + goto fail_qos; + } + if ((pub->pubh = dds_create_writer(node_impl->pub, topic, qos, nullptr)) < 0) { + RMW_SET_ERROR_MSG("failed to create writer"); + goto fail_writer; + } + 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; + } + /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been + created earlier, but the two are equivalent, so this'll do */ + pub->sertopic = sertopic; + dds_delete_qos(qos); + dds_delete(topic); + return pub; + +fail_instance_handle: + if (dds_delete(pub->pubh) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "failed to destroy writer during error handling"); + } +fail_writer: + dds_delete_qos(qos); +fail_qos: + dds_delete(topic); +fail_topic: + delete pub; + return nullptr; } -static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) -{ - dds_qos_t *qos = dds_create_qos (); - switch (qos_policies->history) { - case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: - case RMW_QOS_POLICY_HISTORY_UNKNOWN: - case RMW_QOS_POLICY_HISTORY_KEEP_LAST: - if (qos_policies->depth > INT32_MAX) { - RMW_SET_ERROR_MSG ("unsupported history depth"); - dds_delete_qos (qos); - return nullptr; - } - dds_qset_history (qos, DDS_HISTORY_KEEP_LAST, static_cast (qos_policies->depth)); - break; - case RMW_QOS_POLICY_HISTORY_KEEP_ALL: - dds_qset_history (qos, DDS_HISTORY_KEEP_ALL, DDS_LENGTH_UNLIMITED); - break; - } - switch (qos_policies->reliability) { - case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: - case RMW_QOS_POLICY_RELIABILITY_UNKNOWN: - case RMW_QOS_POLICY_RELIABILITY_RELIABLE: - dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_INFINITY); - break; - case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: - dds_qset_reliability (qos, DDS_RELIABILITY_BEST_EFFORT, 0); - break; - } - switch (qos_policies->durability) { - case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: - case RMW_QOS_POLICY_DURABILITY_UNKNOWN: - case RMW_QOS_POLICY_DURABILITY_VOLATILE: - dds_qset_durability (qos, DDS_DURABILITY_VOLATILE); - break; - case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: - dds_qset_durability (qos, DDS_DURABILITY_TRANSIENT_LOCAL); - break; - } - /* deadline, lifespan, liveliness are not yet supported */ - if (ignore_local_publications) { - dds_qset_ignorelocal (qos, DDS_IGNORELOCAL_PARTICIPANT); - } - return qos; -} - -static bool get_readwrite_qos (dds_entity_t handle, rmw_qos_profile_t *qos_policies) -{ - dds_qos_t *qos = dds_create_qos (); - if (dds_get_qos (handle, qos) < 0) { - RMW_SET_ERROR_MSG ("get_readwrite_qos: invalid handle"); - goto error; - } - - { - dds_history_kind_t kind; - int32_t depth; - if (!dds_qget_history (qos, &kind, &depth)) { - RMW_SET_ERROR_MSG ("get_readwrite_qos: history not set"); - goto error; - } - switch (kind) { - case DDS_HISTORY_KEEP_LAST: - qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; - qos_policies->depth = (uint32_t) depth; - break; - case DDS_HISTORY_KEEP_ALL: - qos_policies->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; - qos_policies->depth = (uint32_t) depth; - break; - } - } - - { - dds_reliability_kind_t kind; - dds_duration_t max_blocking_time; - if (!dds_qget_reliability (qos, &kind, &max_blocking_time)) { - RMW_SET_ERROR_MSG ("get_readwrite_qos: history not set"); - goto error; - } - switch (kind) { - case DDS_RELIABILITY_BEST_EFFORT: - qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; - break; - case DDS_RELIABILITY_RELIABLE: - qos_policies->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - break; - } - } - - { - dds_durability_kind_t kind; - if (!dds_qget_durability (qos, &kind)){ - RMW_SET_ERROR_MSG ("get_readwrite_qos: durability not set"); - goto error; - } - switch (kind) - { - case DDS_DURABILITY_VOLATILE: - qos_policies->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; - break; - case DDS_DURABILITY_TRANSIENT_LOCAL: - qos_policies->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - break; - case DDS_DURABILITY_TRANSIENT: - case DDS_DURABILITY_PERSISTENT: - qos_policies->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - break; - } - } - - { - dds_duration_t deadline; - if (!dds_qget_deadline (qos, &deadline)) { - RMW_SET_ERROR_MSG ("get_readwrite_qos: deadline not set"); - goto error; - } - if (deadline == DDS_INFINITY) - qos_policies->deadline.sec = qos_policies->deadline.nsec = 0; - else { - qos_policies->deadline.sec = (uint64_t) deadline / 1000000000; - qos_policies->deadline.nsec = (uint64_t) deadline % 1000000000; - } - } - - { - dds_duration_t lifespan; - if (!dds_qget_lifespan (qos, &lifespan)) { - lifespan = DDS_INFINITY; - } - if (lifespan == DDS_INFINITY) - qos_policies->lifespan.sec = qos_policies->lifespan.nsec = 0; - else { - qos_policies->lifespan.sec = (uint64_t) lifespan / 1000000000; - qos_policies->lifespan.nsec = (uint64_t) lifespan % 1000000000; - } - } - - { - dds_liveliness_kind_t kind; - dds_duration_t lease_duration; - if (!dds_qget_liveliness (qos, &kind, &lease_duration)) { - RMW_SET_ERROR_MSG ("get_readwrite_qos: liveliness not set"); - goto error; - } - switch (kind) { - case DDS_LIVELINESS_AUTOMATIC: - qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - break; - case DDS_LIVELINESS_MANUAL_BY_PARTICIPANT: - qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE; - break; - case DDS_LIVELINESS_MANUAL_BY_TOPIC: - qos_policies->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - break; - } - if (lease_duration == DDS_INFINITY) - qos_policies->liveliness_lease_duration.sec = qos_policies->liveliness_lease_duration.nsec = 0; - else { - qos_policies->liveliness_lease_duration.sec = (uint64_t) lease_duration / 1000000000; - qos_policies->liveliness_lease_duration.nsec = (uint64_t) lease_duration % 1000000000; - } - } - - dds_delete_qos (qos); - return true; - error: - dds_delete_qos (qos); - return false; -} - -static CddsPublisher *create_cdds_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) -{ - RET_WRONG_IMPLID_X (node, return nullptr); - RET_NULL_OR_EMPTYSTR_X (topic_name, return nullptr); - RET_NULL_X (qos_policies, return nullptr); - auto node_impl = static_cast (node->data); - RET_NULL_X (node_impl, return nullptr); - const rosidl_message_type_support_t *type_support = get_typesupport (type_supports); - RET_NULL_X (type_support, return nullptr); - CddsPublisher *pub = new CddsPublisher (); - dds_entity_t topic; - dds_qos_t *qos; - - std::string fqtopic_name = make_fqtopic (ros_topic_prefix, topic_name, "", qos_policies); - - auto sertopic = create_sertopic (fqtopic_name.c_str (), type_support->typesupport_identifier, create_message_type_support (type_support->data, type_support->typesupport_identifier), false); - if ((topic = dds_create_topic_arbitrary (node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create topic"); - goto fail_topic; - } - if ((qos = create_readwrite_qos (qos_policies, false)) == nullptr) { - goto fail_qos; - } - if ((pub->pubh = dds_create_writer (node_impl->pub, topic, qos, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create writer"); - goto fail_writer; - } - 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; - } - /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been - created earlier, but the two are equivalent, so this'll do */ - pub->sertopic = sertopic; - dds_delete_qos (qos); - dds_delete (topic); - return pub; - - fail_instance_handle: - if (dds_delete (pub->pubh) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to destroy writer during error handling"); - } - fail_writer: - dds_delete_qos (qos); - fail_qos: - dds_delete (topic); - fail_topic: - delete pub; - return nullptr; -} - -extern "C" rmw_ret_t rmw_init_publisher_allocation (const rosidl_message_type_support_t * type_support, const rosidl_message_bounds_t * message_bounds , rmw_publisher_allocation_t * allocation) +extern "C" rmw_ret_t rmw_init_publisher_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_message_bounds_t * message_bounds, rmw_publisher_allocation_t * allocation) { static_cast(type_support); static_cast(message_bounds); static_cast(allocation); - RMW_SET_ERROR_MSG ("rmw_init_publisher_allocation: unimplemented"); + RMW_SET_ERROR_MSG("rmw_init_publisher_allocation: unimplemented"); return RMW_RET_ERROR; } -extern "C" rmw_ret_t rmw_fini_publisher_allocation (rmw_publisher_allocation_t *allocation) +extern "C" rmw_ret_t rmw_fini_publisher_allocation(rmw_publisher_allocation_t * allocation) { static_cast(allocation); - RMW_SET_ERROR_MSG ("rmw_fini_publisher_allocation: unimplemented"); + RMW_SET_ERROR_MSG("rmw_fini_publisher_allocation: unimplemented"); return RMW_RET_ERROR; } -extern "C" rmw_publisher_t *rmw_create_publisher (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies) +extern "C" rmw_publisher_t * rmw_create_publisher( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, const char * topic_name, + const rmw_qos_profile_t * qos_policies) { - CddsPublisher *pub; - rmw_publisher_t *rmw_publisher; - if ((pub = create_cdds_publisher (node, type_supports, topic_name, qos_policies)) == nullptr) { - goto fail_common_init; - } - rmw_publisher = rmw_publisher_allocate (); - RET_ALLOC_X (rmw_publisher, goto fail_publisher); - rmw_publisher->implementation_identifier = eclipse_cyclonedds_identifier; - rmw_publisher->data = pub; - rmw_publisher->topic_name = reinterpret_cast (rmw_allocate (strlen (topic_name) + 1)); - RET_ALLOC_X (rmw_publisher->topic_name, goto fail_topic_name); - memcpy (const_cast (rmw_publisher->topic_name), topic_name, strlen (topic_name) + 1); - return rmw_publisher; - fail_topic_name: - rmw_publisher_free (rmw_publisher); - fail_publisher: - if (dds_delete (pub->pubh) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete writer during error handling"); + CddsPublisher * pub; + rmw_publisher_t * rmw_publisher; + if ((pub = create_cdds_publisher(node, type_supports, topic_name, qos_policies)) == nullptr) { + goto fail_common_init; + } + rmw_publisher = rmw_publisher_allocate(); + RET_ALLOC_X(rmw_publisher, goto fail_publisher); + rmw_publisher->implementation_identifier = eclipse_cyclonedds_identifier; + rmw_publisher->data = pub; + rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + RET_ALLOC_X(rmw_publisher->topic_name, goto fail_topic_name); + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + return rmw_publisher; +fail_topic_name: + rmw_publisher_free(rmw_publisher); +fail_publisher: + if (dds_delete(pub->pubh) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "failed to delete writer during error handling"); + } + delete pub; +fail_common_init: + return nullptr; +} + +extern "C" rmw_ret_t rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) +{ + RET_WRONG_IMPLID(publisher); + RET_NULL(gid); + auto pub = static_cast(publisher->data); + RET_NULL(pub); + gid->implementation_identifier = eclipse_cyclonedds_identifier; + memset(gid->data, 0, sizeof(gid->data)); + assert(sizeof(pub->pubiid) <= sizeof(gid->data)); + memcpy(gid->data, &pub->pubiid, sizeof(pub->pubiid)); + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_compare_gids_equal( + const rmw_gid_t * gid1, const rmw_gid_t * gid2, + bool * result) +{ + RET_WRONG_IMPLID(gid1); + RET_WRONG_IMPLID(gid2); + RET_NULL(result); + /* alignment is potentially lost because of the translation to an array of bytes, so use + memcmp instead of a simple integer comparison */ + *result = memcmp(gid1->data, gid2->data, sizeof(gid1->data)) == 0; + return RMW_RET_OK; +} + +extern "C" rmw_ret_t rmw_publisher_count_matched_subscriptions( + const rmw_publisher_t * publisher, + size_t * subscription_count) +{ + RET_WRONG_IMPLID(publisher); + auto pub = static_cast(publisher->data); + dds_publication_matched_status_t status; + if (dds_get_publication_matched_status(pub->pubh, &status) < 0) { + return RMW_RET_ERROR; + } else { + *subscription_count = status.current_count; + return RMW_RET_OK; + } +} + +rmw_ret_t rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) +{ + RET_WRONG_IMPLID(publisher); + return RMW_RET_OK; +} + +rmw_ret_t rmw_publisher_get_actual_qos(const rmw_publisher_t * publisher, rmw_qos_profile_t * qos) +{ + RET_NULL(qos); + RET_WRONG_IMPLID(publisher); + auto pub = static_cast(publisher->data); + if (get_readwrite_qos(pub->pubh, qos)) { + return RMW_RET_OK; + } else { + return RMW_RET_ERROR; + } +} + +extern "C" rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) +{ + RET_WRONG_IMPLID(node); + RET_WRONG_IMPLID(publisher); + auto pub = static_cast(publisher->data); + if (pub != nullptr) { + if (dds_delete(pub->pubh) < 0) { + RMW_SET_ERROR_MSG("failed to delete writer"); } + ddsi_sertopic_unref(pub->sertopic); delete pub; - fail_common_init: - return nullptr; -} - -extern "C" rmw_ret_t rmw_get_gid_for_publisher (const rmw_publisher_t *publisher, rmw_gid_t *gid) -{ - RET_WRONG_IMPLID (publisher); - RET_NULL (gid); - auto pub = static_cast (publisher->data); - RET_NULL (pub); - gid->implementation_identifier = eclipse_cyclonedds_identifier; - memset (gid->data, 0, sizeof (gid->data)); - assert (sizeof (pub->pubiid) <= sizeof (gid->data)); - memcpy (gid->data, &pub->pubiid, sizeof (pub->pubiid)); - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_compare_gids_equal (const rmw_gid_t *gid1, const rmw_gid_t *gid2, bool *result) -{ - RET_WRONG_IMPLID (gid1); - RET_WRONG_IMPLID (gid2); - RET_NULL (result); - /* alignment is potentially lost because of the translation to an array of bytes, so use - memcmp instead of a simple integer comparison */ - *result = memcmp (gid1->data, gid2->data, sizeof (gid1->data)) == 0; - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_publisher_count_matched_subscriptions (const rmw_publisher_t *publisher, size_t *subscription_count) -{ - RET_WRONG_IMPLID (publisher); - auto pub = static_cast (publisher->data); - dds_publication_matched_status_t status; - if (dds_get_publication_matched_status (pub->pubh, &status) < 0) { - return RMW_RET_ERROR; - } else { - *subscription_count = status.current_count; - return RMW_RET_OK; - } -} - -rmw_ret_t rmw_publisher_assert_liveliness (const rmw_publisher_t *publisher) -{ - RET_WRONG_IMPLID (publisher); - return RMW_RET_OK; -} - -rmw_ret_t rmw_publisher_get_actual_qos (const rmw_publisher_t *publisher, rmw_qos_profile_t *qos) -{ - RET_NULL (qos); - RET_WRONG_IMPLID (publisher); - auto pub = static_cast (publisher->data); - if (get_readwrite_qos (pub->pubh, qos)) { - return RMW_RET_OK; - } else { - return RMW_RET_ERROR; - } -} - -extern "C" rmw_ret_t rmw_destroy_publisher (rmw_node_t *node, rmw_publisher_t *publisher) -{ - RET_WRONG_IMPLID (node); - RET_WRONG_IMPLID (publisher); - auto pub = static_cast (publisher->data); - if (pub != nullptr) { - if (dds_delete (pub->pubh) < 0) { - RMW_SET_ERROR_MSG ("failed to delete writer"); - } - ddsi_sertopic_unref (pub->sertopic); - delete pub; - } - rmw_free (const_cast (publisher->topic_name)); - publisher->topic_name = nullptr; - rmw_publisher_free (publisher); - return RMW_RET_OK; + } + rmw_free(const_cast(publisher->topic_name)); + publisher->topic_name = nullptr; + rmw_publisher_free(publisher); + return RMW_RET_OK; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -941,239 +1039,280 @@ extern "C" rmw_ret_t rmw_destroy_publisher (rmw_node_t *node, rmw_publisher_t *p /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -static CddsSubscription *create_cdds_subscription (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) +static CddsSubscription * create_cdds_subscription( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, const char * topic_name, + const rmw_qos_profile_t * qos_policies, bool ignore_local_publications) { - RET_WRONG_IMPLID_X (node, return nullptr); - RET_NULL_OR_EMPTYSTR_X (topic_name, return nullptr); - RET_NULL_X (qos_policies, return nullptr); - auto node_impl = static_cast (node->data); - RET_NULL_X (node_impl, return nullptr); - const rosidl_message_type_support_t *type_support = get_typesupport (type_supports); - RET_NULL_X (type_support, return nullptr); - CddsSubscription *sub = new CddsSubscription (); - dds_entity_t topic; - dds_qos_t *qos; + RET_WRONG_IMPLID_X(node, return nullptr); + RET_NULL_OR_EMPTYSTR_X(topic_name, return nullptr); + RET_NULL_X(qos_policies, return nullptr); + auto node_impl = static_cast(node->data); + RET_NULL_X(node_impl, return nullptr); + const rosidl_message_type_support_t * type_support = get_typesupport(type_supports); + RET_NULL_X(type_support, return nullptr); + CddsSubscription * sub = new CddsSubscription(); + dds_entity_t topic; + dds_qos_t * qos; - std::string fqtopic_name = make_fqtopic (ros_topic_prefix, topic_name, "", qos_policies); + std::string fqtopic_name = make_fqtopic(ros_topic_prefix, topic_name, "", qos_policies); - auto sertopic = create_sertopic (fqtopic_name.c_str (), type_support->typesupport_identifier, create_message_type_support (type_support->data, type_support->typesupport_identifier), false); - if ((topic = dds_create_topic_arbitrary (node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create topic"); - goto fail_topic; - } - if ((qos = create_readwrite_qos (qos_policies, ignore_local_publications)) == nullptr) { - goto fail_qos; - } - if ((sub->subh = dds_create_reader (node_impl->sub, topic, qos, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create reader"); - goto fail_reader; - } - if ((sub->rdcondh = dds_create_readcondition (sub->subh, DDS_ANY_STATE)) < 0) { - RMW_SET_ERROR_MSG ("failed to create readcondition"); - goto fail_readcond; - } - /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been - created earlier, but the two are equivalent, so this'll do */ - sub->sertopic = sertopic; - dds_delete_qos (qos); - dds_delete (topic); - return sub; - fail_readcond: - if (dds_delete (sub->subh) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete reader during error handling"); - } - fail_reader: - dds_delete_qos (qos); - fail_qos: - dds_delete (topic); - fail_topic: - delete sub; - return nullptr; + auto sertopic = create_sertopic( + fqtopic_name.c_str(), type_support->typesupport_identifier, + create_message_type_support(type_support->data, type_support->typesupport_identifier), false); + if ((topic = + dds_create_topic_arbitrary(node_impl->pp, sertopic, nullptr, nullptr, nullptr)) < 0) + { + RMW_SET_ERROR_MSG("failed to create topic"); + goto fail_topic; + } + if ((qos = create_readwrite_qos(qos_policies, ignore_local_publications)) == nullptr) { + goto fail_qos; + } + if ((sub->subh = dds_create_reader(node_impl->sub, topic, qos, nullptr)) < 0) { + RMW_SET_ERROR_MSG("failed to create reader"); + goto fail_reader; + } + if ((sub->rdcondh = dds_create_readcondition(sub->subh, DDS_ANY_STATE)) < 0) { + RMW_SET_ERROR_MSG("failed to create readcondition"); + goto fail_readcond; + } + /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been + created earlier, but the two are equivalent, so this'll do */ + sub->sertopic = sertopic; + dds_delete_qos(qos); + dds_delete(topic); + return sub; +fail_readcond: + if (dds_delete(sub->subh) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "failed to delete reader during error handling"); + } +fail_reader: + dds_delete_qos(qos); +fail_qos: + dds_delete(topic); +fail_topic: + delete sub; + return nullptr; } -extern "C" rmw_ret_t rmw_init_subscription_allocation (const rosidl_message_type_support_t *type_support, const rosidl_message_bounds_t *message_bounds, rmw_subscription_allocation_t *allocation) +extern "C" rmw_ret_t rmw_init_subscription_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_message_bounds_t * message_bounds, rmw_subscription_allocation_t * allocation) { static_cast(type_support); static_cast(message_bounds); static_cast(allocation); - RMW_SET_ERROR_MSG ("rmw_init_subscription_allocation: unimplemented"); + RMW_SET_ERROR_MSG("rmw_init_subscription_allocation: unimplemented"); return RMW_RET_ERROR; } -extern "C" rmw_ret_t rmw_fini_subscription_allocation (rmw_subscription_allocation_t *allocation) +extern "C" rmw_ret_t rmw_fini_subscription_allocation(rmw_subscription_allocation_t * allocation) { - static_cast(allocation); - RMW_SET_ERROR_MSG ("rmw_fini_subscription_allocation: unimplemented"); + static_cast(allocation); + RMW_SET_ERROR_MSG("rmw_fini_subscription_allocation: unimplemented"); return RMW_RET_ERROR; } -extern "C" rmw_subscription_t *rmw_create_subscription (const rmw_node_t *node, const rosidl_message_type_support_t *type_supports, const char *topic_name, const rmw_qos_profile_t *qos_policies, bool ignore_local_publications) +extern "C" rmw_subscription_t * rmw_create_subscription( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, const char * topic_name, + const rmw_qos_profile_t * qos_policies, bool ignore_local_publications) { - CddsSubscription *sub; - rmw_subscription_t *rmw_subscription; - if ((sub = create_cdds_subscription (node, type_supports, topic_name, qos_policies, ignore_local_publications)) == nullptr) { - goto fail_common_init; + CddsSubscription * sub; + rmw_subscription_t * rmw_subscription; + if ((sub = + create_cdds_subscription(node, type_supports, topic_name, qos_policies, + ignore_local_publications)) == nullptr) + { + goto fail_common_init; + } + rmw_subscription = rmw_subscription_allocate(); + RET_ALLOC_X(rmw_subscription, goto fail_subscription); + rmw_subscription->implementation_identifier = eclipse_cyclonedds_identifier; + rmw_subscription->data = sub; + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + RET_ALLOC_X(rmw_subscription->topic_name, goto fail_topic_name); + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + return rmw_subscription; +fail_topic_name: + rmw_subscription_free(rmw_subscription); +fail_subscription: + if (dds_delete(sub->rdcondh) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", + "failed to delete readcondition during error handling"); + } + if (dds_delete(sub->subh) < 0) { + RCUTILS_LOG_ERROR_NAMED("rmw_cyclonedds_cpp", "failed to delete reader during error handling"); + } + delete sub; +fail_common_init: + return nullptr; +} + +extern "C" rmw_ret_t rmw_subscription_count_matched_publishers( + const rmw_subscription_t * subscription, size_t * publisher_count) +{ + RET_WRONG_IMPLID(subscription); + auto sub = static_cast(subscription->data); + dds_subscription_matched_status_t status; + if (dds_get_subscription_matched_status(sub->subh, &status) < 0) { + return RMW_RET_ERROR; + } else { + *publisher_count = status.current_count; + return RMW_RET_OK; + } +} + +extern "C" rmw_ret_t rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + RET_NULL(qos); + RET_WRONG_IMPLID(subscription); + auto sub = static_cast(subscription->data); + if (get_readwrite_qos(sub->subh, qos)) { + return RMW_RET_OK; + } else { + return RMW_RET_ERROR; + } +} + +extern "C" rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) +{ + RET_WRONG_IMPLID(node); + RET_WRONG_IMPLID(subscription); + auto sub = static_cast(subscription->data); + if (sub != nullptr) { + clean_waitset_caches(); + if (dds_delete(sub->rdcondh) < 0) { + RMW_SET_ERROR_MSG("failed to delete readcondition"); } - rmw_subscription = rmw_subscription_allocate (); - RET_ALLOC_X (rmw_subscription, goto fail_subscription); - rmw_subscription->implementation_identifier = eclipse_cyclonedds_identifier; - rmw_subscription->data = sub; - rmw_subscription->topic_name = reinterpret_cast (rmw_allocate (strlen (topic_name) + 1)); - RET_ALLOC_X (rmw_subscription->topic_name, goto fail_topic_name); - memcpy (const_cast (rmw_subscription->topic_name), topic_name, strlen (topic_name) + 1); - return rmw_subscription; - fail_topic_name: - rmw_subscription_free (rmw_subscription); - fail_subscription: - if (dds_delete (sub->rdcondh) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete readcondition during error handling"); - } - if (dds_delete (sub->subh) < 0) { - RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete reader during error handling"); + if (dds_delete(sub->subh) < 0) { + RMW_SET_ERROR_MSG("failed to delete reader"); } + ddsi_sertopic_unref(sub->sertopic); delete sub; - fail_common_init: - return nullptr; + } + rmw_free(const_cast(subscription->topic_name)); + subscription->topic_name = nullptr; + rmw_subscription_free(subscription); + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_subscription_count_matched_publishers (const rmw_subscription_t *subscription, size_t *publisher_count) +static rmw_ret_t rmw_take_int( + const rmw_subscription_t * subscription, void * ros_message, + bool * taken, rmw_message_info_t * message_info) { - RET_WRONG_IMPLID (subscription); - auto sub = static_cast (subscription->data); - dds_subscription_matched_status_t status; - if (dds_get_subscription_matched_status (sub->subh, &status) < 0) { + RET_NULL(taken); + RET_NULL(ros_message); + RET_WRONG_IMPLID(subscription); + CddsSubscription * sub = static_cast(subscription->data); + RET_NULL(sub); + dds_sample_info_t info; + while (dds_take(sub->subh, &ros_message, &info, 1, 1) == 1) { + if (info.valid_data) { + if (message_info) { + message_info->publisher_gid.implementation_identifier = eclipse_cyclonedds_identifier; + memset(message_info->publisher_gid.data, 0, sizeof(message_info->publisher_gid.data)); + assert(sizeof(info.publication_handle) <= sizeof(message_info->publisher_gid.data)); + memcpy(message_info->publisher_gid.data, &info.publication_handle, + sizeof(info.publication_handle)); + } + *taken = true; + return RMW_RET_OK; + } + } + *taken = false; + return RMW_RET_OK; +} + +static rmw_ret_t rmw_take_ser_int( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, bool * taken, + rmw_message_info_t * message_info) +{ + RET_NULL(taken); + RET_NULL(serialized_message); + RET_WRONG_IMPLID(subscription); + CddsSubscription * sub = static_cast(subscription->data); + RET_NULL(sub); + dds_sample_info_t info; + struct ddsi_serdata * dcmn; + while (dds_takecdr(sub->subh, &dcmn, 1, &info, DDS_ANY_STATE) == 1) { + if (info.valid_data) { + if (message_info) { + message_info->publisher_gid.implementation_identifier = eclipse_cyclonedds_identifier; + memset(message_info->publisher_gid.data, 0, sizeof(message_info->publisher_gid.data)); + assert(sizeof(info.publication_handle) <= sizeof(message_info->publisher_gid.data)); + memcpy(message_info->publisher_gid.data, &info.publication_handle, + sizeof(info.publication_handle)); + } + auto d = static_cast(dcmn); + /* FIXME: what about the header - should be included or not? */ + if (rmw_serialized_message_resize(serialized_message, d->data.size()) != RMW_RET_OK) { + ddsi_serdata_unref(dcmn); + *taken = false; return RMW_RET_ERROR; - } else { - *publisher_count = status.current_count; - return RMW_RET_OK; + } + memcpy(serialized_message->buffer, d->data.data(), d->data.size()); + serialized_message->buffer_length = d->data.size(); + ddsi_serdata_unref(dcmn); + *taken = true; + return RMW_RET_OK; } + ddsi_serdata_unref(dcmn); + } + *taken = false; + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_subscription_get_actual_qos(const rmw_subscription_t *subscription, rmw_qos_profile_t *qos) +extern "C" rmw_ret_t rmw_take( + const rmw_subscription_t * subscription, void * ros_message, + bool * taken, rmw_subscription_allocation_t * allocation) { - RET_NULL (qos); - RET_WRONG_IMPLID (subscription); - auto sub = static_cast (subscription->data); - if (get_readwrite_qos (sub->subh, qos)) { - return RMW_RET_OK; - } else { - return RMW_RET_ERROR; - } + static_cast(allocation); + return rmw_take_int(subscription, ros_message, taken, nullptr); } -extern "C" rmw_ret_t rmw_destroy_subscription (rmw_node_t *node, rmw_subscription_t *subscription) +extern "C" rmw_ret_t rmw_take_with_info( + const rmw_subscription_t * subscription, void * ros_message, + bool * taken, rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RET_WRONG_IMPLID (node); - RET_WRONG_IMPLID (subscription); - auto sub = static_cast (subscription->data); - if (sub != nullptr) { - clean_waitset_caches (); - if (dds_delete (sub->rdcondh) < 0) { - RMW_SET_ERROR_MSG ("failed to delete readcondition"); - } - if (dds_delete (sub->subh) < 0) { - RMW_SET_ERROR_MSG ("failed to delete reader"); - } - ddsi_sertopic_unref (sub->sertopic); - delete sub; - } - rmw_free (const_cast (subscription->topic_name)); - subscription->topic_name = nullptr; - rmw_subscription_free (subscription); - return RMW_RET_OK; + static_cast(allocation); + return rmw_take_int(subscription, ros_message, taken, message_info); } -static rmw_ret_t rmw_take_int (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info) +extern "C" rmw_ret_t rmw_take_serialized_message( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) { - RET_NULL (taken); - RET_NULL (ros_message); - RET_WRONG_IMPLID (subscription); - CddsSubscription *sub = static_cast (subscription->data); - RET_NULL (sub); - dds_sample_info_t info; - while (dds_take (sub->subh, &ros_message, &info, 1, 1) == 1) { - if (info.valid_data) { - if (message_info) { - message_info->publisher_gid.implementation_identifier = eclipse_cyclonedds_identifier; - memset (message_info->publisher_gid.data, 0, sizeof (message_info->publisher_gid.data)); - assert (sizeof (info.publication_handle) <= sizeof (message_info->publisher_gid.data)); - memcpy (message_info->publisher_gid.data, &info.publication_handle, sizeof (info.publication_handle)); - } - *taken = true; - return RMW_RET_OK; - } - } - *taken = false; - return RMW_RET_OK; + static_cast(allocation); + return rmw_take_ser_int(subscription, serialized_message, taken, nullptr); } -static rmw_ret_t rmw_take_ser_int (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_message_info_t *message_info) +extern "C" rmw_ret_t rmw_take_serialized_message_with_info( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, bool * taken, rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) { - RET_NULL (taken); - RET_NULL (serialized_message); - RET_WRONG_IMPLID (subscription); - CddsSubscription *sub = static_cast (subscription->data); - RET_NULL (sub); - dds_sample_info_t info; - struct ddsi_serdata *dcmn; - while (dds_takecdr (sub->subh, &dcmn, 1, &info, DDS_ANY_STATE) == 1) { - if (info.valid_data) { - if (message_info) { - message_info->publisher_gid.implementation_identifier = eclipse_cyclonedds_identifier; - memset (message_info->publisher_gid.data, 0, sizeof (message_info->publisher_gid.data)); - assert (sizeof (info.publication_handle) <= sizeof (message_info->publisher_gid.data)); - memcpy (message_info->publisher_gid.data, &info.publication_handle, sizeof (info.publication_handle)); - } - auto d = static_cast (dcmn); - /* FIXME: what about the header - should be included or not? */ - if (rmw_serialized_message_resize (serialized_message, d->data.size ()) != RMW_RET_OK) { - ddsi_serdata_unref (dcmn); - *taken = false; - return RMW_RET_ERROR; - } - memcpy (serialized_message->buffer, d->data.data (), d->data.size ()); - serialized_message->buffer_length = d->data.size (); - ddsi_serdata_unref (dcmn); - *taken = true; - return RMW_RET_OK; - } - ddsi_serdata_unref (dcmn); - } - *taken = false; - return RMW_RET_OK; + static_cast(allocation); + return rmw_take_ser_int(subscription, serialized_message, taken, message_info); } -extern "C" rmw_ret_t rmw_take (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_subscription_allocation_t *allocation) +extern "C" rmw_ret_t rmw_take_event( + const rmw_events_t * event_handle, void * event_info, + bool * taken) { - static_cast(allocation); - return rmw_take_int (subscription, ros_message, taken, nullptr); -} - -extern "C" rmw_ret_t rmw_take_with_info (const rmw_subscription_t *subscription, void *ros_message, bool *taken, rmw_message_info_t *message_info, rmw_subscription_allocation_t *allocation) -{ - static_cast(allocation); - return rmw_take_int (subscription, ros_message, taken, message_info); -} - -extern "C" rmw_ret_t rmw_take_serialized_message (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_subscription_allocation_t *allocation) -{ - static_cast(allocation); - return rmw_take_ser_int (subscription, serialized_message, taken, nullptr); -} - -extern "C" rmw_ret_t rmw_take_serialized_message_with_info (const rmw_subscription_t *subscription, rmw_serialized_message_t *serialized_message, bool *taken, rmw_message_info_t *message_info, rmw_subscription_allocation_t *allocation) -{ - static_cast(allocation); - return rmw_take_ser_int (subscription, serialized_message, taken, message_info); -} - -extern "C" rmw_ret_t rmw_take_event (const rmw_events_t *event_handle, void *event_info, bool *taken) -{ - static_cast(event_info); - static_cast(event_handle); - RET_NULL (taken); - *taken = false; - return RMW_RET_OK; + static_cast(event_info); + static_cast(event_handle); + RET_NULL(taken); + *taken = false; + return RMW_RET_OK; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -1182,226 +1321,248 @@ extern "C" rmw_ret_t rmw_take_event (const rmw_events_t *event_handle, void *eve /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -extern "C" rmw_guard_condition_t *rmw_create_guard_condition (rmw_context_t *context) +extern "C" rmw_guard_condition_t * rmw_create_guard_condition(rmw_context_t * context) { - static_cast(context); - rmw_guard_condition_t *guard_condition_handle; - auto *gcond_impl = new CddsGuardCondition (); - if (ref_ppant () < 0) { - goto fail_ppant; - } - if ((gcond_impl->gcondh = dds_create_guardcondition (gcdds.ppant)) < 0) { - RMW_SET_ERROR_MSG ("failed to create guardcondition"); - goto fail_guardcond; - } - guard_condition_handle = new rmw_guard_condition_t; - guard_condition_handle->implementation_identifier = eclipse_cyclonedds_identifier; - guard_condition_handle->data = gcond_impl; - return guard_condition_handle; + static_cast(context); + rmw_guard_condition_t * guard_condition_handle; + auto * gcond_impl = new CddsGuardCondition(); + if (ref_ppant() < 0) { + goto fail_ppant; + } + if ((gcond_impl->gcondh = dds_create_guardcondition(gcdds.ppant)) < 0) { + RMW_SET_ERROR_MSG("failed to create guardcondition"); + goto fail_guardcond; + } + guard_condition_handle = new rmw_guard_condition_t; + guard_condition_handle->implementation_identifier = eclipse_cyclonedds_identifier; + guard_condition_handle->data = gcond_impl; + return guard_condition_handle; - fail_guardcond: - unref_ppant (); - fail_ppant: - delete (gcond_impl); - return nullptr; +fail_guardcond: + unref_ppant(); +fail_ppant: + delete (gcond_impl); + return nullptr; } -extern "C" rmw_ret_t rmw_destroy_guard_condition (rmw_guard_condition_t *guard_condition_handle) +extern "C" rmw_ret_t rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition_handle) { - RET_NULL (guard_condition_handle); - auto *gcond_impl = static_cast (guard_condition_handle->data); - clean_waitset_caches (); - dds_delete (gcond_impl->gcondh); - unref_ppant (); - delete gcond_impl; - delete guard_condition_handle; - return RMW_RET_OK; + RET_NULL(guard_condition_handle); + auto * gcond_impl = static_cast(guard_condition_handle->data); + clean_waitset_caches(); + dds_delete(gcond_impl->gcondh); + unref_ppant(); + delete gcond_impl; + delete guard_condition_handle; + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_trigger_guard_condition (const rmw_guard_condition_t *guard_condition_handle) +extern "C" rmw_ret_t rmw_trigger_guard_condition( + const rmw_guard_condition_t * guard_condition_handle) { - RET_WRONG_IMPLID (guard_condition_handle); - auto *gcond_impl = static_cast (guard_condition_handle->data); - dds_set_guardcondition (gcond_impl->gcondh, true); - return RMW_RET_OK; + RET_WRONG_IMPLID(guard_condition_handle); + auto * gcond_impl = static_cast(guard_condition_handle->data); + dds_set_guardcondition(gcond_impl->gcondh, true); + return RMW_RET_OK; } -extern "C" rmw_wait_set_t *rmw_create_wait_set (rmw_context_t *context, size_t max_conditions) +extern "C" rmw_wait_set_t * rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) { - static_cast(context); - (void) max_conditions; - rmw_wait_set_t *wait_set = rmw_wait_set_allocate (); - CddsWaitset *ws = nullptr; - RET_ALLOC_X (wait_set, goto fail_alloc_wait_set); - wait_set->implementation_identifier = eclipse_cyclonedds_identifier; - wait_set->data = rmw_allocate (sizeof (CddsWaitset)); - RET_ALLOC_X (wait_set->data, goto fail_alloc_wait_set_data); - // This should default-construct the fields of CddsWaitset - ws = static_cast (wait_set->data); - RMW_TRY_PLACEMENT_NEW (ws, ws, goto fail_placement_new, CddsWaitset, ); - if (!ws) { - RMW_SET_ERROR_MSG ("failed to construct wait set info struct"); - goto fail_ws; - } - ws->inuse = false; - if ((ws->waitseth = dds_create_waitset (gcdds.ppant)) < 0) { - RMW_SET_ERROR_MSG ("failed to create waitset"); - goto fail_waitset; - } - { - std::lock_guard lock (gcdds.lock); - gcdds.waitsets.insert (ws); - } - return wait_set; + static_cast(context); + (void) max_conditions; + rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); + CddsWaitset * ws = nullptr; + RET_ALLOC_X(wait_set, goto fail_alloc_wait_set); + wait_set->implementation_identifier = eclipse_cyclonedds_identifier; + wait_set->data = rmw_allocate(sizeof(CddsWaitset)); + RET_ALLOC_X(wait_set->data, goto fail_alloc_wait_set_data); + // This should default-construct the fields of CddsWaitset + ws = static_cast(wait_set->data); + RMW_TRY_PLACEMENT_NEW(ws, ws, goto fail_placement_new, CddsWaitset, ); + if (!ws) { + RMW_SET_ERROR_MSG("failed to construct wait set info struct"); + goto fail_ws; + } + ws->inuse = false; + if ((ws->waitseth = dds_create_waitset(gcdds.ppant)) < 0) { + RMW_SET_ERROR_MSG("failed to create waitset"); + goto fail_waitset; + } + { + std::lock_guard lock(gcdds.lock); + gcdds.waitsets.insert(ws); + } + return wait_set; - fail_waitset: - fail_ws: - RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE (ws->~CddsWaitset (), ws); - fail_placement_new: - rmw_free (wait_set->data); - fail_alloc_wait_set_data: - rmw_wait_set_free (wait_set); - fail_alloc_wait_set: - return nullptr; +fail_waitset: +fail_ws: + RMW_TRY_DESTRUCTOR_FROM_WITHIN_FAILURE(ws->~CddsWaitset(), ws); +fail_placement_new: + rmw_free(wait_set->data); +fail_alloc_wait_set_data: + rmw_wait_set_free(wait_set); +fail_alloc_wait_set: + return nullptr; } -extern "C" rmw_ret_t rmw_destroy_wait_set (rmw_wait_set_t *wait_set) +extern "C" rmw_ret_t rmw_destroy_wait_set(rmw_wait_set_t * wait_set) { - RET_WRONG_IMPLID (wait_set); - auto result = RMW_RET_OK; - auto ws = static_cast (wait_set->data); - RET_NULL (ws); - dds_delete (ws->waitseth); - { - std::lock_guard lock (gcdds.lock); - gcdds.waitsets.erase (ws); - } - RMW_TRY_DESTRUCTOR (ws->~CddsWaitset (), ws, result = RMW_RET_ERROR); - rmw_free (wait_set->data); - rmw_wait_set_free (wait_set); - return result; + RET_WRONG_IMPLID(wait_set); + auto result = RMW_RET_OK; + auto ws = static_cast(wait_set->data); + RET_NULL(ws); + dds_delete(ws->waitseth); + { + std::lock_guard lock(gcdds.lock); + gcdds.waitsets.erase(ws); + } + RMW_TRY_DESTRUCTOR(ws->~CddsWaitset(), ws, result = RMW_RET_ERROR); + rmw_free(wait_set->data); + rmw_wait_set_free(wait_set); + return result; } template -static bool require_reattach (const std::vector& cached, size_t count, void **ary) +static bool require_reattach(const std::vector & cached, size_t count, void ** ary) { - if (ary == nullptr || count == 0) { - return (cached.size () != 0); - } else if (count != cached.size ()) { - return true; - } else { - return (memcmp (static_cast (cached.data ()), static_cast (ary), count * sizeof (void *)) != 0); - } + if (ary == nullptr || count == 0) { + return cached.size() != 0; + } else if (count != cached.size()) { + return true; + } else { + return memcmp( + static_cast(cached.data()), static_cast(ary), + count * sizeof(void *)) != 0; + } } -static void waitset_detach (CddsWaitset *ws) +static void waitset_detach(CddsWaitset * ws) { - for (auto&& x : ws->subs) dds_waitset_detach (ws->waitseth, x->rdcondh); - for (auto&& x : ws->gcs) dds_waitset_detach (ws->waitseth, x->gcondh); - for (auto&& x : ws->srvs) dds_waitset_detach (ws->waitseth, x->service.sub->rdcondh); - for (auto&& x : ws->cls) dds_waitset_detach (ws->waitseth, x->client.sub->rdcondh); - ws->subs.resize (0); - ws->gcs.resize (0); - ws->srvs.resize (0); - ws->cls.resize (0); + for (auto && x : ws->subs) { + dds_waitset_detach(ws->waitseth, x->rdcondh); + } + for (auto && x : ws->gcs) { + dds_waitset_detach(ws->waitseth, x->gcondh); + } + for (auto && x : ws->srvs) { + dds_waitset_detach(ws->waitseth, x->service.sub->rdcondh); + } + for (auto && x : ws->cls) { + dds_waitset_detach(ws->waitseth, x->client.sub->rdcondh); + } + ws->subs.resize(0); + ws->gcs.resize(0); + ws->srvs.resize(0); + ws->cls.resize(0); } -static void clean_waitset_caches () +static void clean_waitset_caches() { - /* Called whenever a subscriber, guard condition, service or client is deleted (as these may - have been cached in a waitset), and drops all cached entities from all waitsets (just to keep - life simple). I'm assuming one is not allowed to delete an entity while it is still being - used ... */ - std::lock_guard lock (gcdds.lock); - for (auto&& ws : gcdds.waitsets) { - std::lock_guard lock (ws->lock); - if (!ws->inuse) { - waitset_detach (ws); - } + /* Called whenever a subscriber, guard condition, service or client is deleted (as these may + have been cached in a waitset), and drops all cached entities from all waitsets (just to keep + life simple). I'm assuming one is not allowed to delete an entity while it is still being + used ... */ + std::lock_guard lock(gcdds.lock); + for (auto && ws : gcdds.waitsets) { + std::lock_guard lock(ws->lock); + if (!ws->inuse) { + waitset_detach(ws); } + } } -extern "C" rmw_ret_t rmw_wait (rmw_subscriptions_t *subs, rmw_guard_conditions_t *gcs, rmw_services_t *srvs, rmw_clients_t *cls, rmw_events_t *evs, rmw_wait_set_t *wait_set, const rmw_time_t *wait_timeout) +extern "C" rmw_ret_t rmw_wait( + rmw_subscriptions_t * subs, rmw_guard_conditions_t * gcs, + rmw_services_t * srvs, rmw_clients_t * cls, rmw_events_t * evs, + rmw_wait_set_t * wait_set, const rmw_time_t * wait_timeout) { - static_cast(evs); - RET_NULL (wait_set); - CddsWaitset *ws = static_cast (wait_set->data); - RET_NULL (ws); + static_cast(evs); + RET_NULL(wait_set); + CddsWaitset * ws = static_cast(wait_set->data); + RET_NULL(ws); - { - std::lock_guard lock (ws->lock); - if (ws->inuse) { - RMW_SET_ERROR_MSG ("concurrent calls to rmw_wait on a single waitset is not supported"); - return RMW_RET_ERROR; - } - ws->inuse = true; + { + std::lock_guard lock(ws->lock); + if (ws->inuse) { + RMW_SET_ERROR_MSG("concurrent calls to rmw_wait on a single waitset is not supported"); + return RMW_RET_ERROR; } + ws->inuse = true; + } - if (require_reattach (ws->subs, subs ? subs->subscriber_count : 0, subs ? subs->subscribers : nullptr) || - require_reattach (ws->gcs, gcs ? gcs->guard_condition_count : 0, gcs ? gcs->guard_conditions : nullptr) || - require_reattach (ws->srvs, srvs ? srvs->service_count : 0, srvs ? srvs->services : nullptr) || - require_reattach (ws->cls, cls ? cls->client_count : 0, cls ? cls->clients : nullptr)) { - size_t nelems = 0; - waitset_detach (ws); -#define ATTACH(type, var, name, cond) do { \ - ws->var.resize (0); \ - if (var) { \ - ws->var.reserve (var->name##_count); \ - for (size_t i = 0; i < var->name##_count; i++) { \ - auto x = static_cast (var->name##s[i]); \ - ws->var.push_back (x); \ - dds_waitset_attach (ws->waitseth, x->cond, nelems); \ - nelems++; \ - } \ - } \ - } while (0) - ATTACH (CddsSubscription, subs, subscriber, rdcondh); - ATTACH (CddsGuardCondition, gcs, guard_condition, gcondh); - ATTACH (CddsService, srvs, service, service.sub->rdcondh); - ATTACH (CddsClient, cls, client, client.sub->rdcondh); + if (require_reattach(ws->subs, subs ? subs->subscriber_count : 0, + subs ? subs->subscribers : nullptr) || + require_reattach(ws->gcs, gcs ? gcs->guard_condition_count : 0, + gcs ? gcs->guard_conditions : nullptr) || + require_reattach(ws->srvs, srvs ? srvs->service_count : 0, srvs ? srvs->services : nullptr) || + require_reattach(ws->cls, cls ? cls->client_count : 0, cls ? cls->clients : nullptr)) + { + size_t nelems = 0; + waitset_detach(ws); +#define ATTACH(type, var, name, cond) do { \ + ws->var.resize(0); \ + if (var) { \ + ws->var.reserve(var->name ## _count); \ + for (size_t i = 0; i < var->name ## _count; i++) { \ + auto x = static_cast(var->name ## s[i]); \ + ws->var.push_back(x); \ + dds_waitset_attach(ws->waitseth, x->cond, nelems); \ + nelems++; \ + } \ + } \ +} \ + while (0) + ATTACH(CddsSubscription, subs, subscriber, rdcondh); + ATTACH(CddsGuardCondition, gcs, guard_condition, gcondh); + ATTACH(CddsService, srvs, service, service.sub->rdcondh); + ATTACH(CddsClient, cls, client, client.sub->rdcondh); #undef ATTACH - ws->trigs.resize (nelems + 1); - } + ws->trigs.resize(nelems + 1); + } - const dds_duration_t timeout = - (wait_timeout == NULL) ? DDS_NEVER : (dds_duration_t) wait_timeout->sec * 1000000000 + wait_timeout->nsec; - const dds_return_t ntrig = dds_waitset_wait (ws->waitseth, ws->trigs.data (), ws->trigs.capacity (), timeout); - ws->trigs.resize (ntrig); - std::sort (ws->trigs.begin (), ws->trigs.end ()); - ws->trigs.push_back ((dds_attach_t) -1); + const dds_duration_t timeout = + (wait_timeout == NULL) ? + DDS_NEVER : + (dds_duration_t) wait_timeout->sec * 1000000000 + wait_timeout->nsec; + const dds_return_t ntrig = dds_waitset_wait(ws->waitseth, ws->trigs.data(), + ws->trigs.capacity(), timeout); + ws->trigs.resize(ntrig); + std::sort(ws->trigs.begin(), ws->trigs.end()); + ws->trigs.push_back((dds_attach_t) -1); - { - long trig_idx = 0; - bool dummy; - size_t nelems = 0; -#define DETACH(type, var, name, cond, on_triggered) do { \ - if (var) { \ - for (size_t i = 0; i < var->name##_count; i++) { \ - auto x = static_cast (var->name##s[i]); \ - /*dds_waitset_detach (ws->waitseth, x->cond);*/ \ - if (ws->trigs[trig_idx] == (long) nelems) { \ - on_triggered; \ - trig_idx++; \ - } else { \ - var->name##s[i] = nullptr; \ - } \ - nelems++; \ - } \ - } \ - } while (0) - DETACH (CddsSubscription, subs, subscriber, rdcondh, (void) x); - DETACH (CddsGuardCondition, gcs, guard_condition, gcondh, dds_take_guardcondition (x->gcondh, &dummy)); - DETACH (CddsService, srvs, service, service.sub->rdcondh, (void) x); - DETACH (CddsClient, cls, client, client.sub->rdcondh, (void) x); + { + dds_attach_t trig_idx = 0; + bool dummy; + size_t nelems = 0; +#define DETACH(type, var, name, cond, on_triggered) do { \ + if (var) { \ + for (size_t i = 0; i < var->name ## _count; i++) { \ + auto x = static_cast(var->name ## s[i]); \ + /*dds_waitset_detach (ws->waitseth, x->cond);*/ \ + if (ws->trigs[trig_idx] == static_cast(nelems)) { \ + on_triggered; \ + trig_idx++; \ + } else { \ + var->name ## s[i] = nullptr; \ + } \ + nelems++; \ + } \ + } \ +} while (0) + DETACH(CddsSubscription, subs, subscriber, rdcondh, (void) x); + DETACH(CddsGuardCondition, gcs, guard_condition, gcondh, + dds_take_guardcondition(x->gcondh, &dummy)); + DETACH(CddsService, srvs, service, service.sub->rdcondh, (void) x); + DETACH(CddsClient, cls, client, client.sub->rdcondh, (void) x); #undef DETACH - } + } - { - std::lock_guard lock (ws->lock); - ws->inuse = false; - } + { + std::lock_guard lock(ws->lock); + ws->inuse = false; + } - return (ws->trigs.size () == 1) ? RMW_RET_TIMEOUT : RMW_RET_OK; + return (ws->trigs.size() == 1) ? RMW_RET_TIMEOUT : RMW_RET_OK; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -1410,265 +1571,320 @@ extern "C" rmw_ret_t rmw_wait (rmw_subscriptions_t *subs, rmw_guard_conditions_t /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -static rmw_ret_t rmw_take_response_request (CddsCS *cs, rmw_request_id_t *request_header, void *ros_data, bool *taken, dds_instance_handle_t srcfilter) +static rmw_ret_t rmw_take_response_request( + CddsCS * cs, rmw_request_id_t * request_header, + void * ros_data, bool * taken, + dds_instance_handle_t srcfilter) { - RET_NULL (taken); - RET_NULL (ros_data); - RET_NULL (request_header); - cdds_request_wrapper_t wrap; - dds_sample_info_t info; - wrap.data = ros_data; - void *wrap_ptr = static_cast (&wrap); - while (dds_take (cs->sub->subh, &wrap_ptr, &info, 1, 1) == 1) { - if (info.valid_data) { - memset (request_header, 0, sizeof (wrap.header)); - assert (sizeof (wrap.header.guid) <= sizeof (request_header->writer_guid)); - memcpy (request_header->writer_guid, &wrap.header.guid, sizeof (wrap.header.guid)); - request_header->sequence_number = wrap.header.seq; - if (srcfilter == 0 || srcfilter == wrap.header.guid) { - *taken = true; - return RMW_RET_OK; - } - } - } - *taken = false; - return RMW_RET_OK; -} - -extern "C" rmw_ret_t rmw_take_response (const rmw_client_t *client, rmw_request_id_t *request_header, void *ros_response, bool *taken) -{ - RET_WRONG_IMPLID (client); - auto info = static_cast (client->data); - return rmw_take_response_request (&info->client, request_header, ros_response, taken, info->client.pub->pubiid); -} - -extern "C" rmw_ret_t rmw_take_request (const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_request, bool *taken) -{ - RET_WRONG_IMPLID (service); - auto info = static_cast (service->data); - return rmw_take_response_request (&info->service, request_header, ros_request, taken, 0); -} - -static rmw_ret_t rmw_send_response_request (CddsCS *cs, const cdds_request_header_t& header, const void *ros_data) -{ - const cdds_request_wrapper_t wrap = { header, const_cast (ros_data) }; - if (dds_write (cs->pub->pubh, static_cast (&wrap)) >= 0) { + RET_NULL(taken); + RET_NULL(ros_data); + RET_NULL(request_header); + cdds_request_wrapper_t wrap; + dds_sample_info_t info; + wrap.data = ros_data; + void * wrap_ptr = static_cast(&wrap); + while (dds_take(cs->sub->subh, &wrap_ptr, &info, 1, 1) == 1) { + if (info.valid_data) { + memset(request_header, 0, sizeof(wrap.header)); + assert(sizeof(wrap.header.guid) <= sizeof(request_header->writer_guid)); + memcpy(request_header->writer_guid, &wrap.header.guid, sizeof(wrap.header.guid)); + request_header->sequence_number = wrap.header.seq; + if (srcfilter == 0 || srcfilter == wrap.header.guid) { + *taken = true; return RMW_RET_OK; - } else { - RMW_SET_ERROR_MSG ("cannot publish data"); - return RMW_RET_ERROR; + } } + } + *taken = false; + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_send_response (const rmw_service_t *service, rmw_request_id_t *request_header, void *ros_response) +extern "C" rmw_ret_t rmw_take_response( + const rmw_client_t * client, + rmw_request_id_t * request_header, void * ros_response, + bool * taken) { - RET_WRONG_IMPLID (service); - RET_NULL (request_header); - RET_NULL (ros_response); - CddsService *info = static_cast (service->data); - cdds_request_header_t header; - memcpy (&header.guid, request_header->writer_guid, sizeof (header.guid)); - header.seq = request_header->sequence_number; - return rmw_send_response_request (&info->service, header, ros_response); + RET_WRONG_IMPLID(client); + auto info = static_cast(client->data); + return rmw_take_response_request(&info->client, request_header, ros_response, taken, + info->client.pub->pubiid); } -extern "C" rmw_ret_t rmw_send_request (const rmw_client_t *client, const void *ros_request, int64_t *sequence_id) +extern "C" rmw_ret_t rmw_take_request( + const rmw_service_t * service, + rmw_request_id_t * request_header, void * ros_request, + bool * taken) { - static std::atomic_uint next_request_id; - RET_WRONG_IMPLID (client); - RET_NULL (ros_request); - RET_NULL (sequence_id); - auto info = static_cast (client->data); - cdds_request_header_t header; - header.guid = info->client.pub->pubiid; - header.seq = *sequence_id = ++next_request_id; - return rmw_send_response_request (&info->client, header, ros_request); + RET_WRONG_IMPLID(service); + auto info = static_cast(service->data); + return rmw_take_response_request(&info->service, request_header, ros_request, taken, 0); } -static const rosidl_service_type_support_t *get_service_typesupport (const rosidl_service_type_support_t *type_supports) +static rmw_ret_t rmw_send_response_request( + CddsCS * cs, const cdds_request_header_t & header, + const void * ros_data) { - const rosidl_service_type_support_t *ts; - if ((ts = get_service_typesupport_handle (type_supports, rosidl_typesupport_introspection_c__identifier)) != nullptr) { - return ts; - } else if ((ts = get_service_typesupport_handle (type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) { - return ts; - } else { - RMW_SET_ERROR_MSG ("service type support not from this implementation"); - return nullptr; - } -} - -static rmw_ret_t rmw_init_cs (CddsCS *cs, const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies, bool is_service) -{ - RET_WRONG_IMPLID (node); - RET_NULL_OR_EMPTYSTR (service_name); - RET_NULL (qos_policies); - auto node_impl = static_cast (node->data); - RET_NULL (node_impl); - const rosidl_service_type_support_t *type_support = get_service_typesupport (type_supports); - RET_NULL (type_support); - - auto pub = new CddsPublisher (); - auto sub = new CddsSubscription (); - std::string subtopic_name, pubtopic_name; - void *pub_type_support, *sub_type_support; - if (is_service) { - sub_type_support = create_request_type_support (type_support->data, type_support->typesupport_identifier); - pub_type_support = create_response_type_support (type_support->data, type_support->typesupport_identifier); - subtopic_name = make_fqtopic (ros_service_requester_prefix, service_name, "Request", qos_policies); - pubtopic_name = make_fqtopic (ros_service_response_prefix, service_name, "Reply", qos_policies); - } else { - pub_type_support = create_request_type_support (type_support->data, type_support->typesupport_identifier); - sub_type_support = create_response_type_support (type_support->data, type_support->typesupport_identifier); - pubtopic_name = make_fqtopic (ros_service_requester_prefix, service_name, "Request", qos_policies); - subtopic_name = make_fqtopic (ros_service_response_prefix, service_name, "Reply", qos_policies); - } - - RCUTILS_LOG_DEBUG_NAMED ("rmw_cyclonedds_cpp", "************ %s Details *********", is_service ? "Service" : "Client"); - RCUTILS_LOG_DEBUG_NAMED ("rmw_cyclonedds_cpp", "Sub Topic %s", subtopic_name.c_str ()); - RCUTILS_LOG_DEBUG_NAMED ("rmw_cyclonedds_cpp", "Pub Topic %s", pubtopic_name.c_str ()); - RCUTILS_LOG_DEBUG_NAMED ("rmw_cyclonedds_cpp", "***********"); - - dds_entity_t pubtopic, subtopic; - - auto pub_st = create_sertopic (pubtopic_name.c_str (), type_support->typesupport_identifier, pub_type_support, true); - auto sub_st = create_sertopic (subtopic_name.c_str (), type_support->typesupport_identifier, sub_type_support, true); - - dds_qos_t *qos; - if ((pubtopic = dds_create_topic_arbitrary (node_impl->pp, pub_st, nullptr, nullptr, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create topic"); - goto fail_pubtopic; - } - if ((subtopic = dds_create_topic_arbitrary (node_impl->pp, sub_st, nullptr, nullptr, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create topic"); - goto fail_subtopic; - } - if ((qos = dds_create_qos ()) == nullptr) { - goto fail_qos; - } - dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (1)); - dds_qset_history (qos, DDS_HISTORY_KEEP_ALL, DDS_LENGTH_UNLIMITED); - if ((pub->pubh = dds_create_writer (node_impl->pub, pubtopic, qos, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create writer"); - goto fail_writer; - } - /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been - created earlier, but the two are equivalent, so this'll do */ - pub->sertopic = pub_st; - if ((sub->subh = dds_create_reader (node_impl->sub, subtopic, qos, nullptr)) < 0) { - RMW_SET_ERROR_MSG ("failed to create reader"); - goto fail_reader; - } - /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been - created earlier, but the two are equivalent, so this'll do */ - sub->sertopic = sub_st; - if ((sub->rdcondh = dds_create_readcondition (sub->subh, DDS_ANY_STATE)) < 0) { - RMW_SET_ERROR_MSG ("failed to create readcondition"); - goto fail_readcond; - } - 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_delete_qos (qos); - dds_delete (subtopic); - dds_delete (pubtopic); - - cs->pub = pub; - cs->sub = sub; + const cdds_request_wrapper_t wrap = {header, const_cast(ros_data)}; + if (dds_write(cs->pub->pubh, static_cast(&wrap)) >= 0) { return RMW_RET_OK; - - fail_instance_handle: - dds_delete (sub->rdcondh); - fail_readcond: - dds_delete (sub->subh); - fail_reader: - dds_delete (pub->pubh); - fail_writer: - dds_delete_qos (qos); - fail_qos: - dds_delete (subtopic); - fail_subtopic: - dds_delete (pubtopic); - fail_pubtopic: + } else { + RMW_SET_ERROR_MSG("cannot publish data"); return RMW_RET_ERROR; + } } -static void rmw_fini_cs (CddsCS *cs) +extern "C" rmw_ret_t rmw_send_response( + const rmw_service_t * service, + rmw_request_id_t * request_header, void * ros_response) { - ddsi_sertopic_unref (cs->sub->sertopic); - ddsi_sertopic_unref (cs->pub->sertopic); - dds_delete (cs->sub->rdcondh); - dds_delete (cs->sub->subh); - dds_delete (cs->pub->pubh); + RET_WRONG_IMPLID(service); + RET_NULL(request_header); + RET_NULL(ros_response); + CddsService * info = static_cast(service->data); + cdds_request_header_t header; + memcpy(&header.guid, request_header->writer_guid, sizeof(header.guid)); + header.seq = request_header->sequence_number; + return rmw_send_response_request(&info->service, header, ros_response); } -extern "C" rmw_client_t *rmw_create_client (const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies) +extern "C" rmw_ret_t rmw_send_request( + const rmw_client_t * client, const void * ros_request, + int64_t * sequence_id) { - CddsClient *info = new CddsClient (); - if (rmw_init_cs (&info->client, node, type_supports, service_name, qos_policies, false) != RMW_RET_OK) { - delete (info); - return nullptr; + static std::atomic_uint next_request_id; + RET_WRONG_IMPLID(client); + RET_NULL(ros_request); + RET_NULL(sequence_id); + auto info = static_cast(client->data); + cdds_request_header_t header; + header.guid = info->client.pub->pubiid; + header.seq = *sequence_id = ++next_request_id; + return rmw_send_response_request(&info->client, header, ros_request); +} + +static const rosidl_service_type_support_t * get_service_typesupport( + const rosidl_service_type_support_t * type_supports) +{ + const rosidl_service_type_support_t * ts; + if ((ts = + get_service_typesupport_handle(type_supports, + rosidl_typesupport_introspection_c__identifier)) != nullptr) + { + return ts; + } else { + if ((ts = + get_service_typesupport_handle(type_supports, + rosidl_typesupport_introspection_cpp::typesupport_identifier)) != nullptr) + { + return ts; + } else { + RMW_SET_ERROR_MSG("service type support not from this implementation"); + return nullptr; } - rmw_client_t *rmw_client = rmw_client_allocate (); - RET_NULL_X (rmw_client, goto fail_client); - rmw_client->implementation_identifier = eclipse_cyclonedds_identifier; - rmw_client->data = info; - rmw_client->service_name = reinterpret_cast (rmw_allocate (strlen (service_name) + 1)); - RET_NULL_X (rmw_client->service_name, goto fail_service_name); - memcpy (const_cast (rmw_client->service_name), service_name, strlen (service_name) + 1); - return rmw_client; - fail_service_name: - rmw_client_free (rmw_client); - fail_client: - rmw_fini_cs (&info->client); + } +} + +static rmw_ret_t rmw_init_cs( + CddsCS * cs, const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, const rmw_qos_profile_t * qos_policies, + bool is_service) +{ + RET_WRONG_IMPLID(node); + RET_NULL_OR_EMPTYSTR(service_name); + RET_NULL(qos_policies); + auto node_impl = static_cast(node->data); + RET_NULL(node_impl); + const rosidl_service_type_support_t * type_support = get_service_typesupport(type_supports); + RET_NULL(type_support); + + auto pub = new CddsPublisher(); + auto sub = new CddsSubscription(); + std::string subtopic_name, pubtopic_name; + void * pub_type_support, * sub_type_support; + if (is_service) { + sub_type_support = create_request_type_support(type_support->data, + type_support->typesupport_identifier); + pub_type_support = create_response_type_support(type_support->data, + type_support->typesupport_identifier); + subtopic_name = + make_fqtopic(ros_service_requester_prefix, service_name, "Request", qos_policies); + pubtopic_name = make_fqtopic(ros_service_response_prefix, service_name, "Reply", qos_policies); + } else { + pub_type_support = create_request_type_support(type_support->data, + type_support->typesupport_identifier); + sub_type_support = create_response_type_support(type_support->data, + type_support->typesupport_identifier); + pubtopic_name = + make_fqtopic(ros_service_requester_prefix, service_name, "Request", qos_policies); + subtopic_name = make_fqtopic(ros_service_response_prefix, service_name, "Reply", qos_policies); + } + + RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "************ %s Details *********", + is_service ? "Service" : "Client"); + RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "Sub Topic %s", subtopic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "Pub Topic %s", pubtopic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "***********"); + + dds_entity_t pubtopic, subtopic; + + auto pub_st = create_sertopic( + pubtopic_name.c_str(), type_support->typesupport_identifier, pub_type_support, true); + auto sub_st = create_sertopic( + subtopic_name.c_str(), type_support->typesupport_identifier, sub_type_support, true); + + dds_qos_t * qos; + if ((pubtopic = + dds_create_topic_arbitrary(node_impl->pp, pub_st, nullptr, nullptr, nullptr)) < 0) + { + RMW_SET_ERROR_MSG("failed to create topic"); + goto fail_pubtopic; + } + if ((subtopic = + dds_create_topic_arbitrary(node_impl->pp, sub_st, nullptr, nullptr, nullptr)) < 0) + { + RMW_SET_ERROR_MSG("failed to create topic"); + goto fail_subtopic; + } + if ((qos = dds_create_qos()) == nullptr) { + goto fail_qos; + } + dds_qset_reliability(qos, DDS_RELIABILITY_RELIABLE, DDS_SECS(1)); + dds_qset_history(qos, DDS_HISTORY_KEEP_ALL, DDS_LENGTH_UNLIMITED); + if ((pub->pubh = dds_create_writer(node_impl->pub, pubtopic, qos, nullptr)) < 0) { + RMW_SET_ERROR_MSG("failed to create writer"); + goto fail_writer; + } + /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been + created earlier, but the two are equivalent, so this'll do */ + pub->sertopic = pub_st; + if ((sub->subh = dds_create_reader(node_impl->sub, subtopic, qos, nullptr)) < 0) { + RMW_SET_ERROR_MSG("failed to create reader"); + goto fail_reader; + } + /* FIXME: not guaranteed that "topic" will refer to "sertopic" because topic might have been + created earlier, but the two are equivalent, so this'll do */ + sub->sertopic = sub_st; + if ((sub->rdcondh = dds_create_readcondition(sub->subh, DDS_ANY_STATE)) < 0) { + RMW_SET_ERROR_MSG("failed to create readcondition"); + goto fail_readcond; + } + 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_delete_qos(qos); + dds_delete(subtopic); + dds_delete(pubtopic); + + cs->pub = pub; + cs->sub = sub; + return RMW_RET_OK; + +fail_instance_handle: + dds_delete(sub->rdcondh); +fail_readcond: + dds_delete(sub->subh); +fail_reader: + dds_delete(pub->pubh); +fail_writer: + dds_delete_qos(qos); +fail_qos: + dds_delete(subtopic); +fail_subtopic: + dds_delete(pubtopic); +fail_pubtopic: + return RMW_RET_ERROR; +} + +static void rmw_fini_cs(CddsCS * cs) +{ + ddsi_sertopic_unref(cs->sub->sertopic); + ddsi_sertopic_unref(cs->pub->sertopic); + dds_delete(cs->sub->rdcondh); + dds_delete(cs->sub->subh); + dds_delete(cs->pub->pubh); +} + +extern "C" rmw_client_t * rmw_create_client( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, + const rmw_qos_profile_t * qos_policies) +{ + CddsClient * info = new CddsClient(); + if (rmw_init_cs(&info->client, node, type_supports, service_name, qos_policies, + false) != RMW_RET_OK) + { + delete (info); return nullptr; + } + rmw_client_t * rmw_client = rmw_client_allocate(); + RET_NULL_X(rmw_client, goto fail_client); + rmw_client->implementation_identifier = eclipse_cyclonedds_identifier; + rmw_client->data = info; + rmw_client->service_name = reinterpret_cast(rmw_allocate(strlen(service_name) + 1)); + RET_NULL_X(rmw_client->service_name, goto fail_service_name); + memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + return rmw_client; +fail_service_name: + rmw_client_free(rmw_client); +fail_client: + rmw_fini_cs(&info->client); + return nullptr; } -extern "C" rmw_ret_t rmw_destroy_client (rmw_node_t *node, rmw_client_t *client) +extern "C" rmw_ret_t rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) { - RET_WRONG_IMPLID (node); - RET_WRONG_IMPLID (client); - auto info = static_cast (client->data); - clean_waitset_caches (); - rmw_fini_cs (&info->client); - rmw_free (const_cast (client->service_name)); - rmw_client_free (client); - return RMW_RET_OK; + RET_WRONG_IMPLID(node); + RET_WRONG_IMPLID(client); + auto info = static_cast(client->data); + clean_waitset_caches(); + rmw_fini_cs(&info->client); + rmw_free(const_cast(client->service_name)); + rmw_client_free(client); + return RMW_RET_OK; } -extern "C" rmw_service_t *rmw_create_service (const rmw_node_t *node, const rosidl_service_type_support_t *type_supports, const char *service_name, const rmw_qos_profile_t *qos_policies) +extern "C" rmw_service_t * rmw_create_service( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, + const rmw_qos_profile_t * qos_policies) { - CddsService *info = new CddsService (); - if (rmw_init_cs (&info->service, node, type_supports, service_name, qos_policies, true) != RMW_RET_OK) { - delete (info); - return nullptr; - } - rmw_service_t *rmw_service = rmw_service_allocate (); - RET_NULL_X (rmw_service, goto fail_service); - rmw_service->implementation_identifier = eclipse_cyclonedds_identifier; - rmw_service->data = info; - rmw_service->service_name = reinterpret_cast (rmw_allocate (strlen (service_name) + 1)); - RET_NULL_X (rmw_service->service_name, goto fail_service_name); - memcpy (const_cast (rmw_service->service_name), service_name, strlen (service_name) + 1); - return rmw_service; - fail_service_name: - rmw_service_free (rmw_service); - fail_service: - rmw_fini_cs (&info->service); + CddsService * info = new CddsService(); + if (rmw_init_cs(&info->service, node, type_supports, service_name, qos_policies, + true) != RMW_RET_OK) + { + delete (info); return nullptr; + } + rmw_service_t * rmw_service = rmw_service_allocate(); + RET_NULL_X(rmw_service, goto fail_service); + rmw_service->implementation_identifier = eclipse_cyclonedds_identifier; + rmw_service->data = info; + rmw_service->service_name = + reinterpret_cast(rmw_allocate(strlen(service_name) + 1)); + RET_NULL_X(rmw_service->service_name, goto fail_service_name); + memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + return rmw_service; +fail_service_name: + rmw_service_free(rmw_service); +fail_service: + rmw_fini_cs(&info->service); + return nullptr; } -extern "C" rmw_ret_t rmw_destroy_service (rmw_node_t *node, rmw_service_t *service) +extern "C" rmw_ret_t rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) { - RET_WRONG_IMPLID (node); - RET_WRONG_IMPLID (service); - auto info = static_cast (service->data); - clean_waitset_caches (); - rmw_fini_cs (&info->service); - rmw_free (const_cast (service->service_name)); - rmw_service_free (service); - return RMW_RET_OK; + RET_WRONG_IMPLID(node); + RET_WRONG_IMPLID(service); + auto info = static_cast(service->data); + clean_waitset_caches(); + rmw_fini_cs(&info->service); + rmw_free(const_cast(service->service_name)); + rmw_service_free(service); + return RMW_RET_OK; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -1677,395 +1893,513 @@ extern "C" rmw_ret_t rmw_destroy_service (rmw_node_t *node, rmw_service_t *servi /////////// /////////// ///////////////////////////////////////////////////////////////////////////////////////// -static rmw_ret_t do_for_node (std::function oper) +static rmw_ret_t do_for_node( + std::function oper) { - dds_entity_t rd; - if ((rd = dds_create_reader (ref_ppant (), DDS_BUILTIN_TOPIC_DCPSPARTICIPANT, NULL, NULL)) < 0) { - unref_ppant (); - RMW_SET_ERROR_MSG ("rmw_get_node_names: failed to create reader"); - return RMW_RET_ERROR; + dds_entity_t rd; + if ((rd = dds_create_reader(ref_ppant(), DDS_BUILTIN_TOPIC_DCPSPARTICIPANT, NULL, NULL)) < 0) { + unref_ppant(); + RMW_SET_ERROR_MSG("rmw_get_node_names: failed to create reader"); + return RMW_RET_ERROR; + } + dds_sample_info_t info; + void * msg = NULL; + int32_t n; + bool cont = true; + while (cont && (n = dds_take(rd, &msg, &info, 1, 1)) == 1) { + if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { + auto sample = static_cast(msg); + cont = oper(*sample); } - dds_sample_info_t info; - void *msg = NULL; - int32_t n; - bool cont = true; - while (cont && (n = dds_take (rd, &msg, &info, 1, 1)) == 1) { - if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { - auto sample = static_cast (msg); - cont = oper (*sample); - } - dds_return_loan (rd, &msg, n); - } - dds_delete (rd); - unref_ppant (); - if (n < 0) { - RMW_SET_ERROR_MSG ("rmw_get_node_names: error reading participants"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; + dds_return_loan(rd, &msg, n); + } + dds_delete(rd); + unref_ppant(); + if (n < 0) { + RMW_SET_ERROR_MSG("rmw_get_node_names: error reading participants"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; } -static rmw_ret_t do_for_node_user_data (std::function oper) +static rmw_ret_t do_for_node_user_data( + std::function oper) { - auto f = [oper](const dds_builtintopic_participant_t& sample) -> bool { - void *ud; - size_t udsz; - if (dds_qget_userdata (sample.qos, &ud, &udsz) && ud != nullptr) { - /* CycloneDDS guarantees a null-terminated user data so we pretend it's a - string */ - bool ret = oper (sample, static_cast (ud)); - dds_free (ud); - return ret; - } else { - /* If no user data present treat it as an empty string */ - return oper (sample, ""); - } - }; - return do_for_node (f); -} - -extern "C" rmw_ret_t rmw_get_node_names (const rmw_node_t *node, rcutils_string_array_t *node_names, rcutils_string_array_t *node_namespaces) -{ - RET_WRONG_IMPLID (node); - if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK || - rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) { - return RMW_RET_ERROR; - } - - std::set< std::pair > ns; - const auto re = std::regex ("^name=(.*);namespace=(.*);$", std::regex::extended); - auto oper = - [&ns, re](const dds_builtintopic_participant_t& sample, const char *ud) -> bool { - std::cmatch cm; - static_cast(sample); - if (std::regex_search (ud, cm, re)) { - ns.insert (std::make_pair (std::string (cm[1]), std::string (cm[2]))); - } - return true; - }; - rmw_ret_t ret; - if ((ret = do_for_node_user_data (oper)) != RMW_RET_OK) { + auto f = [oper](const dds_builtintopic_participant_t & sample) -> bool { + void * ud; + size_t udsz; + if (dds_qget_userdata(sample.qos, &ud, &udsz) && ud != nullptr) { + /* CycloneDDS guarantees a null-terminated user data so we pretend it's a + string */ + bool ret = oper(sample, static_cast(ud)); + dds_free(ud); return ret; - } + } else { + /* If no user data present treat it as an empty string */ + return oper(sample, ""); + } + }; + return do_for_node(f); +} - rcutils_allocator_t allocator = rcutils_get_default_allocator (); - if (rcutils_string_array_init (node_names, ns.size (), &allocator) != RCUTILS_RET_OK || - rcutils_string_array_init (node_namespaces, ns.size(), &allocator) != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG (rcutils_get_error_string ().str); - goto fail_alloc; +extern "C" rmw_ret_t rmw_get_node_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces) +{ + RET_WRONG_IMPLID(node); + if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK || + rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) + { + return RMW_RET_ERROR; + } + + std::set> ns; + const auto re = std::regex("^name=(.*);namespace=(.*);$", std::regex::extended); + auto oper = + [&ns, re](const dds_builtintopic_participant_t & sample, const char * ud) -> bool { + std::cmatch cm; + static_cast(sample); + if (std::regex_search(ud, cm, re)) { + ns.insert(std::make_pair(std::string(cm[1]), std::string(cm[2]))); + } + return true; + }; + rmw_ret_t ret; + if ((ret = do_for_node_user_data(oper)) != RMW_RET_OK) { + return ret; + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + if (rcutils_string_array_init(node_names, ns.size(), &allocator) != RCUTILS_RET_OK || + rcutils_string_array_init(node_namespaces, ns.size(), &allocator) != RCUTILS_RET_OK) + { + RMW_SET_ERROR_MSG(rcutils_get_error_string().str); + goto fail_alloc; + } + size_t i; + i = 0; + for (auto && n : ns) { + node_names->data[i] = rcutils_strdup(n.first.c_str(), allocator); + node_namespaces->data[i] = rcutils_strdup(n.second.c_str(), allocator); + if (!node_names->data[i] || !node_namespaces->data[i]) { + RMW_SET_ERROR_MSG("rmw_get_node_names for name/namespace"); + goto fail_alloc; } - size_t i; - i = 0; - for (auto&& n : ns) { - node_names->data[i] = rcutils_strdup (n.first.c_str (), allocator); - node_namespaces->data[i] = rcutils_strdup (n.second.c_str (), allocator); - if (!node_names->data[i] || !node_namespaces->data[i]) { - RMW_SET_ERROR_MSG ("rmw_get_node_names for name/namespace"); - goto fail_alloc; - } - i++; - } - return RMW_RET_OK; + i++; + } + return RMW_RET_OK; fail_alloc: - if (node_names) { - if (rcutils_string_array_fini (node_names) != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED ( - "rmw_cyclonedds_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string ().str); - rcutils_reset_error(); - } + if (node_names) { + if (rcutils_string_array_fini(node_names) != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_cyclonedds_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + rcutils_reset_error(); } - if (node_namespaces) { - if (rcutils_string_array_fini(node_namespaces) != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_cyclonedds_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string ().str); - rcutils_reset_error(); - } + } + if (node_namespaces) { + if (rcutils_string_array_fini(node_namespaces) != RCUTILS_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_cyclonedds_cpp", + "failed to cleanup during error handling: %s", rcutils_get_error_string().str); + rcutils_reset_error(); } - return RMW_RET_BAD_ALLOC; + } + return RMW_RET_BAD_ALLOC; } -static rmw_ret_t rmw_collect_tptyp_for_kind (std::map>& tt, dds_entity_t builtin_topic, std::function filter_and_map) +static rmw_ret_t rmw_collect_tptyp_for_kind( + std::map> & tt, + dds_entity_t builtin_topic, + std::function filter_and_map) { - assert (builtin_topic == DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION || builtin_topic == DDS_BUILTIN_TOPIC_DCPSPUBLICATION); - dds_entity_t rd; - if ((rd = dds_create_reader (ref_ppant (), builtin_topic, NULL, NULL)) < 0) { - unref_ppant (); - RMW_SET_ERROR_MSG ("rmw_collect_tptyp_for_kind failed to create reader"); - return RMW_RET_ERROR; - } - dds_sample_info_t info; - void *msg = NULL; - int32_t n; - while ((n = dds_take (rd, &msg, &info, 1, 1)) == 1) { - if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { - auto sample = static_cast (msg); - std::string topic_name, type_name; - if (filter_and_map (*sample, topic_name, type_name)) { - tt[topic_name].insert (type_name); - } - } - dds_return_loan (rd, &msg, n); - } - dds_delete (rd); - unref_ppant (); - if (n == 0) { - return RMW_RET_OK; - } else { - RMW_SET_ERROR_MSG ("rmw_collect_tptyp_for_kind dds_take failed"); - return RMW_RET_ERROR; - } -} - -static rmw_ret_t make_names_and_types (rmw_names_and_types_t *tptyp, const std::map>& source, rcutils_allocator_t *allocator) -{ - if (source.size () == 0) { - return RMW_RET_OK; - } - rmw_ret_t ret; - if ((ret = rmw_names_and_types_init (tptyp, source.size (), allocator)) != RMW_RET_OK) { - return ret; - } - size_t index = 0; - for (const auto& tp : source) { - if ((tptyp->names.data[index] = rcutils_strdup (tp.first.c_str (), *allocator)) == NULL) { - goto fail_mem; - } - if (rcutils_string_array_init (&tptyp->types[index], tp.second.size (), allocator) != RCUTILS_RET_OK) { - goto fail_mem; - } - size_t type_index = 0; - for (const auto& type : tp.second) { - if ((tptyp->types[index].data[type_index] = rcutils_strdup (type.c_str (), *allocator)) == NULL) { - goto fail_mem; - } - type_index++; - } - index++; + assert( + builtin_topic == DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION || + builtin_topic == DDS_BUILTIN_TOPIC_DCPSPUBLICATION); + dds_entity_t rd; + if ((rd = dds_create_reader(ref_ppant(), builtin_topic, NULL, NULL)) < 0) { + unref_ppant(); + RMW_SET_ERROR_MSG("rmw_collect_tptyp_for_kind failed to create reader"); + return RMW_RET_ERROR; + } + dds_sample_info_t info; + void * msg = NULL; + int32_t n; + while ((n = dds_take(rd, &msg, &info, 1, 1)) == 1) { + if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { + auto sample = static_cast(msg); + std::string topic_name, type_name; + if (filter_and_map(*sample, topic_name, type_name)) { + tt[topic_name].insert(type_name); + } } + dds_return_loan(rd, &msg, n); + } + dds_delete(rd); + unref_ppant(); + if (n == 0) { return RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("rmw_collect_tptyp_for_kind dds_take failed"); + return RMW_RET_ERROR; + } +} + +static rmw_ret_t make_names_and_types( + rmw_names_and_types_t * tptyp, const std::map> & source, + rcutils_allocator_t * allocator) +{ + if (source.size() == 0) { + return RMW_RET_OK; + } + rmw_ret_t ret; + if ((ret = rmw_names_and_types_init(tptyp, source.size(), allocator)) != RMW_RET_OK) { + return ret; + } + size_t index = 0; + for (const auto & tp : source) { + if ((tptyp->names.data[index] = rcutils_strdup(tp.first.c_str(), *allocator)) == NULL) { + goto fail_mem; + } + if (rcutils_string_array_init(&tptyp->types[index], tp.second.size(), + allocator) != RCUTILS_RET_OK) + { + goto fail_mem; + } + size_t type_index = 0; + for (const auto & type : tp.second) { + if ((tptyp->types[index].data[type_index] = + rcutils_strdup(type.c_str(), *allocator)) == NULL) + { + goto fail_mem; + } + type_index++; + } + index++; + } + return RMW_RET_OK; fail_mem: - if (rmw_names_and_types_fini (tptyp) != RMW_RET_OK) { - RMW_SET_ERROR_MSG ("rmw_collect_tptyp_for_kind: rmw_names_and_types_fini failed"); - } - return RMW_RET_BAD_ALLOC; + if (rmw_names_and_types_fini(tptyp) != RMW_RET_OK) { + RMW_SET_ERROR_MSG("rmw_collect_tptyp_for_kind: rmw_names_and_types_fini failed"); + } + return RMW_RET_BAD_ALLOC; } -static rmw_ret_t get_node_guids (const char *node_name, const char *node_namespace, std::set& guids) +static rmw_ret_t get_node_guids( + const char * node_name, const char * node_namespace, + std::set & guids) { - std::string needle = get_node_user_data (node_name, node_namespace); - auto oper = [&guids, needle](const dds_builtintopic_participant_t& sample, const char *ud) -> bool { - if (std::string (ud) == needle) { - guids.insert (sample.key); - } - return true; /* do keep looking - what if there are many? */ - }; - rmw_ret_t ret = do_for_node_user_data (oper); - if (ret != RMW_RET_OK) { - return ret; - } else if (guids.size () == 0) { - /* It appears the get_..._by_node operations are supposed to return ERROR if no such node exists */ - return RMW_RET_ERROR; - } else { - return RMW_RET_OK; - } -} - -static rmw_ret_t get_endpoint_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, bool no_demangle, rmw_names_and_types_t *tptyp, bool subs, bool pubs) -{ - RET_WRONG_IMPLID (node); - RET_NULL (allocator); - rmw_ret_t ret = rmw_names_and_types_check_zero (tptyp); - if (ret != RMW_RET_OK) { - return ret; - } - if (node_name) { - int dummy_validation_result; - size_t dummy_invalid_index; - if ((ret = rmw_validate_node_name (node_name, &dummy_validation_result, &dummy_invalid_index)) != RMW_RET_OK) { - return ret; - } - } - std::set guids; - if (node_name != nullptr && (ret = get_node_guids (node_name, node_namespace, guids)) != RMW_RET_OK) { - return ret; - } - const auto re_tp = std::regex ("^" + std::string (ros_topic_prefix) + "(/.*)", std::regex::extended); - const auto re_typ = std::regex ("^(.*::)dds_::(.*)_$", std::regex::extended); - const auto filter_and_map = - [re_tp, re_typ, guids, node_name](const dds_builtintopic_endpoint_t& sample, std::string& topic_name, std::string& type_name) -> bool { - std::cmatch cm_tp, cm_typ; - if (node_name != nullptr && guids.count (sample.participant_key) == 0) { - return false; - } else if (! std::regex_search (sample.topic_name, cm_tp, re_tp) || ! std::regex_search (sample.type_name, cm_typ, re_typ)) { - return false; - } else { - std::string demangled_type = std::regex_replace (std::string (cm_typ[1]), std::regex ("::"), "/"); - topic_name = std::string (cm_tp[1]); - type_name = std::string (demangled_type) + std::string (cm_typ[2]); - return true; - } - }; - std::map> tt; - if (subs && (ret = rmw_collect_tptyp_for_kind (tt, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, filter_and_map)) != RMW_RET_OK) { - return ret; - } - if (pubs && (ret = rmw_collect_tptyp_for_kind (tt, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, filter_and_map)) != RMW_RET_OK) { - return ret; - } - return make_names_and_types (tptyp, tt, allocator); -} - -static bool endpoint_is_from_service (bool is_request, const dds_builtintopic_guid_t& guid) -{ - /* Final byte of GUID is entity type, application writers use 0x02 (write with key) and 0x03 - (writer without key); readers use 0x04 and 0x07. See DDSI specification, table 9.1. - - A service has a reader for a request and a writer for the reply; a client has it the other - way round. Therefore, it is a service iff is_request is set and the GUID indicates it is a - writer. */ - if (is_request) { - return (guid.v[15] == 0x04 || guid.v[15] == 0x07); - } else { - return (guid.v[15] == 0x02 || guid.v[15] == 0x03); - } -} - -static rmw_ret_t get_cs_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, rmw_names_and_types_t *sntyp, bool looking_for_services) -{ - RET_WRONG_IMPLID (node); - RET_NULL (allocator); - rmw_ret_t ret = rmw_names_and_types_check_zero (sntyp); - if (ret != RMW_RET_OK) { - return ret; - } - if (node_name) { - int dummy_validation_result; - size_t dummy_invalid_index; - if ((ret = rmw_validate_node_name (node_name, &dummy_validation_result, &dummy_invalid_index)) != RMW_RET_OK) { - return ret; - } - } - std::set guids; - if (node_name != nullptr && (ret = get_node_guids (node_name, node_namespace, guids)) != RMW_RET_OK) { - return ret; - } - const auto re_tp = std::regex ("^(" + std::string (ros_service_requester_prefix) + "|" + - std::string (ros_service_response_prefix) + ")(/.*)(Request|Reply)$", - std::regex::extended); - const auto re_typ = std::regex ("^(.*::)dds_::(.*)_(Response|Request)_$", std::regex::extended); - const auto filter_and_map = - [re_tp, re_typ, guids, node_name, looking_for_services](const dds_builtintopic_endpoint_t& sample, std::string& topic_name, std::string& type_name) -> bool { - std::cmatch cm_tp, cm_typ; - if (node_name != nullptr && guids.count (sample.participant_key) == 0) { - return false; - } if (! std::regex_search (sample.topic_name, cm_tp, re_tp) || ! std::regex_search (sample.type_name, cm_typ, re_typ)) { - return false; - } else if (looking_for_services != endpoint_is_from_service (std::string (cm_tp[3]) == "Request", sample.key)) { - return false; - } else { - std::string demangled_type = std::regex_replace (std::string (cm_typ[1]), std::regex ("::"), "/"); - topic_name = std::string (cm_tp[2]); - type_name = std::string (demangled_type) + std::string (cm_typ[2]); - return true; - } - }; - std::map> tt; - if ((ret = rmw_collect_tptyp_for_kind (tt, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, filter_and_map)) != RMW_RET_OK || - (ret = rmw_collect_tptyp_for_kind (tt, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, filter_and_map)) != RMW_RET_OK) { - return ret; - } - return make_names_and_types (sntyp, tt, allocator); -} - -extern "C" rmw_ret_t rmw_get_topic_names_and_types (const rmw_node_t *node, rcutils_allocator_t *allocator, bool no_demangle, rmw_names_and_types_t *tptyp) -{ - return get_endpoint_names_and_types_by_node (node, allocator, nullptr, nullptr, no_demangle, tptyp, true, true); -} - -extern "C" rmw_ret_t rmw_get_service_names_and_types (const rmw_node_t *node, rcutils_allocator_t *allocator, rmw_names_and_types_t *sntyp) -{ - return get_cs_names_and_types_by_node (node, allocator, nullptr, nullptr, sntyp, true); -} - -extern "C" rmw_ret_t rmw_service_server_is_available (const rmw_node_t *node, const rmw_client_t *client, bool *is_available) -{ - RET_WRONG_IMPLID (node); - RET_WRONG_IMPLID (client); - RET_NULL (is_available); - auto info = static_cast (client->data); - dds_publication_matched_status_t ps; - dds_subscription_matched_status_t cs; - if (dds_get_publication_matched_status (info->client.pub->pubh, &ps) < 0 || - dds_get_subscription_matched_status (info->client.sub->subh, &cs) < 0) { - RMW_SET_ERROR_MSG ("rmw_service_server_is_available: get_..._matched_status failed"); - return RMW_RET_ERROR; - } - *is_available = ps.current_count > 0 && cs.current_count > 0; + std::string needle = get_node_user_data(node_name, node_namespace); + auto oper = + [&guids, needle](const dds_builtintopic_participant_t & sample, const char * ud) -> bool { + if (std::string(ud) == needle) { + guids.insert(sample.key); + } + return true; /* do keep looking - what if there are many? */ + }; + rmw_ret_t ret = do_for_node_user_data(oper); + if (ret != RMW_RET_OK) { + return ret; + } else if (guids.size() == 0) { + /* It appears the get_..._by_node operations are supposed to return ERROR if no such node + exists */ + return RMW_RET_ERROR; + } else { return RMW_RET_OK; + } } -static rmw_ret_t rmw_count_pubs_or_subs (const rmw_node_t *node, dds_entity_t builtin_topic, const char *topic_name, size_t *count) +static rmw_ret_t get_endpoint_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, bool no_demangle, + rmw_names_and_types_t * tptyp, bool subs, + bool pubs) { - assert (builtin_topic == DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION || builtin_topic == DDS_BUILTIN_TOPIC_DCPSPUBLICATION); - RET_NULL (topic_name); - RET_NULL (count); - RET_WRONG_IMPLID (node); - - std::string fqtopic_name = make_fqtopic (ros_topic_prefix, topic_name, "", false); - dds_entity_t rd; - if ((rd = dds_create_reader (ref_ppant (), builtin_topic, NULL, NULL)) < 0) { - unref_ppant (); - RMW_SET_ERROR_MSG ("rmw_count_pubs_or_subs failed to create reader"); - return RMW_RET_ERROR; + RET_WRONG_IMPLID(node); + RET_NULL(allocator); + rmw_ret_t ret = rmw_names_and_types_check_zero(tptyp); + if (ret != RMW_RET_OK) { + return ret; + } + if (node_name) { + int dummy_validation_result; + size_t dummy_invalid_index; + if ((ret = + rmw_validate_node_name(node_name, &dummy_validation_result, + &dummy_invalid_index)) != RMW_RET_OK) + { + return ret; } - dds_sample_info_t info; - void *msg = NULL; - int32_t n; - *count = 0; - while ((n = dds_take (rd, &msg, &info, 1, 1)) == 1) { - if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { - auto sample = static_cast (msg); - if (fqtopic_name == std::string (sample->topic_name)) { - (*count)++; - } + } + std::set guids; + if (node_name != nullptr && + (ret = get_node_guids(node_name, node_namespace, guids)) != RMW_RET_OK) + { + return ret; + } + const auto re_tp = + std::regex("^" + std::string(ros_topic_prefix) + "(/.*)", std::regex::extended); + const auto re_typ = std::regex("^(.*::)dds_::(.*)_$", std::regex::extended); + const auto filter_and_map = + [re_tp, re_typ, guids, node_name](const dds_builtintopic_endpoint_t & sample, + std::string & topic_name, std::string & type_name) -> bool { + std::cmatch cm_tp, cm_typ; + if (node_name != nullptr && guids.count(sample.participant_key) == 0) { + return false; + } else { + if (!std::regex_search(sample.topic_name, cm_tp, + re_tp) || !std::regex_search(sample.type_name, cm_typ, re_typ)) + { + return false; + } else { + std::string demangled_type = std::regex_replace(std::string(cm_typ[1]), std::regex( + "::"), "/"); + topic_name = std::string(cm_tp[1]); + type_name = std::string(demangled_type) + std::string(cm_typ[2]); + return true; } - dds_return_loan (rd, &msg, n); + } + }; + std::map> tt; + if (subs && + (ret = + rmw_collect_tptyp_for_kind(tt, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, + filter_and_map)) != RMW_RET_OK) + { + return ret; + } + if (pubs && + (ret = + rmw_collect_tptyp_for_kind(tt, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, + filter_and_map)) != RMW_RET_OK) + { + return ret; + } + return make_names_and_types(tptyp, tt, allocator); +} + +static bool endpoint_is_from_service(bool is_request, const dds_builtintopic_guid_t & guid) +{ + /* Final byte of GUID is entity type, application writers use 0x02 (write with key) and 0x03 + (writer without key); readers use 0x04 and 0x07. See DDSI specification, table 9.1. + + A service has a reader for a request and a writer for the reply; a client has it the other + way round. Therefore, it is a service iff is_request is set and the GUID indicates it is a + writer. */ + if (is_request) { + return guid.v[15] == 0x04 || guid.v[15] == 0x07; + } else { + return guid.v[15] == 0x02 || guid.v[15] == 0x03; + } +} + +static rmw_ret_t get_cs_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, const char * node_namespace, + rmw_names_and_types_t * sntyp, + bool looking_for_services) +{ + RET_WRONG_IMPLID(node); + RET_NULL(allocator); + rmw_ret_t ret = rmw_names_and_types_check_zero(sntyp); + if (ret != RMW_RET_OK) { + return ret; + } + if (node_name) { + int dummy_validation_result; + size_t dummy_invalid_index; + if ((ret = + rmw_validate_node_name(node_name, &dummy_validation_result, + &dummy_invalid_index)) != RMW_RET_OK) + { + return ret; } - dds_delete (rd); - unref_ppant (); - return RMW_RET_OK; + } + std::set guids; + if (node_name != nullptr && + (ret = get_node_guids(node_name, node_namespace, guids)) != RMW_RET_OK) + { + return ret; + } + const auto re_tp = std::regex("^(" + std::string(ros_service_requester_prefix) + "|" + + std::string(ros_service_response_prefix) + ")(/.*)(Request|Reply)$", + std::regex::extended); + const auto re_typ = std::regex("^(.*::)dds_::(.*)_(Response|Request)_$", std::regex::extended); + const auto filter_and_map = + [re_tp, re_typ, guids, node_name, + looking_for_services](const dds_builtintopic_endpoint_t & sample, std::string & topic_name, + std::string & type_name) -> bool { + std::cmatch cm_tp, cm_typ; + if (node_name != nullptr && guids.count(sample.participant_key) == 0) { + return false; + } + if (!std::regex_search(sample.topic_name, cm_tp, + re_tp) || !std::regex_search(sample.type_name, cm_typ, re_typ)) + { + return false; + } else { + if (looking_for_services != + endpoint_is_from_service(std::string(cm_tp[3]) == "Request", sample.key)) + { + return false; + } else { + std::string demangled_type = std::regex_replace(std::string(cm_typ[1]), std::regex( + "::"), "/"); + topic_name = std::string(cm_tp[2]); + type_name = std::string(demangled_type) + std::string(cm_typ[2]); + return true; + } + } + }; + std::map> tt; + if ((ret = + rmw_collect_tptyp_for_kind(tt, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, + filter_and_map)) != RMW_RET_OK || + (ret = + rmw_collect_tptyp_for_kind(tt, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, + filter_and_map)) != RMW_RET_OK) + { + return ret; + } + return make_names_and_types(sntyp, tt, allocator); } -extern "C" rmw_ret_t rmw_count_publishers (const rmw_node_t *node, const char *topic_name, size_t *count) +extern "C" rmw_ret_t rmw_get_topic_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, rmw_names_and_types_t * tptyp) { - return rmw_count_pubs_or_subs (node, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, topic_name, count); + return get_endpoint_names_and_types_by_node(node, allocator, nullptr, nullptr, no_demangle, tptyp, + true, true); } -extern "C" rmw_ret_t rmw_count_subscribers (const rmw_node_t *node, const char *topic_name, size_t *count) +extern "C" rmw_ret_t rmw_get_service_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * sntyp) { - return rmw_count_pubs_or_subs (node, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, topic_name, count); + return get_cs_names_and_types_by_node(node, allocator, nullptr, nullptr, sntyp, true); } -extern "C" rmw_ret_t rmw_get_subscriber_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, bool no_demangle, rmw_names_and_types_t *tptyp) +extern "C" rmw_ret_t rmw_service_server_is_available( + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) { - return get_endpoint_names_and_types_by_node (node, allocator, node_name, node_namespace, no_demangle, tptyp, true, false); + RET_WRONG_IMPLID(node); + RET_WRONG_IMPLID(client); + RET_NULL(is_available); + auto info = static_cast(client->data); + dds_publication_matched_status_t ps; + dds_subscription_matched_status_t cs; + if (dds_get_publication_matched_status(info->client.pub->pubh, &ps) < 0 || + dds_get_subscription_matched_status(info->client.sub->subh, &cs) < 0) + { + RMW_SET_ERROR_MSG("rmw_service_server_is_available: get_..._matched_status failed"); + return RMW_RET_ERROR; + } + *is_available = ps.current_count > 0 && cs.current_count > 0; + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_get_publisher_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, bool no_demangle, rmw_names_and_types_t *tptyp) +static rmw_ret_t rmw_count_pubs_or_subs( + const rmw_node_t * node, dds_entity_t builtin_topic, + const char * topic_name, size_t * count) { - return get_endpoint_names_and_types_by_node (node, allocator, node_name, node_namespace, no_demangle, tptyp, false, true); + assert( + builtin_topic == DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION || + builtin_topic == DDS_BUILTIN_TOPIC_DCPSPUBLICATION); + RET_NULL(topic_name); + RET_NULL(count); + RET_WRONG_IMPLID(node); + + std::string fqtopic_name = make_fqtopic(ros_topic_prefix, topic_name, "", false); + dds_entity_t rd; + if ((rd = dds_create_reader(ref_ppant(), builtin_topic, NULL, NULL)) < 0) { + unref_ppant(); + RMW_SET_ERROR_MSG("rmw_count_pubs_or_subs failed to create reader"); + return RMW_RET_ERROR; + } + dds_sample_info_t info; + void * msg = NULL; + int32_t n; + *count = 0; + while ((n = dds_take(rd, &msg, &info, 1, 1)) == 1) { + if (info.valid_data && info.instance_state == DDS_IST_ALIVE) { + auto sample = static_cast(msg); + if (fqtopic_name == std::string(sample->topic_name)) { + (*count)++; + } + } + dds_return_loan(rd, &msg, n); + } + dds_delete(rd); + unref_ppant(); + return RMW_RET_OK; } -extern "C" rmw_ret_t rmw_get_service_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, rmw_names_and_types_t *sntyp) +extern "C" rmw_ret_t rmw_count_publishers( + const rmw_node_t * node, const char * topic_name, + size_t * count) { - return get_cs_names_and_types_by_node (node, allocator, node_name, node_namespace, sntyp, true); + return rmw_count_pubs_or_subs(node, DDS_BUILTIN_TOPIC_DCPSPUBLICATION, topic_name, count); } -extern "C" rmw_ret_t rmw_get_client_names_and_types_by_node (const rmw_node_t *node, rcutils_allocator_t *allocator, const char *node_name, const char *node_namespace, rmw_names_and_types_t *sntyp) +extern "C" rmw_ret_t rmw_count_subscribers( + const rmw_node_t * node, const char * topic_name, + size_t * count) { - return get_cs_names_and_types_by_node (node, allocator, node_name, node_namespace, sntyp, false); + return rmw_count_pubs_or_subs(node, DDS_BUILTIN_TOPIC_DCPSSUBSCRIPTION, topic_name, count); +} + +extern "C" rmw_ret_t rmw_get_subscriber_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * tptyp) +{ + return get_endpoint_names_and_types_by_node(node, allocator, node_name, node_namespace, + no_demangle, tptyp, true, false); +} + +extern "C" rmw_ret_t rmw_get_publisher_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * tptyp) +{ + return get_endpoint_names_and_types_by_node(node, allocator, node_name, node_namespace, + no_demangle, tptyp, false, true); +} + +extern "C" rmw_ret_t rmw_get_service_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * sntyp) +{ + return get_cs_names_and_types_by_node(node, allocator, node_name, node_namespace, sntyp, true); +} + +extern "C" rmw_ret_t rmw_get_client_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * sntyp) +{ + return get_cs_names_and_types_by_node(node, allocator, node_name, node_namespace, sntyp, false); } diff --git a/rmw_cyclonedds_cpp/src/serdata.cpp b/rmw_cyclonedds_cpp/src/serdata.cpp index f38d734..d454b63 100644 --- a/rmw_cyclonedds_cpp/src/serdata.cpp +++ b/rmw_cyclonedds_cpp/src/serdata.cpp @@ -11,9 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include #include +#include #include "rmw/error_handling.h" @@ -26,356 +29,413 @@ #include "dds/ddsi/ddsi_iid.h" #include "dds/ddsi/q_radmin.h" -using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport; -using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport; -using RequestTypeSupport_c = rmw_cyclonedds_cpp::RequestTypeSupport; -using RequestTypeSupport_cpp = rmw_cyclonedds_cpp::RequestTypeSupport; -using ResponseTypeSupport_c = rmw_cyclonedds_cpp::ResponseTypeSupport; -using ResponseTypeSupport_cpp = rmw_cyclonedds_cpp::ResponseTypeSupport; +using MessageTypeSupport_c = + rmw_cyclonedds_cpp::MessageTypeSupport; +using MessageTypeSupport_cpp = + rmw_cyclonedds_cpp::MessageTypeSupport; +using RequestTypeSupport_c = rmw_cyclonedds_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_c__ServiceMembers, + rosidl_typesupport_introspection_c__MessageMembers>; +using RequestTypeSupport_cpp = rmw_cyclonedds_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_cpp::ServiceMembers, + rosidl_typesupport_introspection_cpp::MessageMembers>; +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>; -static bool using_introspection_c_typesupport (const char *typesupport_identifier) +static bool using_introspection_c_typesupport(const char * typesupport_identifier) { - return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; + return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; } -static bool using_introspection_cpp_typesupport (const char *typesupport_identifier) +static bool using_introspection_cpp_typesupport(const char * typesupport_identifier) { - return typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier; + return typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier; } -void *create_message_type_support (const void *untyped_members, const char *typesupport_identifier) +void * create_message_type_support( + const void * untyped_members, + const char * typesupport_identifier) { - if (using_introspection_c_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new MessageTypeSupport_c (members); - } else if (using_introspection_cpp_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new MessageTypeSupport_cpp (members); + if (using_introspection_c_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new MessageTypeSupport_c(members); + } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new MessageTypeSupport_cpp(members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} + +void * create_request_type_support( + const void * untyped_members, + const char * typesupport_identifier) +{ + if (using_introspection_c_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new RequestTypeSupport_c(members); + } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new RequestTypeSupport_cpp(members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} + +void * create_response_type_support( + const void * untyped_members, + const char * typesupport_identifier) +{ + if (using_introspection_c_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new ResponseTypeSupport_c(members); + } else if (using_introspection_cpp_typesupport(typesupport_identifier)) { + auto members = + static_cast(untyped_members); + return new ResponseTypeSupport_cpp(members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} + +static uint32_t serdata_rmw_size(const struct ddsi_serdata * dcmn) +{ + const struct serdata_rmw * d = static_cast(dcmn); + return static_cast(d->data.size()); +} + +static void serdata_rmw_free(struct ddsi_serdata * dcmn) +{ + const struct serdata_rmw * d = static_cast(dcmn); + delete d; +} + +static struct serdata_rmw * rmw_serdata_new( + const struct ddsi_sertopic * topic, + enum ddsi_serdata_kind kind) +{ + struct serdata_rmw * sd = new serdata_rmw; + ddsi_serdata_init(sd, topic, kind); + return sd; +} + +static struct ddsi_serdata * serdata_rmw_from_ser( + const struct ddsi_sertopic * topic, + enum ddsi_serdata_kind kind, + const struct nn_rdata * fragchain, size_t size) +{ + struct serdata_rmw * d = rmw_serdata_new(topic, kind); + uint32_t off = 0; + assert(fragchain->min == 0); + assert(fragchain->maxp1 >= off); /* CDR header must be in first fragment */ + d->data.reserve(size); + while (fragchain) { + if (fragchain->maxp1 > off) { + /* only copy if this fragment adds data */ + const unsigned char * payload = + NN_RMSG_PAYLOADOFF(fragchain->rmsg, NN_RDATA_PAYLOAD_OFF(fragchain)); + d->data.insert( + d->data.end(), payload + off - fragchain->min, payload + fragchain->maxp1 - fragchain->min); + off = fragchain->maxp1; } - RMW_SET_ERROR_MSG ("Unknown typesupport identifier"); - return nullptr; + fragchain = fragchain->nextfrag; + } + return d; } -void *create_request_type_support (const void *untyped_members, const char *typesupport_identifier) +static struct ddsi_serdata * serdata_rmw_from_keyhash( + const struct ddsi_sertopic * topic, + const struct nn_keyhash * keyhash) { - if (using_introspection_c_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new RequestTypeSupport_c (members); - } else if (using_introspection_cpp_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new RequestTypeSupport_cpp (members); + static_cast(keyhash); // unused + /* there is no key field, so from_keyhash is trivial */ + return rmw_serdata_new(topic, SDK_KEY); +} + +static struct ddsi_serdata * serdata_rmw_from_sample( + const struct ddsi_sertopic * topiccmn, + enum ddsi_serdata_kind kind, + const void * sample) +{ + const struct sertopic_rmw * topic = static_cast(topiccmn); + struct serdata_rmw * d = rmw_serdata_new(topic, kind); + if (kind != SDK_DATA) { + /* ROS2 doesn't do keys, so SDK_KEY is trivial */ + } else if (!topic->is_request_header) { + cycser sd(d->data); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(sample, sd, nullptr); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(sample, sd, nullptr); } - RMW_SET_ERROR_MSG ("Unknown typesupport identifier"); - return nullptr; -} - -void *create_response_type_support (const void *untyped_members, const char *typesupport_identifier) -{ - if (using_introspection_c_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new ResponseTypeSupport_c (members); - } else if (using_introspection_cpp_typesupport (typesupport_identifier)) { - auto members = static_cast (untyped_members); - return new ResponseTypeSupport_cpp (members); + } else { + /* The "prefix" lambda is there to inject the service invocation header data into the CDR + stream -- I haven't checked how it is done in the official RMW implementations, so it is + probably incompatible. */ + const cdds_request_wrapper_t * wrap = static_cast(sample); + auto prefix = [wrap](cycser & ser) {ser << wrap->header.guid; ser << wrap->header.seq;}; + cycser sd(d->data); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix); } - RMW_SET_ERROR_MSG ("Unknown typesupport identifier"); - return nullptr; + } + /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector + when copying data to network. Should fix Cyclone to handle that more elegantly. */ + while (d->data.size() % 4) { + d->data.push_back(0); + } + return d; } -static uint32_t serdata_rmw_size (const struct ddsi_serdata *dcmn) +struct ddsi_serdata * serdata_rmw_from_serialized_message( + const struct ddsi_sertopic * topiccmn, + const void * raw, size_t size) { - const struct serdata_rmw *d = static_cast (dcmn); - return static_cast(d->data.size()); + const struct sertopic_rmw * topic = static_cast(topiccmn); + struct serdata_rmw * d = rmw_serdata_new(topic, SDK_DATA); + /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector + when copying data to network. Should fix Cyclone to handle that more elegantly. */ + d->data.resize(size); + memcpy(d->data.data(), raw, size); + while (d->data.size() % 4) { + d->data.push_back(0); + } + return d; } -static void serdata_rmw_free (struct ddsi_serdata *dcmn) +static struct ddsi_serdata * serdata_rmw_to_topicless(const struct ddsi_serdata * dcmn) { - const struct serdata_rmw *d = static_cast (dcmn); - delete d; + const struct serdata_rmw * d = static_cast(dcmn); + struct serdata_rmw * d1 = rmw_serdata_new(d->topic, SDK_KEY); + d1->topic = nullptr; + return d1; } -static struct serdata_rmw *rmw_serdata_new (const struct ddsi_sertopic *topic, enum ddsi_serdata_kind kind) +static void serdata_rmw_to_ser(const struct ddsi_serdata * dcmn, size_t off, size_t sz, void * buf) { - struct serdata_rmw *sd = new serdata_rmw; - ddsi_serdata_init (sd, topic, kind); - return sd; + const struct serdata_rmw * d = static_cast(dcmn); + memcpy(buf, reinterpret_cast(d->data.data()) + off, sz); } -static struct ddsi_serdata *serdata_rmw_from_ser (const struct ddsi_sertopic *topic, enum ddsi_serdata_kind kind, const struct nn_rdata *fragchain, size_t size) +static struct ddsi_serdata * serdata_rmw_to_ser_ref( + const struct ddsi_serdata * dcmn, size_t off, + size_t sz, ddsrt_iovec_t * ref) { - struct serdata_rmw *d = rmw_serdata_new (topic, kind); - uint32_t off = 0; - assert (fragchain->min == 0); - assert (fragchain->maxp1 >= off); /* CDR header must be in first fragment */ - d->data.reserve (size); - while (fragchain) { - if (fragchain->maxp1 > off) { - /* only copy if this fragment adds data */ - const unsigned char *payload = NN_RMSG_PAYLOADOFF (fragchain->rmsg, NN_RDATA_PAYLOAD_OFF (fragchain)); - d->data.insert (d->data.end (), payload + off - fragchain->min, payload + fragchain->maxp1 - fragchain->min); - off = fragchain->maxp1; - } - fragchain = fragchain->nextfrag; - } - return d; + const struct serdata_rmw * d = static_cast(dcmn); + ref->iov_base = const_cast(reinterpret_cast(d->data.data())) + off; + ref->iov_len = (ddsrt_iov_len_t) sz; + return ddsi_serdata_ref(d); } -static struct ddsi_serdata *serdata_rmw_from_keyhash (const struct ddsi_sertopic *topic, const struct nn_keyhash *keyhash) +static void serdata_rmw_to_ser_unref(struct ddsi_serdata * dcmn, const ddsrt_iovec_t * ref) { - static_cast(keyhash); // unused - /* there is no key field, so from_keyhash is trivial */ - return rmw_serdata_new (topic, SDK_KEY); + static_cast(ref); // unused + struct serdata_rmw * d = static_cast(dcmn); + ddsi_serdata_unref(d); } -static struct ddsi_serdata *serdata_rmw_from_sample (const struct ddsi_sertopic *topiccmn, enum ddsi_serdata_kind kind, const void *sample) +static bool serdata_rmw_to_sample( + const struct ddsi_serdata * dcmn, void * sample, void ** bufptr, + void * buflim) { - const struct sertopic_rmw *topic = static_cast (topiccmn); - struct serdata_rmw *d = rmw_serdata_new (topic, kind); - if (kind != SDK_DATA) { - /* ROS2 doesn't do keys, so SDK_KEY is trivial */ - } else if (!topic->is_request_header) { - cycser sd (d->data); - if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage (sample, sd, nullptr); - } else if (using_introspection_cpp_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage (sample, sd, nullptr); - } - } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - const cdds_request_wrapper_t *wrap = static_cast (sample); - auto prefix = [wrap](cycser& ser) { ser << wrap->header.guid; ser << wrap->header.seq; }; - cycser sd (d->data); - if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage (wrap->data, sd, prefix); - } else if (using_introspection_cpp_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage (wrap->data, sd, prefix); - } - } - /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector - when copying data to network. Should fix Cyclone to handle that more elegantly. */ - while (d->data.size () % 4) { - d->data.push_back (0); - } - return d; -} - -struct ddsi_serdata *serdata_rmw_from_serialized_message (const struct ddsi_sertopic *topiccmn, const void *raw, size_t size) -{ - const struct sertopic_rmw *topic = static_cast (topiccmn); - struct serdata_rmw *d = rmw_serdata_new (topic, SDK_DATA); - /* FIXME: CDR padding in DDSI makes me do this to avoid reading beyond the bounds of the vector - when copying data to network. Should fix Cyclone to handle that more elegantly. */ - d->data.resize (size); - memcpy (d->data.data (), raw, size); - while (d->data.size () % 4) { - d->data.push_back (0); - } - return d; -} - -static struct ddsi_serdata *serdata_rmw_to_topicless (const struct ddsi_serdata *dcmn) -{ - const struct serdata_rmw *d = static_cast (dcmn); - struct serdata_rmw *d1 = rmw_serdata_new (d->topic, SDK_KEY); - d1->topic = nullptr; - return d1; -} - -static void serdata_rmw_to_ser (const struct ddsi_serdata *dcmn, size_t off, size_t sz, void *buf) -{ - const struct serdata_rmw *d = static_cast (dcmn); - memcpy (buf, (char *) d->data.data () + off, sz); -} - -static struct ddsi_serdata *serdata_rmw_to_ser_ref (const struct ddsi_serdata *dcmn, size_t off, size_t sz, ddsrt_iovec_t *ref) -{ - const struct serdata_rmw *d = static_cast (dcmn); - ref->iov_base = (char *) d->data.data () + off; - ref->iov_len = (ddsrt_iov_len_t) sz; - return ddsi_serdata_ref (d); -} - -static void serdata_rmw_to_ser_unref (struct ddsi_serdata *dcmn, const ddsrt_iovec_t *ref) -{ - static_cast(ref); // unused - struct serdata_rmw *d = static_cast (dcmn); - ddsi_serdata_unref (d); -} - -static bool serdata_rmw_to_sample (const struct ddsi_serdata *dcmn, void *sample, void **bufptr, void *buflim) -{ - static_cast(bufptr); // unused - static_cast(buflim); // unused - const struct serdata_rmw *d = static_cast (dcmn); - const struct sertopic_rmw *topic = static_cast (d->topic); - assert (bufptr == NULL); - assert (buflim == NULL); - if (d->kind != SDK_DATA) { - /* ROS2 doesn't do keys in a meaningful way yet */ - } else if (!topic->is_request_header) { - cycdeser sd (static_cast (d->data.data ()), d->data.size ()); - if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage (sd, sample, nullptr); - } else if (using_introspection_cpp_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage (sd, sample, nullptr); - } - } else { - /* The "prefix" lambda is there to inject the service invocation header data into the CDR - stream -- I haven't checked how it is done in the official RMW implementations, so it is - probably incompatible. */ - cdds_request_wrapper_t * const wrap = static_cast (sample); - auto prefix = [wrap](cycdeser& ser) { ser >> wrap->header.guid; ser >> wrap->header.seq; }; - cycdeser sd (static_cast (d->data.data ()), d->data.size ()); - if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage (sd, wrap->data, prefix); - } else if (using_introspection_cpp_typesupport (topic->type_support.typesupport_identifier_)) { - auto typed_typesupport = static_cast (topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage (sd, wrap->data, prefix); - } - } - return false; -} - -static bool serdata_rmw_topicless_to_sample (const struct ddsi_sertopic *topic, const struct ddsi_serdata *dcmn, void *sample, void **bufptr, void *buflim) -{ - static_cast(topic); - static_cast(dcmn); - static_cast(sample); - static_cast(bufptr); - static_cast(buflim); + static_cast(bufptr); // unused + static_cast(buflim); // unused + const struct serdata_rmw * d = static_cast(dcmn); + const struct sertopic_rmw * topic = static_cast(d->topic); + assert(bufptr == NULL); + assert(buflim == NULL); + if (d->kind != SDK_DATA) { /* ROS2 doesn't do keys in a meaningful way yet */ - return true; + } else if (!topic->is_request_header) { + cycdeser sd(static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, sample, nullptr); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, sample, nullptr); + } + } else { + /* The "prefix" lambda is there to inject the service invocation header data into the CDR + stream -- I haven't checked how it is done in the official RMW implementations, so it is + probably incompatible. */ + cdds_request_wrapper_t * const wrap = static_cast(sample); + auto prefix = [wrap](cycdeser & ser) {ser >> wrap->header.guid; ser >> wrap->header.seq;}; + cycdeser sd(static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->deserializeROSmessage(sd, wrap->data, prefix); + } + } + return false; } -static bool serdata_rmw_eqkey (const struct ddsi_serdata *a, const struct ddsi_serdata *b) +static bool serdata_rmw_topicless_to_sample( + const struct ddsi_sertopic * topic, + const struct ddsi_serdata * dcmn, void * sample, + void ** bufptr, void * buflim) { - static_cast(a); - static_cast(b); - /* ROS2 doesn't do keys in a meaningful way yet */ - return true; + static_cast(topic); + static_cast(dcmn); + static_cast(sample); + static_cast(bufptr); + static_cast(buflim); + /* ROS2 doesn't do keys in a meaningful way yet */ + return true; +} + +static bool serdata_rmw_eqkey(const struct ddsi_serdata * a, const struct ddsi_serdata * b) +{ + static_cast(a); + static_cast(b); + /* ROS2 doesn't do keys in a meaningful way yet */ + return true; } static const struct ddsi_serdata_ops serdata_rmw_ops = { - serdata_rmw_eqkey, - serdata_rmw_size, - serdata_rmw_from_ser, - serdata_rmw_from_keyhash, - serdata_rmw_from_sample, - serdata_rmw_to_ser, - serdata_rmw_to_ser_ref, - serdata_rmw_to_ser_unref, - serdata_rmw_to_sample, - serdata_rmw_to_topicless, - serdata_rmw_topicless_to_sample, - serdata_rmw_free + serdata_rmw_eqkey, + serdata_rmw_size, + serdata_rmw_from_ser, + serdata_rmw_from_keyhash, + serdata_rmw_from_sample, + serdata_rmw_to_ser, + serdata_rmw_to_ser_ref, + serdata_rmw_to_ser_unref, + serdata_rmw_to_sample, + serdata_rmw_to_topicless, + serdata_rmw_topicless_to_sample, + serdata_rmw_free }; -static void sertopic_rmw_free (struct ddsi_sertopic *tpcmn) +static void sertopic_rmw_free(struct ddsi_sertopic * tpcmn) { - struct sertopic_rmw *tp = static_cast (tpcmn); - delete tp; + struct sertopic_rmw * tp = static_cast(tpcmn); + delete tp; } -static void sertopic_rmw_zero_samples (const struct ddsi_sertopic *d, void *samples, size_t count) +static void sertopic_rmw_zero_samples(const struct ddsi_sertopic * d, void * samples, size_t count) { - static_cast(d); - static_cast(samples); - static_cast(count); - /* Not using code paths that rely on the samples getting zero'd out */ + static_cast(d); + static_cast(samples); + static_cast(count); + /* Not using code paths that rely on the samples getting zero'd out */ } -static void sertopic_rmw_realloc_samples (void **ptrs, const struct ddsi_sertopic *d, void *old, size_t oldcount, size_t count) +static void sertopic_rmw_realloc_samples( + void ** ptrs, const struct ddsi_sertopic * d, void * old, + size_t oldcount, size_t count) { - static_cast(ptrs); - static_cast(d); - static_cast(old); - static_cast(oldcount); - static_cast(count); - /* Not using code paths that rely on this (loans, dispose, unregister with instance handle, - content filters) */ - abort (); + static_cast(ptrs); + static_cast(d); + static_cast(old); + static_cast(oldcount); + static_cast(count); + /* Not using code paths that rely on this (loans, dispose, unregister with instance handle, + content filters) */ + abort(); } -static void sertopic_rmw_free_samples (const struct ddsi_sertopic *d, void **ptrs, size_t count, dds_free_op_t op) +static void sertopic_rmw_free_samples( + const struct ddsi_sertopic * d, void ** ptrs, size_t count, + dds_free_op_t op) { - static_cast(d); // unused - static_cast(ptrs); // unused - static_cast(count); // unused - /* Not using code paths that rely on this (dispose, unregister with instance handle, content - filters) */ - assert (!(op & DDS_FREE_ALL_BIT)); - (void) op; + static_cast(d); // unused + static_cast(ptrs); // unused + static_cast(count); // unused + /* Not using code paths that rely on this (dispose, unregister with instance handle, content + filters) */ + assert(!(op & DDS_FREE_ALL_BIT)); + (void) op; } static const struct ddsi_sertopic_ops sertopic_rmw_ops = { - sertopic_rmw_free, - sertopic_rmw_zero_samples, - sertopic_rmw_realloc_samples, - sertopic_rmw_free_samples + sertopic_rmw_free, + sertopic_rmw_zero_samples, + sertopic_rmw_realloc_samples, + sertopic_rmw_free_samples }; template ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL -inline std::string create_type_name (const void * untyped_members) +inline std::string create_type_name(const void * untyped_members) { - auto members = static_cast (untyped_members); + auto members = static_cast(untyped_members); if (!members) { RMW_SET_ERROR_MSG("members handle is null"); return ""; } std::ostringstream ss; - std::string message_namespace (members->message_namespace_); + std::string message_namespace(members->message_namespace_); // Find and replace C namespace separator with C++, in case this is using C typesupport - message_namespace = std::regex_replace (message_namespace, std::regex("__"), "::"); - std::string message_name (members->message_name_); - if (!message_namespace.empty ()) { + message_namespace = std::regex_replace(message_namespace, std::regex("__"), "::"); + std::string message_name(members->message_name_); + if (!message_namespace.empty()) { ss << message_namespace << "::"; } ss << "dds_::" << message_name << "_"; - return ss.str (); + return ss.str(); } -static std::string get_type_name (const char *type_support_identifier, void *type_support) +static std::string get_type_name(const char * type_support_identifier, void * type_support) { - if (using_introspection_c_typesupport (type_support_identifier)) { - auto typed_typesupport = static_cast (type_support); - return typed_typesupport->getName (); - } else if (using_introspection_cpp_typesupport (type_support_identifier)) { - auto typed_typesupport = static_cast (type_support); - return typed_typesupport->getName (); - } else { - return "absent"; - } + if (using_introspection_c_typesupport(type_support_identifier)) { + auto typed_typesupport = static_cast(type_support); + return typed_typesupport->getName(); + } else if (using_introspection_cpp_typesupport(type_support_identifier)) { + auto typed_typesupport = static_cast(type_support); + return typed_typesupport->getName(); + } else { + return "absent"; + } } -struct sertopic_rmw *create_sertopic (const char *topicname, const char *type_support_identifier, void *type_support, bool is_request_header) +struct sertopic_rmw * create_sertopic( + const char * topicname, const char * type_support_identifier, + void * type_support, bool is_request_header) { - struct sertopic_rmw *st = new struct sertopic_rmw; - memset (st, 0, sizeof (struct ddsi_sertopic)); - st->cpp_name = std::string (topicname); - st->cpp_type_name = get_type_name (type_support_identifier, type_support); - st->cpp_name_type_name = st->cpp_name + std::string (";") + std::string (st->cpp_type_name); - st->ops = &sertopic_rmw_ops; - st->serdata_ops = &serdata_rmw_ops; - st->serdata_basehash = ddsi_sertopic_compute_serdata_basehash (st->serdata_ops); - st->name_type_name = const_cast (st->cpp_name_type_name.c_str ()); - st->name = const_cast (st->cpp_name.c_str ()); - st->type_name = const_cast (st->cpp_type_name.c_str ()); - st->iid = ddsi_iid_gen (); - ddsrt_atomic_st32 (&st->refc, 1); + struct sertopic_rmw * st = new struct sertopic_rmw; + memset(st, 0, sizeof(struct ddsi_sertopic)); + st->cpp_name = std::string(topicname); + st->cpp_type_name = get_type_name(type_support_identifier, type_support); + st->cpp_name_type_name = st->cpp_name + std::string(";") + std::string(st->cpp_type_name); + st->ops = &sertopic_rmw_ops; + st->serdata_ops = &serdata_rmw_ops; + st->serdata_basehash = ddsi_sertopic_compute_serdata_basehash(st->serdata_ops); + st->name_type_name = const_cast(st->cpp_name_type_name.c_str()); + st->name = const_cast(st->cpp_name.c_str()); + st->type_name = const_cast(st->cpp_type_name.c_str()); + st->iid = ddsi_iid_gen(); + ddsrt_atomic_st32(&st->refc, 1); - st->type_support.typesupport_identifier_ = type_support_identifier; - st->type_support.type_support_ = type_support; - st->is_request_header = is_request_header; - return st; + st->type_support.typesupport_identifier_ = type_support_identifier; + st->type_support.type_support_ = type_support; + st->is_request_header = is_request_header; + return st; } diff --git a/rmw_cyclonedds_cpp/src/serdes.cpp b/rmw_cyclonedds_cpp/src/serdes.cpp index eaca6d3..2472df8 100644 --- a/rmw_cyclonedds_cpp/src/serdes.cpp +++ b/rmw_cyclonedds_cpp/src/serdes.cpp @@ -1,21 +1,36 @@ +// Copyright 2018 to 2019 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. +#include + #include "rmw_cyclonedds_cpp/serdes.hpp" -cycser::cycser (std::vector& dst_) - : dst (dst_) - , off (0) +cycser::cycser(std::vector & dst_) +: dst(dst_), + off(0) { - dst.reserve (4); + dst.reserve(4); /* FIXME: hard code to little endian ... and ignoring endianness in deser */ - dst.push_back (0); - dst.push_back (3); + dst.push_back(0); + dst.push_back(3); /* options: */ - dst.push_back (0); - dst.push_back (0); + dst.push_back(0); + dst.push_back(0); } -cycdeser::cycdeser (const void *data_, size_t size_) - : data (static_cast (data_) + 4) - , pos (0) - , lim (size_ - 4) +cycdeser::cycdeser(const void * data_, size_t size_) +: data(static_cast(data_) + 4), + pos(0), + lim(size_ - 4) { }