niklas/rmw_cyclonedds
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

update for Cyclone DDS changes and ROS2 changes

Browse source 
The changes in this commit make it compile with ROS2 Crystal Clemmys and
current Cyclone DDS.  The RMW interface of ROS2 was modified in some
ways and extended in some other ways since Bouncy Bolson; and similarly,
Cyclone now has a somewhat reasonable interface for custom sample
representations and serialization, but the code in this commit probably
contains mistakes in using it.

Therefore, the expectation should be that this doesn't actually work
just yet, though it probably is quite close.  As the old state wouldn't
build at all with any version of Cyclone DDS except the early commits,
this is significant progress already.
This commit is contained in:
Erik Boasson 2019-04-29 09:33:13 +02:00
parent 40a042c6dc
commit c9a23a9b8a
9 changed files with 1202 additions and 1079 deletions
Download patch file Download diff file Expand all files Collapse all files

5
rmw_cyclonedds_cpp/CMakeLists.txt
View file

@ -52,12 +52,11 @@ include_directories(include)
add_library(rmw_cyclonedds_cpp add_library(rmw_cyclonedds_cpp
src/namespace_prefix.cpp src/namespace_prefix.cpp
src/rmw_node.cpp src/rmw_node.cpp
src/serdata.cpp
src/serdes.cpp src/serdes.cpp
) )
idlc_generate(rmw_cyclonedds_topic_lib src/rmw_cyclonedds_topic.idl) target_link_libraries(rmw_cyclonedds_cpp CycloneDDS::ddsc)
target_link_libraries(rmw_cyclonedds_cpp
rmw_cyclonedds_topic_lib CycloneDDS::ddsc)
ament_target_dependencies(rmw_cyclonedds_cpp ament_target_dependencies(rmw_cyclonedds_cpp
"rcutils" "rcutils"

4
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp
View file

@ -59,13 +59,13 @@ struct StringHelper<rosidl_typesupport_introspection_c__MessageMembers>
if (!c_string) { if (!c_string) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rmw_cyclonedds_cpp", "rmw_cyclonedds_cpp",
"Failed to cast data as rosidl_generator_c__String") "Failed to cast data as rosidl_generator_c__String");
return ""; return "";
} }
if (!c_string->data) { if (!c_string->data) {
RCUTILS_LOG_ERROR_NAMED( RCUTILS_LOG_ERROR_NAMED(
"rmw_cyclonedds_cpp", "rmw_cyclonedds_cpp",
"rosidl_generator_c_String had invalid data") "rosidl_generator_c_String had invalid data");
return ""; return "";
} }
return std::string(c_string->data); return std::string(c_string->data);

90
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp
View file

@ -30,7 +30,7 @@
#include "rosidl_typesupport_introspection_c/message_introspection.h" #include "rosidl_typesupport_introspection_c/message_introspection.h"
#include "rosidl_typesupport_introspection_c/service_introspection.h" #include "rosidl_typesupport_introspection_c/service_introspection.h"
#include "rosidl_generator_c/primitives_array_functions.h" #include "rosidl_generator_c/primitives_sequence_functions.h"
#include "serdes.hpp" #include "serdes.hpp"
@ -38,37 +38,37 @@ namespace rmw_cyclonedds_cpp
{ {
template<typename T> template<typename T>
struct GenericCArray; struct GenericCSequence;
// multiple definitions of ambiguous primitive types // multiple definitions of ambiguous primitive types
SPECIALIZE_GENERIC_C_ARRAY(bool, bool) SPECIALIZE_GENERIC_C_SEQUENCE(bool, bool)
SPECIALIZE_GENERIC_C_ARRAY(byte, uint8_t) SPECIALIZE_GENERIC_C_SEQUENCE(byte, uint8_t)
SPECIALIZE_GENERIC_C_ARRAY(char, char) SPECIALIZE_GENERIC_C_SEQUENCE(char, char)
SPECIALIZE_GENERIC_C_ARRAY(float32, float) SPECIALIZE_GENERIC_C_SEQUENCE(float32, float)
SPECIALIZE_GENERIC_C_ARRAY(float64, double) SPECIALIZE_GENERIC_C_SEQUENCE(float64, double)
SPECIALIZE_GENERIC_C_ARRAY(int8, int8_t) SPECIALIZE_GENERIC_C_SEQUENCE(int8, int8_t)
SPECIALIZE_GENERIC_C_ARRAY(int16, int16_t) SPECIALIZE_GENERIC_C_SEQUENCE(int16, int16_t)
SPECIALIZE_GENERIC_C_ARRAY(uint16, uint16_t) SPECIALIZE_GENERIC_C_SEQUENCE(uint16, uint16_t)
SPECIALIZE_GENERIC_C_ARRAY(int32, int32_t) SPECIALIZE_GENERIC_C_SEQUENCE(int32, int32_t)
SPECIALIZE_GENERIC_C_ARRAY(uint32, uint32_t) SPECIALIZE_GENERIC_C_SEQUENCE(uint32, uint32_t)
SPECIALIZE_GENERIC_C_ARRAY(int64, int64_t) SPECIALIZE_GENERIC_C_SEQUENCE(int64, int64_t)
SPECIALIZE_GENERIC_C_ARRAY(uint64, uint64_t) SPECIALIZE_GENERIC_C_SEQUENCE(uint64, uint64_t)
typedef struct rosidl_generator_c__void__Array typedef struct rosidl_generator_c__void__Sequence
{ {
void * data; void * data;
/// The number of valid items in data /// The number of valid items in data
size_t size; size_t size;
/// The number of allocated items in data /// The number of allocated items in data
size_t capacity; size_t capacity;
} rosidl_generator_c__void__Array; } rosidl_generator_c__void__Sequence;
inline inline
bool bool
rosidl_generator_c__void__Array__init( rosidl_generator_c__void__Sequence__init(
rosidl_generator_c__void__Array * array, size_t size, size_t member_size) rosidl_generator_c__void__Sequence * sequence, size_t size, size_t member_size)
{ {
if (!array) { if (!sequence) {
return false; return false;
} }
void * data = nullptr; void * data = nullptr;
@ -78,31 +78,31 @@ rosidl_generator_c__void__Array__init(
return false; return false;
} }
} }
array->data = data; sequence->data = data;
array->size = size; sequence->size = size;
array->capacity = size; sequence->capacity = size;
return true; return true;
} }
inline inline
void void
rosidl_generator_c__void__Array__fini(rosidl_generator_c__void__Array * array) rosidl_generator_c__void__Sequence__fini(rosidl_generator_c__void__Sequence * sequence)
{ {
if (!array) { if (!sequence) {
return; return;
} }
if (array->data) { if (sequence->data) {
// ensure that data and capacity values are consistent // ensure that data and capacity values are consistent
assert(array->capacity > 0); assert(sequence->capacity > 0);
// finalize all array elements // finalize all sequence elements
free(array->data); free(sequence->data);
array->data = nullptr; sequence->data = nullptr;
array->size = 0; sequence->size = 0;
array->capacity = 0; sequence->capacity = 0;
} else { } else {
// ensure that data, size, and capacity values are consistent // ensure that data, size, and capacity values are consistent
assert(0 == array->size); assert(0 == sequence->size);
assert(0 == array->capacity); assert(0 == sequence->capacity);
} }
} }
@ -231,7 +231,7 @@ void serialize_field(
} else if (member->array_size_ && !member->is_upper_bound_) { } else if (member->array_size_ && !member->is_upper_bound_) {
ser.serializeA(static_cast<T *>(field), member->array_size_); ser.serializeA(static_cast<T *>(field), member->array_size_);
} else { } else {
auto & data = *reinterpret_cast<typename GenericCArray<T>::type *>(field); auto & data = *reinterpret_cast<typename GenericCSequence<T>::type *>(field);
ser.serializeS(reinterpret_cast<T *>(data.data), data.size); ser.serializeS(reinterpret_cast<T *>(data.data), data.size);
} }
} }
@ -264,7 +264,7 @@ void serialize_field<std::string>(
ser.serialize(tmpstring); ser.serialize(tmpstring);
} }
} else { } else {
auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Array *>(field); auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Sequence *>(field);
std::vector<std::string> cpp_string_vector; std::vector<std::string> cpp_string_vector;
for (size_t i = 0; i < string_array_field.size; ++i) { for (size_t i = 0; i < string_array_field.size; ++i) {
cpp_string_vector.push_back( cpp_string_vector.push_back(
@ -300,12 +300,12 @@ size_t get_array_size_and_assign_field(
void * & subros_message, void * & subros_message,
size_t, size_t) size_t, size_t)
{ {
auto tmparray = static_cast<rosidl_generator_c__void__Array *>(field); auto tmpsequence = static_cast<rosidl_generator_c__void__Sequence *>(field);
if (member->is_upper_bound_ && tmparray->size > member->array_size_) { if (member->is_upper_bound_ && tmpsequence->size > member->array_size_) {
throw std::runtime_error("vector overcomes the maximum length"); throw std::runtime_error("vector overcomes the maximum length");
} }
subros_message = reinterpret_cast<void *>(tmparray->data); subros_message = reinterpret_cast<void *>(tmpsequence->data);
return tmparray->size; return tmpsequence->size;
} }
template<typename MembersType> template<typename MembersType>
@ -434,10 +434,10 @@ void deserialize_field(
} else if (member->array_size_ && !member->is_upper_bound_) { } else if (member->array_size_ && !member->is_upper_bound_) {
deser.deserializeA(static_cast<T *>(field), member->array_size_); deser.deserializeA(static_cast<T *>(field), member->array_size_);
} else { } else {
auto & data = *reinterpret_cast<typename GenericCArray<T>::type *>(field); auto & data = *reinterpret_cast<typename GenericCSequence<T>::type *>(field);
int32_t dsize = 0; int32_t dsize = 0;
deser >> dsize; deser >> dsize;
GenericCArray<T>::init(&data, dsize); GenericCSequence<T>::init(&data, dsize);
deser.deserializeA(reinterpret_cast<T *>(data.data), dsize); deser.deserializeA(reinterpret_cast<T *>(data.data), dsize);
} }
} }
@ -469,8 +469,8 @@ inline void deserialize_field<std::string>(
std::vector<std::string> cpp_string_vector; std::vector<std::string> cpp_string_vector;
deser >> cpp_string_vector; deser >> cpp_string_vector;
auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Array *>(field); auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Sequence *>(field);
if (!rosidl_generator_c__String__Array__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"); throw std::runtime_error("unable to initialize rosidl_generator_c__String array");
} }
@ -521,8 +521,8 @@ inline size_t get_submessage_array_deserialize(
// Deserialize length // Deserialize length
uint32_t vsize = 0; uint32_t vsize = 0;
deser >> vsize; deser >> vsize;
auto tmparray = static_cast<rosidl_generator_c__void__Array *>(field); auto tmparray = static_cast<rosidl_generator_c__void__Sequence *>(field);
rosidl_generator_c__void__Array__init(tmparray, vsize, sub_members_size); rosidl_generator_c__void__Sequence__init(tmparray, vsize, sub_members_size);
subros_message = reinterpret_cast<void *>(tmparray->data); subros_message = reinterpret_cast<void *>(tmparray->data);
return vsize; return vsize;
} }

10
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/macros.hpp
View file

@ -18,18 +18,18 @@
#include <limits> #include <limits>
#include <string> #include <string>
#define SPECIALIZE_GENERIC_C_ARRAY(C_NAME, C_TYPE) \ #define SPECIALIZE_GENERIC_C_SEQUENCE(C_NAME, C_TYPE) \
template<> \ template<> \
struct GenericCArray<C_TYPE> \ struct GenericCSequence<C_TYPE> \
{ \ { \
using type = rosidl_generator_c__ ## C_NAME ## __Array; \ using type = rosidl_generator_c__ ## C_NAME ## __Sequence; \
\ \
static void fini(type * array) { \ static void fini(type * array) { \
rosidl_generator_c__ ## C_NAME ## __Array__fini(array); \ rosidl_generator_c__ ## C_NAME ## __Sequence__fini(array); \
} \ } \
\ \
static bool init(type * array, size_t size) { \ static bool init(type * array, size_t size) { \
return rosidl_generator_c__ ## C_NAME ## __Array__init(array, size); \ return rosidl_generator_c__ ## C_NAME ## __Sequence__init(array, size); \
} \ } \
}; };

55
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp Normal file
View file

@ -0,0 +1,55 @@
// Copyright 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.
#ifndef SERDATA_HPP
#define SERDATA_HPP
#include "dds/ddsi/ddsi_sertopic.h"
#include "dds/ddsi/ddsi_serdata.h"
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 serdata_rmw : ddsi_serdata {
/* first two bytes of data is CDR encoding
second two bytes are encoding options */
std::vector<unsigned char> data;
};
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;
} 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);
struct sertopic_rmw *create_sertopic (const char *topicname, const char *type_support_identifier, void *type_support, bool is_request_header);
#endif

100
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp
View file

@ -20,42 +20,10 @@
#include <vector> #include <vector>
#include <type_traits> #include <type_traits>
#include "ddsc/dds.h"
extern "C" {
struct serstate;
struct serdata;
struct sertopic;
struct serdata *ddsi_serdata_ref(struct serdata *serdata);
void ddsi_serdata_unref(struct serdata *serdata);
struct serstate *ddsi_serstate_new(const struct sertopic *topic);
struct serdata *ddsi_serstate_fix(struct serstate *st);
void ddsi_serstate_release(struct serstate *st);
void *ddsi_serstate_append(struct serstate *st, size_t n);
void *ddsi_serstate_append_align(struct serstate *st, size_t a);
void *ddsi_serstate_append_aligned(struct serstate *st, size_t n, size_t a);
void ddsi_serdata_getblob(void **raw, size_t *sz, struct serdata *serdata);
}
template<class T>
struct simpletype : std::integral_constant< bool, std::is_arithmetic<T>::value && !std::is_same<bool, T>::value > {};
template<class T>
struct complextype : std::integral_constant< bool, !(std::is_arithmetic<T>::value && !std::is_same<bool, T>::value) > {};
template<unsigned N> struct rank : rank<N-1> {};
template<> struct rank<0> {};
class cycser { class cycser {
public: public:
cycser(struct sertopic *topic); cycser (std::vector<unsigned char>& dst_);
cycser () = delete; cycser () = delete;
~cycser();
cycser& fix();
cycser& ref();
void unref();
struct serdata *data();
inline cycser& operator<< (bool 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<< (char x) { serialize (x); return *this; }
@ -74,8 +42,12 @@ public:
template<class T, size_t S> inline cycser& operator<< (const std::array<T, S>& x) { serialize (x); return *this; } template<class T, size_t S> inline cycser& operator<< (const std::array<T, S>& x) { serialize (x); return *this; }
#define SIMPLE(T) inline void serialize (T x) { \ #define SIMPLE(T) inline void serialize (T x) { \
T *p = (T *)ddsi_serstate_append_aligned(st, sizeof(T), sizeof(T)); \ if ((off % sizeof (T)) != 0) { \
*p = x; \ off += sizeof (T) - (off % sizeof (T)); \
} \
reserve (off + sizeof (T)); \
*(T *) (data () + off) = x; \
off += sizeof (T); \
} }
SIMPLE (char); SIMPLE (char);
SIMPLE (unsigned char); SIMPLE (unsigned char);
@ -96,21 +68,26 @@ public:
{ {
size_t sz = strlen (x) + 1; size_t sz = strlen (x) + 1;
serialize (static_cast<uint32_t> (sz)); serialize (static_cast<uint32_t> (sz));
char *p = (char *)ddsi_serstate_append(st, sz); reserve (off + sz);
memcpy(p, x, sz); memcpy (data () + off, x, sz);
off += sz;
} }
inline void serialize (const std::string& x) inline void serialize (const std::string& x)
{ {
size_t sz = x.size(); size_t sz = x.size () + 1;
serialize(static_cast<uint32_t>(sz+1)); serialize (static_cast<uint32_t> (sz));
char *p = (char *)ddsi_serstate_append(st, sz+1); reserve (off + sz);
memcpy(p, x.data(), sz); memcpy (data () + off, x.data (), sz - 1);
p[sz] = 0; *(data () + off + sz - 1) = 0;
} }
#define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \ #define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \
void *p = (void *)ddsi_serstate_append_aligned(st, (cnt) * sizeof(T), sizeof(T)); \ if ((off % sizeof (T)) != 0) { \
memcpy(p, (void *)x, (cnt) * sizeof(T)); \ off += sizeof (T) - (off % sizeof (T)); \
} \
reserve (off + cnt * sizeof (T)); \
memcpy (data () + off, (void *) x, cnt * sizeof (T)); \
off += cnt * sizeof (T); \
} }
SIMPLEA (char); SIMPLEA (char);
SIMPLEA (unsigned char); SIMPLEA (unsigned char);
@ -144,8 +121,11 @@ public:
} }
private: private:
struct serstate *st; inline void reserve (size_t n) { dst.reserve (n + 4); }
struct serdata *sd; inline unsigned char *data () { return dst.data () + 4; }
std::vector<unsigned char>& dst;
size_t off;
}; };
class cycdeser { class cycdeser {
@ -153,7 +133,6 @@ public:
// FIXME: byteswapping // FIXME: byteswapping
cycdeser (const void *data, size_t size); cycdeser (const void *data, size_t size);
cycdeser () = delete; cycdeser () = delete;
~cycdeser();
inline cycdeser& operator>> (bool& 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>> (char& x) { deserialize (x); return *this; }
@ -173,8 +152,8 @@ public:
#define SIMPLE(T) inline void deserialize (T& x) { \ #define SIMPLE(T) inline void deserialize (T& x) { \
align (sizeof (x)); \ align (sizeof (x)); \
x = *reinterpret_cast<const T *>(data); \ x = *reinterpret_cast<const T *> (data + pos); \
data += sizeof(x); \ pos += sizeof (x); \
} }
SIMPLE (char); SIMPLE (char);
SIMPLE (unsigned char); SIMPLE (unsigned char);
@ -195,16 +174,21 @@ public:
uint32_t sz; deserialize (sz); return sz; uint32_t sz; deserialize (sz); return sz;
} }
inline void deserialize (char *& x) { inline void deserialize (char *& x) {
const uint32_t sz = deserialize32(); x = (char *)malloc(sz); memcpy(x, data, sz); data += sz; const uint32_t sz = deserialize32 ();
x = (char *) malloc (sz);
memcpy (x, data + pos, sz);
pos += sz;
} }
inline void deserialize (std::string& x) { inline void deserialize (std::string& x) {
const uint32_t sz = deserialize32(); x = std::string(data, sz-1); data += sz; 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) { \ #define SIMPLEA(T) inline void deserializeA (T *x, size_t cnt) { \
align (sizeof (T)); \ align (sizeof (T)); \
memcpy((void *)x, (void *)data, (cnt) * sizeof(T)); \ memcpy ((void *) x, (void *) (data + pos), (cnt) * sizeof (T)); \
data += (cnt) * sizeof(T); \ pos += (cnt) * sizeof (T); \
} }
SIMPLEA (char); SIMPLEA (char);
SIMPLEA (unsigned char); SIMPLEA (unsigned char);
@ -229,8 +213,8 @@ public:
inline void deserialize (std::vector<bool>& x) { inline void deserialize (std::vector<bool>& x) {
const uint32_t sz = deserialize32 (); const uint32_t sz = deserialize32 ();
x.resize (sz); x.resize (sz);
for (auto&& i : x) i = (data[i] != 0); for (auto&& i : x) i = ((data + pos)[i] != 0);
data += sz; pos += sz;
} }
template<class T, size_t S> inline void deserialize (std::array<T, S>& x) { template<class T, size_t S> inline void deserialize (std::array<T, S>& x) {
deserializeA (x.data (), x.size ()); deserializeA (x.data (), x.size ());
@ -239,10 +223,8 @@ public:
private: private:
inline void align (size_t a) inline void align (size_t a)
{ {
const uintptr_t x = reinterpret_cast<uintptr_t>(data); if ((pos % a) != 0) {
if ((x % a) != 0) { pos += a - (pos % a);
const uintptr_t pad = a - (x % a);
data = reinterpret_cast<const char *>(x + pad);
} }
} }

360
rmw_cyclonedds_cpp/src/rmw_node.cpp
View file

@ -22,45 +22,22 @@
- topic creation: shouldn't leak topics - topic creation: shouldn't leak topics
- hash set of writer handles should be thread safe: no guarantee that no writers get - introspection stuff not done yet
added/deleted in parallel to each other or to takes
- introspection stuff not done yet (probably requires additions to cdds)
- check / make sure a node remains valid while one of its subscriptions exists - check / make sure a node remains valid while one of its subscriptions exists
- writecdr/takecdr interface abuse is beyond redemption
- service/client simply use the instance handle of its publisher as its GUID -- yikes! but it is - service/client simply use the instance handle of its publisher as its GUID -- yikes! but it is
actually only kinda wrong because the instance handles allocated by different instance of cdds actually only kinda wrong because the instance handles allocated by different instance of cdds
are actually taken from uncorrelated (close to uncorrelated anyway) permutations of 64-bit are actually taken from uncorrelated (close to uncorrelated anyway) permutations of 64-bit
unsigned integers unsigned integers
- ... and many more things ... - ... and probably many more things ...
*/ */
#include <algorithm>
#include <array>
#include <atomic>
#include <cassert>
#include <condition_variable>
#include <limits>
#include <list>
#include <map>
#include <mutex> #include <mutex>
#include <set>
#include <string>
#include <string>
#include <utility>
#include <vector>
#include <thread>
#include <unordered_set> #include <unordered_set>
#include "rcutils/allocator.h"
#include "rcutils/filesystem.h"
#include "rcutils/logging_macros.h" #include "rcutils/logging_macros.h"
#include "rcutils/strdup.h"
#include "rcutils/types.h"
#include "rmw/allocators.h" #include "rmw/allocators.h"
#include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/convert_rcutils_ret_to_rmw_ret.h"
@ -71,15 +48,15 @@
#include "rmw/impl/cpp/macros.hpp" #include "rmw/impl/cpp/macros.hpp"
#include "namespace_prefix.hpp"
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp" #include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp" #include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
#include "ddsc/dds.h" #include "namespace_prefix.hpp"
#include "rmw_cyclonedds_topic.h" #include "dds/dds.h"
#include "dds/ddsi/ddsi_sertopic.h"
#include "rmw_cyclonedds_cpp/serdes.hpp" #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_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_NULL_X(var, code) do { if (!var) RET_ERR_X (#var " is null", code); } while (0)
@ -103,11 +80,6 @@
const char *const adlink_cyclonedds_identifier = "rmw_cyclonedds_cpp"; const char *const adlink_cyclonedds_identifier = "rmw_cyclonedds_cpp";
struct CddsTypeSupport {
void *type_support_;
const char *typesupport_identifier_;
};
/* instance handles are unsigned 64-bit integers carefully constructed to be as close to uniformly /* 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 distributed as possible for no other reason than making them near-perfect hash keys, hence we can
improve over the default hash function */ improve over the default hash function */
@ -120,22 +92,16 @@ public:
struct CddsNode { struct CddsNode {
rmw_guard_condition_t *graph_guard_condition; rmw_guard_condition_t *graph_guard_condition;
std::unordered_set<dds_instance_handle_t, dds_instance_handle_hash> own_writers;
}; };
struct CddsPublisher { struct CddsPublisher {
dds_entity_t pubh; dds_entity_t pubh;
dds_instance_handle_t pubiid; dds_instance_handle_t pubiid;
struct sertopic *sertopic;
CddsTypeSupport ts;
}; };
struct CddsSubscription { struct CddsSubscription {
dds_entity_t subh; dds_entity_t subh;
dds_entity_t rdcondh; dds_entity_t rdcondh;
CddsNode *node;
bool ignore_local_publications;
CddsTypeSupport ts;
}; };
struct CddsCS { struct CddsCS {
@ -176,133 +142,10 @@ struct Cdds {
Cdds () : refcount (0), ppant (0) {} Cdds () : refcount (0), ppant (0) {}
}; };
typedef struct cdds_request_header {
uint64_t guid;
int64_t seq;
} cdds_request_header_t;
static Cdds gcdds; static Cdds gcdds;
using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>;
using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>;
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>;
extern "C" {
struct dds_entity;
struct dds_domain;
int32_t dds_entity_lock(dds_entity_t hdl, dds_entity_kind_t kind, struct dds_entity **e);
void dds_entity_unlock(struct dds_entity *e);
struct sertopic *dds_topic_lookup (struct dds_domain *domain, const char *name);
dds_domain *dds__entity_domain(dds_entity* e);
void sertopic_free (struct sertopic * tp);
}
static void clean_waitset_caches (); static void clean_waitset_caches ();
static struct sertopic *get_sertopic(dds_entity_t topic, const std::string& name)
{
struct dds_entity *x;
struct sertopic *sertopic;
if (dds_entity_lock(topic, DDS_KIND_TOPIC, &x) < 0) {
abort();
}
sertopic = dds_topic_lookup(dds__entity_domain(x), name.c_str());
dds_entity_unlock(x);
return sertopic;
}
static bool using_introspection_c_typesupport(const char *typesupport_identifier)
{
return typesupport_identifier == rosidl_typesupport_introspection_c__identifier;
}
static bool using_introspection_cpp_typesupport(const char *typesupport_identifier)
{
return typesupport_identifier == rosidl_typesupport_introspection_cpp::typesupport_identifier;
}
static void *create_message_type_support(const void *untyped_members, const char *typesupport_identifier)
{
if (using_introspection_c_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_c__MessageMembers *>(untyped_members);
return new MessageTypeSupport_c(members);
} else if (using_introspection_cpp_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *>(untyped_members);
return new MessageTypeSupport_cpp(members);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return nullptr;
}
static void *create_request_type_support(const void *untyped_members, const char *typesupport_identifier)
{
if (using_introspection_c_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *>(untyped_members);
return new RequestTypeSupport_c(members);
} else if (using_introspection_cpp_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *>(untyped_members);
return new RequestTypeSupport_cpp(members);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return nullptr;
}
static void *create_response_type_support(const void *untyped_members, const char *typesupport_identifier)
{
if (using_introspection_c_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *>(untyped_members);
return new ResponseTypeSupport_c(members);
} else if (using_introspection_cpp_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *>(untyped_members);
return new ResponseTypeSupport_cpp(members);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return nullptr;
}
template<typename ServiceType> const void *get_request_ptr(const void *untyped_service_members)
{
auto service_members = static_cast<const ServiceType *>(untyped_service_members);
RET_NULL_X(service_members, return nullptr);
return service_members->request_members_;
}
template<typename ServiceType> const void *get_response_ptr(const void *untyped_service_members)
{
auto service_members = static_cast<const ServiceType *>(untyped_service_members);
RET_NULL_X(service_members, return nullptr);
return service_members->response_members_;
}
static bool sermsg(const void *ros_message, cycser& ser, std::function<void(cycser&)> prefix, const CddsTypeSupport& ts)
{
if (using_introspection_c_typesupport(ts.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_c *>(ts.type_support_);
return typed_typesupport->serializeROSmessage(ros_message, ser, prefix);
} else if (using_introspection_cpp_typesupport(ts.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_cpp *>(ts.type_support_);
return typed_typesupport->serializeROSmessage(ros_message, ser, prefix);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return false;
}
static bool desermsg(cycdeser& deser, void *ros_message, std::function<void(cycdeser&)> prefix, const CddsTypeSupport& ts)
{
if (using_introspection_c_typesupport(ts.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_c *>(ts.type_support_);
return typed_typesupport->deserializeROSmessage(deser, ros_message, prefix);
} else if (using_introspection_cpp_typesupport(ts.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_cpp *>(ts.type_support_);
return typed_typesupport->deserializeROSmessage(deser, ros_message, prefix);
}
RMW_SET_ERROR_MSG("Unknown typesupport identifier");
return false;
}
#pragma GCC visibility push (default) #pragma GCC visibility push (default)
extern "C" const char *rmw_get_implementation_identifier () extern "C" const char *rmw_get_implementation_identifier ()
@ -310,7 +153,7 @@ extern "C" const char *rmw_get_implementation_identifier()
return adlink_cyclonedds_identifier; return adlink_cyclonedds_identifier;
} }
extern "C" rmw_ret_t rmw_init() extern "C" rmw_ret_t rmw_init (const rmw_init_options_t *options __attribute__ ((unused)), rmw_context_t *context __attribute__ ((unused)))
{ {
return RMW_RET_OK; return RMW_RET_OK;
} }
@ -343,7 +186,7 @@ static void unref_ppant()
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
extern "C" rmw_node_t *rmw_create_node(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 __attribute__ ((unused)), const char *name, const char *namespace_, size_t domain_id, const rmw_node_security_options_t *security_options)
{ {
RET_NULL_X (name, return nullptr); RET_NULL_X (name, return nullptr);
RET_NULL_X (namespace_, return nullptr); RET_NULL_X (namespace_, return nullptr);
@ -358,7 +201,7 @@ extern "C" rmw_node_t *rmw_create_node(const char *name, const char *namespace_,
rmw_node_t *node_handle = nullptr; rmw_node_t *node_handle = nullptr;
RET_ALLOC_X (node_impl, goto fail_node_impl); RET_ALLOC_X (node_impl, goto fail_node_impl);
rmw_guard_condition_t *graph_guard_condition; rmw_guard_condition_t *graph_guard_condition;
if (!(graph_guard_condition = rmw_create_guard_condition())) { if (!(graph_guard_condition = rmw_create_guard_condition (context))) {
goto fail_ggc; goto fail_ggc;
} }
node_impl->graph_guard_condition = graph_guard_condition; node_impl->graph_guard_condition = graph_guard_condition;
@ -428,34 +271,19 @@ extern "C" const rmw_guard_condition_t *rmw_node_get_graph_guard_condition(const
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
extern "C" {
int dds_writecdr(dds_entity_t writer, struct serdata *serdata);
}
static rmw_ret_t rmw_write_ser(dds_entity_t pubh, cycser& sd)
{
if (dds_writecdr(pubh, sd.fix().ref().data()) >= 0) {
return RMW_RET_OK;
} else {
/* FIXME: what is the expected behavior when it times out? */
RMW_SET_ERROR_MSG("cannot publish data");
//return RMW_RET_ERROR;
return RMW_RET_OK;
}
}
extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *ros_message) extern "C" rmw_ret_t rmw_publish (const rmw_publisher_t *publisher, const void *ros_message)
{ {
RET_WRONG_IMPLID (publisher); RET_WRONG_IMPLID (publisher);
RET_NULL (ros_message); RET_NULL (ros_message);
auto pub = static_cast<CddsPublisher *> (publisher->data); auto pub = static_cast<CddsPublisher *> (publisher->data);
assert (pub); assert (pub);
cycser sd(pub->sertopic); if (dds_write (pub->pubh, ros_message) >= 0) {
if (sermsg(ros_message, sd, nullptr, pub->ts)) { return RMW_RET_OK;
return rmw_write_ser(pub->pubh, sd);
} else { } else {
RMW_SET_ERROR_MSG("cannot serialize data"); /* FIXME: what is the expected behavior when it times out? */
return RMW_RET_ERROR; RMW_SET_ERROR_MSG ("cannot publish data");
//return RMW_RET_ERROR;
return RMW_RET_OK;
} }
} }
@ -486,15 +314,15 @@ static std::string make_fqtopic(const char *prefix, const char *topic_name, cons
return make_fqtopic (prefix, topic_name, suffix, qos_policies->avoid_ros_namespace_conventions); 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) static dds_qos_t *create_readwrite_qos (const rmw_qos_profile_t *qos_policies, bool ignore_local_publications)
{ {
dds_qos_t *qos = dds_qos_create(); dds_qos_t *qos = dds_create_qos ();
switch (qos_policies->history) { switch (qos_policies->history) {
case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT:
case RMW_QOS_POLICY_HISTORY_KEEP_LAST: case RMW_QOS_POLICY_HISTORY_KEEP_LAST:
if (qos_policies->depth > INT32_MAX) { if (qos_policies->depth > INT32_MAX) {
RMW_SET_ERROR_MSG ("unsupported history depth"); RMW_SET_ERROR_MSG ("unsupported history depth");
dds_qos_delete(qos); dds_delete_qos (qos);
return nullptr; return nullptr;
} }
dds_qset_history (qos, DDS_HISTORY_KEEP_LAST, static_cast<uint32_t> (qos_policies->depth)); dds_qset_history (qos, DDS_HISTORY_KEEP_LAST, static_cast<uint32_t> (qos_policies->depth));
@ -521,6 +349,9 @@ static dds_qos_t *create_readwrite_qos(const rmw_qos_profile_t *qos_policies)
dds_qset_durability (qos, DDS_DURABILITY_TRANSIENT_LOCAL); dds_qset_durability (qos, DDS_DURABILITY_TRANSIENT_LOCAL);
break; break;
} }
if (ignore_local_publications) {
dds_qset_ignorelocal (qos, DDS_IGNORELOCAL_PARTICIPANT);
}
return qos; return qos;
} }
@ -537,28 +368,26 @@ static CddsPublisher *create_cdds_publisher(const rmw_node_t *node, const rosidl
dds_entity_t topic; dds_entity_t topic;
dds_qos_t *qos; dds_qos_t *qos;
pub->ts.typesupport_identifier_ = type_support->typesupport_identifier;
pub->ts.type_support_ = create_message_type_support(type_support->data, pub->ts.typesupport_identifier_);
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);
if ((topic = dds_create_topic(gcdds.ppant, &rmw_cyclonedds_topic_desc, fqtopic_name.c_str(), nullptr, nullptr)) < 0) { auto sertopic = create_sertopic (topic_name, type_support->typesupport_identifier, create_message_type_support (type_support->data, type_support->typesupport_identifier), false);
if ((topic = dds_create_topic_arbitrary (gcdds.ppant, sertopic, fqtopic_name.c_str (), nullptr, nullptr, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create topic"); RMW_SET_ERROR_MSG ("failed to create topic");
goto fail_topic; goto fail_topic;
} }
if ((qos = create_readwrite_qos(qos_policies)) == nullptr) { if ((qos = create_readwrite_qos (qos_policies, false)) == nullptr) {
goto fail_qos; goto fail_qos;
} }
if ((pub->pubh = dds_create_writer (gcdds.ppant, topic, qos, nullptr)) < 0) { if ((pub->pubh = dds_create_writer (gcdds.ppant, topic, qos, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create writer"); RMW_SET_ERROR_MSG ("failed to create writer");
goto fail_writer; goto fail_writer;
} }
pub->sertopic = get_sertopic(topic, fqtopic_name);
if (dds_get_instance_handle (pub->pubh, &pub->pubiid) < 0) { if (dds_get_instance_handle (pub->pubh, &pub->pubiid) < 0) {
RMW_SET_ERROR_MSG ("failed to get instance handle for writer"); RMW_SET_ERROR_MSG ("failed to get instance handle for writer");
goto fail_instance_handle; goto fail_instance_handle;
} }
dds_qos_delete(qos); dds_delete_qos (qos);
node_impl->own_writers.insert(pub->pubiid); ddsi_sertopic_unref (sertopic);
/* FIXME: leak the topic for now */ /* FIXME: leak the topic for now */
return pub; return pub;
@ -567,12 +396,13 @@ static CddsPublisher *create_cdds_publisher(const rmw_node_t *node, const rosidl
RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to destroy writer during error handling"); RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to destroy writer during error handling");
} }
fail_writer: fail_writer:
dds_qos_delete(qos); dds_delete_qos (qos);
fail_qos: fail_qos:
/* not deleting topic -- have to sort out proper topic handling & that requires fixing a few /* not deleting topic -- have to sort out proper topic handling & that requires fixing a few
things in cyclone as well */ things in cyclone as well */
fail_topic: fail_topic:
delete pub; delete pub;
ddsi_sertopic_unref (sertopic);
return nullptr; return nullptr;
} }
@ -580,7 +410,6 @@ extern "C" rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const r
{ {
CddsPublisher *pub; CddsPublisher *pub;
rmw_publisher_t *rmw_publisher; rmw_publisher_t *rmw_publisher;
auto node_impl = static_cast<CddsNode *>(node->data);
if ((pub = create_cdds_publisher (node, type_supports, topic_name, qos_policies)) == nullptr) { if ((pub = create_cdds_publisher (node, type_supports, topic_name, qos_policies)) == nullptr) {
goto fail_common_init; goto fail_common_init;
} }
@ -595,7 +424,6 @@ extern "C" rmw_publisher_t *rmw_create_publisher(const rmw_node_t *node, const r
fail_topic_name: fail_topic_name:
rmw_publisher_free (rmw_publisher); rmw_publisher_free (rmw_publisher);
fail_publisher: fail_publisher:
node_impl->own_writers.erase(pub->pubiid);
if (dds_delete (pub->pubh) < 0) { if (dds_delete (pub->pubh) < 0) {
RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete writer during error handling"); RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete writer during error handling");
} }
@ -632,10 +460,8 @@ extern "C" rmw_ret_t rmw_destroy_publisher(rmw_node_t *node, rmw_publisher_t *pu
{ {
RET_WRONG_IMPLID (node); RET_WRONG_IMPLID (node);
RET_WRONG_IMPLID (publisher); RET_WRONG_IMPLID (publisher);
auto node_impl = static_cast<CddsNode *>(node->data);
auto pub = static_cast<CddsPublisher *> (publisher->data); auto pub = static_cast<CddsPublisher *> (publisher->data);
if (pub != nullptr) { if (pub != nullptr) {
node_impl->own_writers.erase(pub->pubiid);
if (dds_delete (pub->pubh) < 0) { if (dds_delete (pub->pubh) < 0) {
RMW_SET_ERROR_MSG ("failed to delete writer"); RMW_SET_ERROR_MSG ("failed to delete writer");
} }
@ -666,17 +492,14 @@ static CddsSubscription *create_cdds_subscription(const rmw_node_t *node, const
dds_entity_t topic; dds_entity_t topic;
dds_qos_t *qos; dds_qos_t *qos;
sub->node = node_impl;
sub->ignore_local_publications = ignore_local_publications;
sub->ts.typesupport_identifier_ = type_support->typesupport_identifier;
sub->ts.type_support_ = create_message_type_support(type_support->data, sub->ts.typesupport_identifier_);
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);
if ((topic = dds_create_topic(gcdds.ppant, &rmw_cyclonedds_topic_desc, fqtopic_name.c_str(), nullptr, nullptr)) < 0) { auto sertopic = create_sertopic (topic_name, type_support->typesupport_identifier, create_message_type_support (type_support->data, type_support->typesupport_identifier), false);
if ((topic = dds_create_topic_arbitrary (gcdds.ppant, sertopic, fqtopic_name.c_str (), nullptr, nullptr, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create topic"); RMW_SET_ERROR_MSG ("failed to create topic");
goto fail_topic; goto fail_topic;
} }
if ((qos = create_readwrite_qos(qos_policies)) == nullptr) { if ((qos = create_readwrite_qos (qos_policies, ignore_local_publications)) == nullptr) {
goto fail_qos; goto fail_qos;
} }
if ((sub->subh = dds_create_reader (gcdds.ppant, topic, qos, nullptr)) < 0) { if ((sub->subh = dds_create_reader (gcdds.ppant, topic, qos, nullptr)) < 0) {
@ -687,18 +510,20 @@ static CddsSubscription *create_cdds_subscription(const rmw_node_t *node, const
RMW_SET_ERROR_MSG ("failed to create readcondition"); RMW_SET_ERROR_MSG ("failed to create readcondition");
goto fail_readcond; goto fail_readcond;
} }
dds_qos_delete(qos); dds_delete_qos (qos);
ddsi_sertopic_unref (sertopic);
return sub; return sub;
fail_readcond: fail_readcond:
if (dds_delete (sub->subh) < 0) { if (dds_delete (sub->subh) < 0) {
RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete reader during error handling"); RCUTILS_LOG_ERROR_NAMED ("rmw_cyclonedds_cpp", "failed to delete reader during error handling");
} }
fail_reader: fail_reader:
dds_qos_delete(qos); dds_delete_qos (qos);
fail_qos: fail_qos:
/* FIXME: leak topic */ /* FIXME: leak topic */
fail_topic: fail_topic:
delete sub; delete sub;
ddsi_sertopic_unref (sertopic);
return nullptr; return nullptr;
} }
@ -759,17 +584,9 @@ static rmw_ret_t rmw_take_int(const rmw_subscription_t *subscription, void *ros_
RET_WRONG_IMPLID (subscription); RET_WRONG_IMPLID (subscription);
CddsSubscription *sub = static_cast<CddsSubscription *> (subscription->data); CddsSubscription *sub = static_cast<CddsSubscription *> (subscription->data);
RET_NULL (sub); RET_NULL (sub);
struct serdata *sd;
dds_sample_info_t info; dds_sample_info_t info;
while (dds_takecdr(sub->subh, &sd, 1, &info, DDS_ANY_SAMPLE_STATE | DDS_ANY_VIEW_STATE | DDS_ANY_INSTANCE_STATE) == 1) { while (dds_take (sub->subh, &ros_message, &info, 1, 1) == 1) {
if (info.valid_data && !(sub->ignore_local_publications && sub->node->own_writers.count(info.publication_handle))) { if (info.valid_data) {
size_t sz;
void *raw;
ddsi_serdata_getblob(&raw, &sz, sd);
/* FIXME: endianness (i.e., the "+ 4") */
cycdeser deser(static_cast<void *>(static_cast<char *>(raw) + 4), sz);
desermsg(deser, ros_message, nullptr, sub->ts);
ddsi_serdata_unref(sd);
if (message_info) { if (message_info) {
message_info->publisher_gid.implementation_identifier = adlink_cyclonedds_identifier; message_info->publisher_gid.implementation_identifier = adlink_cyclonedds_identifier;
memset (message_info->publisher_gid.data, 0, sizeof (message_info->publisher_gid.data)); memset (message_info->publisher_gid.data, 0, sizeof (message_info->publisher_gid.data));
@ -778,8 +595,6 @@ static rmw_ret_t rmw_take_int(const rmw_subscription_t *subscription, void *ros_
} }
*taken = true; *taken = true;
return RMW_RET_OK; return RMW_RET_OK;
} else {
ddsi_serdata_unref(sd);
} }
} }
*taken = false; *taken = false;
@ -802,7 +617,7 @@ extern "C" rmw_ret_t rmw_take_with_info(const rmw_subscription_t *subscription,
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
extern "C" rmw_guard_condition_t *rmw_create_guard_condition() extern "C" rmw_guard_condition_t *rmw_create_guard_condition (rmw_context_t *context __attribute__ ((unused)))
{ {
rmw_guard_condition_t *guard_condition_handle; rmw_guard_condition_t *guard_condition_handle;
auto *gcond_impl = new CddsGuardCondition (); auto *gcond_impl = new CddsGuardCondition ();
@ -1032,28 +847,20 @@ static rmw_ret_t rmw_take_response_request(CddsCS *cs, rmw_request_id_t *request
RET_NULL (taken); RET_NULL (taken);
RET_NULL (ros_data); RET_NULL (ros_data);
RET_NULL (request_header); RET_NULL (request_header);
struct serdata *sd; cdds_request_wrapper_t wrap;
dds_sample_info_t info; dds_sample_info_t info;
while (dds_takecdr(cs->sub->subh, &sd, 1, &info, DDS_ANY_SAMPLE_STATE | DDS_ANY_VIEW_STATE | DDS_ANY_INSTANCE_STATE) == 1) { wrap.data = ros_data;
void *wrap_ptr = static_cast<void *> (&wrap);
while (dds_take (cs->sub->subh, &wrap_ptr, &info, 1, 1) == 1) {
if (info.valid_data) { if (info.valid_data) {
size_t sz; memset (request_header, 0, sizeof (wrap.header));
void *raw; assert (sizeof (wrap.header.guid) < sizeof (wrap.header.guid));
ddsi_serdata_getblob(&raw, &sz, sd); memcpy (request_header->writer_guid, &wrap.header.guid, sizeof (wrap.header.guid));
/* FIXME: endianness (i.e., the "+ 4") */ request_header->sequence_number = wrap.header.seq;
cycdeser deser(static_cast<void *>(static_cast<char *>(raw) + 4), sz); if (srcfilter == 0 || srcfilter == wrap.header.guid) {
cdds_request_header_t header;
desermsg(deser, ros_data, [&header](cycdeser& ser) { ser >> header.guid; ser >> header.seq; }, cs->sub->ts);
ddsi_serdata_unref(sd);
memset(request_header, 0, sizeof(*request_header));
assert(sizeof(header.guid) < sizeof(request_header->writer_guid));
memcpy(request_header->writer_guid, &header.guid, sizeof(header.guid));
request_header->sequence_number = header.seq;
if (srcfilter == 0 || srcfilter == header.guid) {
*taken = true; *taken = true;
return RMW_RET_OK; return RMW_RET_OK;
} }
} else {
ddsi_serdata_unref(sd);
} }
} }
*taken = false; *taken = false;
@ -1074,14 +881,16 @@ extern "C" rmw_ret_t rmw_take_request(const rmw_service_t *service, rmw_request_
return rmw_take_response_request (&info->service, request_header, ros_request, taken, 0); return rmw_take_response_request (&info->service, request_header, ros_request, taken, 0);
} }
static rmw_ret_t rmw_send_response_request(CddsCS *cs, cdds_request_header_t *header, const void *ros_data) static rmw_ret_t rmw_send_response_request (CddsCS *cs, const cdds_request_header_t& header, const void *ros_data)
{ {
cycser sd(cs->pub->sertopic); const cdds_request_wrapper_t wrap = { header, const_cast<void *> (ros_data) };
if (sermsg(ros_data, sd, [&header](cycser& ser) { ser << header->guid; ser << header->seq; }, cs->pub->ts)) { if (dds_write (cs->pub->pubh, static_cast<const void *> (&wrap)) >= 0) {
return rmw_write_ser(cs->pub->pubh, sd); return RMW_RET_OK;
} else { } else {
RMW_SET_ERROR_MSG("cannot serialize data"); /* FIXME: what is the expected behavior when it times out? */
return RMW_RET_ERROR; RMW_SET_ERROR_MSG ("cannot publish data");
//return RMW_RET_ERROR;
return RMW_RET_OK;
} }
} }
@ -1094,7 +903,7 @@ extern "C" rmw_ret_t rmw_send_response(const rmw_service_t *service, rmw_request
cdds_request_header_t header; cdds_request_header_t header;
memcpy (&header.guid, request_header->writer_guid, sizeof (header.guid)); memcpy (&header.guid, request_header->writer_guid, sizeof (header.guid));
header.seq = request_header->sequence_number; header.seq = request_header->sequence_number;
return rmw_send_response_request(&info->service, &header, ros_response); return rmw_send_response_request (&info->service, header, ros_response);
} }
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_send_request (const rmw_client_t *client, const void *ros_request, int64_t *sequence_id)
@ -1107,7 +916,7 @@ extern "C" rmw_ret_t rmw_send_request(const rmw_client_t *client, const void *ro
cdds_request_header_t header; cdds_request_header_t header;
header.guid = info->client.pub->pubiid; header.guid = info->client.pub->pubiid;
header.seq = *sequence_id = next_request_id++; header.seq = *sequence_id = next_request_id++;
return rmw_send_response_request(&info->client, &header, ros_request); 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) static const rosidl_service_type_support_t *get_service_typesupport (const rosidl_service_type_support_t *type_supports)
@ -1136,36 +945,39 @@ static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_se
auto pub = new CddsPublisher (); auto pub = new CddsPublisher ();
auto sub = new CddsSubscription (); auto sub = new CddsSubscription ();
std::string subtopic_name, pubtopic_name; std::string subtopic_name, pubtopic_name;
pub->ts.typesupport_identifier_ = type_support->typesupport_identifier; void *pub_type_support, *sub_type_support;
sub->ts.typesupport_identifier_ = type_support->typesupport_identifier;
if (is_service) { if (is_service) {
sub->ts.type_support_ = create_request_type_support(type_support->data, type_support->typesupport_identifier); sub_type_support = create_request_type_support (type_support->data, type_support->typesupport_identifier);
pub->ts.type_support_ = create_response_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); 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); pubtopic_name = make_fqtopic (ros_service_response_prefix, service_name, "_reply", qos_policies);
} else { } else {
pub->ts.type_support_ = create_request_type_support(type_support->data, type_support->typesupport_identifier); pub_type_support = create_request_type_support (type_support->data, type_support->typesupport_identifier);
sub->ts.type_support_ = create_response_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); 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); 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", "************ %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", "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", "Pub Topic %s", pubtopic_name.c_str ());
RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "***********") RCUTILS_LOG_DEBUG_NAMED ("rmw_cyclonedds_cpp", "***********");
dds_entity_t pubtopic, subtopic; 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; dds_qos_t *qos;
if ((pubtopic = dds_create_topic(gcdds.ppant, &rmw_cyclonedds_topic_desc, pubtopic_name.c_str(), nullptr, nullptr)) < 0) { if ((pubtopic = dds_create_topic_arbitrary (gcdds.ppant, pub_st, pubtopic_name.c_str (), nullptr, nullptr, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create topic"); RMW_SET_ERROR_MSG ("failed to create topic");
goto fail_pubtopic; goto fail_pubtopic;
} }
if ((subtopic = dds_create_topic(gcdds.ppant, &rmw_cyclonedds_topic_desc, subtopic_name.c_str(), nullptr, nullptr)) < 0) { if ((subtopic = dds_create_topic_arbitrary (gcdds.ppant, sub_st, subtopic_name.c_str (), nullptr, nullptr, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create topic"); RMW_SET_ERROR_MSG ("failed to create topic");
goto fail_subtopic; goto fail_subtopic;
} }
if ((qos = dds_qos_create()) == nullptr) { if ((qos = dds_create_qos ()) == nullptr) {
goto fail_qos; goto fail_qos;
} }
dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (1)); dds_qset_reliability (qos, DDS_RELIABILITY_RELIABLE, DDS_SECS (1));
@ -1174,8 +986,6 @@ static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_se
RMW_SET_ERROR_MSG ("failed to create writer"); RMW_SET_ERROR_MSG ("failed to create writer");
goto fail_writer; goto fail_writer;
} }
pub->sertopic = get_sertopic(pubtopic, pubtopic_name);
sub->node = node_impl;
if ((sub->subh = dds_create_reader (gcdds.ppant, subtopic, qos, nullptr)) < 0) { if ((sub->subh = dds_create_reader (gcdds.ppant, subtopic, qos, nullptr)) < 0) {
RMW_SET_ERROR_MSG ("failed to create reader"); RMW_SET_ERROR_MSG ("failed to create reader");
goto fail_reader; goto fail_reader;
@ -1188,8 +998,9 @@ static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_se
RMW_SET_ERROR_MSG ("failed to get instance handle for writer"); RMW_SET_ERROR_MSG ("failed to get instance handle for writer");
goto fail_instance_handle; goto fail_instance_handle;
} }
dds_qos_delete(qos); dds_delete_qos (qos);
node_impl->own_writers.insert(pub->pubiid); ddsi_sertopic_unref (pub_st);
ddsi_sertopic_unref (sub_st);
cs->pub = pub; cs->pub = pub;
cs->sub = sub; cs->sub = sub;
@ -1202,12 +1013,14 @@ static rmw_ret_t rmw_init_cs(CddsCS *cs, const rmw_node_t *node, const rosidl_se
fail_reader: fail_reader:
dds_delete (pub->pubh); dds_delete (pub->pubh);
fail_writer: fail_writer:
dds_qos_delete(qos); dds_delete_qos (qos);
fail_qos: fail_qos:
/* leak subtopic */ /* leak subtopic */
fail_subtopic: fail_subtopic:
/* leak pubtopic */ /* leak pubtopic */
fail_pubtopic: fail_pubtopic:
ddsi_sertopic_unref (pub_st);
ddsi_sertopic_unref (sub_st);
return RMW_RET_ERROR; return RMW_RET_ERROR;
} }
@ -1216,7 +1029,6 @@ static void rmw_fini_cs(CddsCS *cs)
dds_delete (cs->sub->rdcondh); dds_delete (cs->sub->rdcondh);
dds_delete (cs->sub->subh); dds_delete (cs->sub->subh);
dds_delete (cs->pub->pubh); dds_delete (cs->pub->pubh);
cs->sub->node->own_writers.erase(cs->pub->pubiid);
} }
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_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)
@ -1293,7 +1105,7 @@ extern "C" rmw_ret_t rmw_destroy_service(rmw_node_t *node, rmw_service_t *servic
/////////// /////////// /////////// ///////////
///////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////
extern "C" rmw_ret_t rmw_get_node_names(const rmw_node_t *node, rcutils_string_array_t *node_names) 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)
{ {
#if 0 // NIY #if 0 // NIY
RET_WRONG_IMPLID (node); RET_WRONG_IMPLID (node);
@ -1320,14 +1132,14 @@ extern "C" rmw_ret_t rmw_get_node_names(const rmw_node_t *node, rcutils_string_a
if (rcutils_ret != RCUTILS_RET_OK) { if (rcutils_ret != RCUTILS_RET_OK) {
RCUTILS_LOG_ERROR_NAMED ( RCUTILS_LOG_ERROR_NAMED (
"rmw_cyclonedds_cpp", "rmw_cyclonedds_cpp",
"failed to cleanup during error handling: %s", rcutils_get_error_string_safe()) "failed to cleanup during error handling: %s", rcutils_get_error_string_safe ());
} }
return RMW_RET_BAD_ALLOC; return RMW_RET_BAD_ALLOC;
} }
} }
return RMW_RET_OK; return RMW_RET_OK;
#else #else
(void)node; (void)node_names; (void) node; (void) node_names; (void) node_namespaces;
return RMW_RET_TIMEOUT; return RMW_RET_TIMEOUT;
#endif #endif
} }
@ -1390,7 +1202,7 @@ extern "C" rmw_ret_t rmw_get_topic_names_and_types(const rmw_node_t *node, rcuti
if (rmw_ret != RMW_RET_OK) { if (rmw_ret != RMW_RET_OK) {
RCUTILS_LOG_ERROR_NAMED ( RCUTILS_LOG_ERROR_NAMED (
"rmw_cyclonedds_cpp", "rmw_cyclonedds_cpp",
"error during report of error: %s", rmw_get_error_string_safe()) "error during report of error: %s", rmw_get_error_string_safe ());
} }
}; };
// Setup demangling functions based on no_demangle option // Setup demangling functions based on no_demangle option
@ -1527,7 +1339,7 @@ extern "C" rmw_ret_t rmw_get_service_names_and_types(const rmw_node_t *node, rcu
if (rmw_ret != RMW_RET_OK) { if (rmw_ret != RMW_RET_OK) {
RCUTILS_LOG_ERROR_NAMED ( RCUTILS_LOG_ERROR_NAMED (
"rmw_cyclonedds_cpp", "rmw_cyclonedds_cpp",
"error during report of error: %s", rmw_get_error_string_safe()) "error during report of error: %s", rmw_get_error_string_safe ());
} }
}; };
// For each service, store the name, initialize the string array for types, and store all types // For each service, store the name, initialize the string array for types, and store all types
@ -1703,7 +1515,7 @@ extern "C" rmw_ret_t rmw_count_publishers(const rmw_node_t *node, const char *to
RCUTILS_LOG_DEBUG_NAMED ( RCUTILS_LOG_DEBUG_NAMED (
"rmw_fastrtps_cpp", "rmw_fastrtps_cpp",
"looking for subscriber topic: %s, number of matches: %zu", "looking for subscriber topic: %s, number of matches: %zu",
topic_name, *count) topic_name, *count);
return RMW_RET_OK; return RMW_RET_OK;
#else #else
@ -1743,7 +1555,7 @@ extern "C" rmw_ret_t rmw_count_subscribers(const rmw_node_t *node, const char *t
RCUTILS_LOG_DEBUG_NAMED ( RCUTILS_LOG_DEBUG_NAMED (
"rmw_fastrtps_cpp", "rmw_fastrtps_cpp",
"looking for subscriber topic: %s, number of matches: %zu", "looking for subscriber topic: %s, number of matches: %zu",
topic_name, *count) topic_name, *count);
return RMW_RET_OK; return RMW_RET_OK;
#else #else

306
rmw_cyclonedds_cpp/src/serdata.cpp Normal file
View file

@ -0,0 +1,306 @@
// Copyright 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 <vector>
#include "rmw/error_handling.h"
#include "rmw_cyclonedds_cpp/MessageTypeSupport.hpp"
#include "rmw_cyclonedds_cpp/ServiceTypeSupport.hpp"
#include "rmw_cyclonedds_cpp/serdes.hpp"
#include "rmw_cyclonedds_cpp/serdata.hpp"
#include "dds/ddsi/ddsi_iid.h"
#include "dds/ddsi/q_radmin.h"
using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>;
using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>;
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)
{
return typesupport_identifier == rosidl_typesupport_introspection_c__identifier;
}
static bool using_introspection_cpp_typesupport (const char *typesupport_identifier)
{
return typesupport_identifier == rosidl_typesupport_introspection_cpp::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<const rosidl_typesupport_introspection_c__MessageMembers *> (untyped_members);
return new MessageTypeSupport_c (members);
} else if (using_introspection_cpp_typesupport (typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *> (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<const rosidl_typesupport_introspection_c__ServiceMembers *> (untyped_members);
return new RequestTypeSupport_c (members);
} else if (using_introspection_cpp_typesupport (typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *> (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<const rosidl_typesupport_introspection_c__ServiceMembers *> (untyped_members);
return new ResponseTypeSupport_c (members);
} else if (using_introspection_cpp_typesupport (typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *> (untyped_members);
return new ResponseTypeSupport_cpp (members);
}
RMW_SET_ERROR_MSG ("Unknown typesupport identifier");
return nullptr;
}
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
static uint32_t serdata_rmw_size (const struct ddsi_serdata *dcmn)
{
const struct serdata_rmw *d = static_cast<const struct serdata_rmw *> (dcmn);
return d->data.size ();
}
static void serdata_rmw_free (struct ddsi_serdata *dcmn)
{
const struct serdata_rmw *d = static_cast<const struct serdata_rmw *> (dcmn);
delete d;
}
static struct serdata_rmw *new_serdata_rmw (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 = new_serdata_rmw (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;
}
static struct ddsi_serdata *serdata_rmw_from_keyhash (const struct ddsi_sertopic *topic, const struct nn_keyhash *keyhash __attribute__ ((unused)))
{
/* there is no key field, so from_keyhash is trivial */
return new_serdata_rmw (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<const struct sertopic_rmw *> (topiccmn);
struct serdata_rmw *d = new_serdata_rmw (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<MessageTypeSupport_c *> (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<MessageTypeSupport_cpp *> (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<const cdds_request_wrapper_t *> (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<MessageTypeSupport_c *> (topic->type_support.type_support_);
(void) typed_typesupport->serializeROSmessage (wrap->data, sd, prefix);
} else if (using_introspection_cpp_typesupport (topic->type_support.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_cpp *> (topic->type_support.type_support_);
(void) typed_typesupport->serializeROSmessage (wrap->data, sd, prefix);
}
}
return d;
}
static struct ddsi_serdata *serdata_rmw_to_topicless (const struct ddsi_serdata *dcmn)
{
const struct serdata_rmw *d = static_cast<const struct serdata_rmw *> (dcmn);
struct serdata_rmw *d1 = new_serdata_rmw (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<const struct serdata_rmw *> (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<const struct serdata_rmw *> (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 __attribute__ ((unused)))
{
struct serdata_rmw *d = static_cast<struct serdata_rmw *> (dcmn);
ddsi_serdata_unref (d);
}
static bool serdata_rmw_to_sample (const struct ddsi_serdata *dcmn, void *sample, void **bufptr __attribute__ ((unused)), void *buflim __attribute__ ((unused)))
{
const struct serdata_rmw *d = static_cast<const struct serdata_rmw *> (dcmn);
const struct sertopic_rmw *topic = static_cast<const struct sertopic_rmw *> (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<const void *> (d->data.data ()), d->data.size ());
if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_c *> (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<MessageTypeSupport_cpp *> (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<cdds_request_wrapper_t *> (sample);
auto prefix = [wrap](cycdeser& ser) { ser >> wrap->header.guid; ser >> wrap->header.seq; };
cycdeser sd (static_cast<const void *> (d->data.data ()), d->data.size ());
if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) {
auto typed_typesupport = static_cast<MessageTypeSupport_c *> (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<MessageTypeSupport_cpp *> (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 __attribute__ ((unused)), const struct ddsi_serdata *dcmn __attribute__ ((unused)), void *sample __attribute__ ((unused)), void **bufptr __attribute__ ((unused)), void *buflim __attribute__ ((unused)))
{
/* ROS2 doesn't do keys in a meaningful way yet */
return true;
}
static bool serdata_rmw_eqkey (const struct ddsi_serdata *a __attribute__ ((unused)), const struct ddsi_serdata *b __attribute__ ((unused)))
{
/* ROS2 doesn't do keys in a meaningful way yet */
return true;
}
static const struct ddsi_serdata_ops serdata_rmw_ops = {
.eqkey = serdata_rmw_eqkey,
.get_size = serdata_rmw_size,
.from_ser = serdata_rmw_from_ser,
.from_keyhash = serdata_rmw_from_keyhash,
.from_sample = serdata_rmw_from_sample,
.to_ser = serdata_rmw_to_ser,
.to_ser_ref = serdata_rmw_to_ser_ref,
.to_ser_unref = serdata_rmw_to_ser_unref,
.to_sample = serdata_rmw_to_sample,
.to_topicless = serdata_rmw_to_topicless,
.topicless_to_sample = serdata_rmw_topicless_to_sample,
.free = serdata_rmw_free
};
static void sertopic_rmw_deinit (struct ddsi_sertopic *tpcmn)
{
struct sertopic_rmw *tp = static_cast<struct sertopic_rmw *> (tpcmn);
delete tp;
}
static void sertopic_rmw_zero_samples (const struct ddsi_sertopic *d __attribute__ ((unused)), void *samples __attribute__ ((unused)), size_t count __attribute__ ((unused)))
{
/* Not using code paths that rely on the samples getting zero'd out */
}
static void sertopic_rmw_realloc_samples (void **ptrs __attribute__ ((unused)), const struct ddsi_sertopic *d __attribute__ ((unused)), void *old __attribute__ ((unused)), size_t oldcount __attribute__ ((unused)), size_t count __attribute__ ((unused)))
{
/* 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 __attribute__ ((unused)), void **ptrs __attribute__ ((unused)), size_t count __attribute__ ((unused)), dds_free_op_t op)
{
/* Not using code paths that rely on this (dispose, unregister with instance handle, content
filters) */
assert (!(op & DDS_FREE_ALL_BIT));
}
static const struct ddsi_sertopic_ops sertopic_rmw_ops = {
.deinit = sertopic_rmw_deinit,
.zero_samples = sertopic_rmw_zero_samples,
.realloc_samples = sertopic_rmw_realloc_samples,
.free_samples = sertopic_rmw_free_samples
};
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;
st->cpp_name = std::string (topicname);
st->cpp_type_name = std::string ("absent"); // FIXME: obviously a hack
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<char *> (st->cpp_name_type_name.c_str ());
st->name = const_cast<char *> (st->cpp_name.c_str ());
st->type_name = const_cast<char *> (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;
}
#pragma GCC diagnostic pop

59
rmw_cyclonedds_cpp/src/serdes.cpp
View file

@ -1,53 +1,22 @@
#include "rmw_cyclonedds_cpp/serdes.hpp" #include "rmw_cyclonedds_cpp/serdes.hpp"
cycser::cycser(struct sertopic *topic) cycser::cycser (std::vector<unsigned char>& dst_)
: dst (dst_)
, off (0)
{ {
st = ddsi_serstate_new(topic); dst.reserve (4);
sd = nullptr; unsigned char *magic = dst.data ();
} /* FIXME: hard code to little endian ... and ignoring endianness in deser */
magic[0] = 0;
cycser::~cycser() magic[1] = 3;
{ /* options: */
if (sd == nullptr) { magic[2] = 0;
ddsi_serstate_release(st); magic[3] = 0;
} else {
ddsi_serdata_unref(sd);
}
}
cycser& cycser::ref()
{
assert(sd != nullptr);
ddsi_serdata_ref(sd);
return *this;
}
void cycser::unref()
{
assert(sd != nullptr);
ddsi_serdata_unref(sd);
}
cycser& cycser::fix()
{
assert(sd == nullptr);
sd = ddsi_serstate_fix(st);
return *this;
}
struct serdata *cycser::data()
{
assert(sd != nullptr);
return sd;
} }
cycdeser::cycdeser (const void *data_, size_t size_) cycdeser::cycdeser (const void *data_, size_t size_)
{ : data (static_cast<const char *> (data_) + 4)
data = static_cast<const char *>(data_); , pos (0)
lim = size_; , lim (size_ - 4)
pos = 0;
}
cycdeser::~cycdeser()
{ {
} }