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

Uncrustify and fix issues reported by cpplint

Browse source 
Signed-off-by: Erik Boasson <eb@ilities.com>
This commit is contained in:
Erik Boasson 2019-08-17 15:18:57 +02:00 • committed by eboasson
parent 130d6ea10e
commit 5e137205f2
7 changed files with 2664 additions and 2204 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -111,21 +111,27 @@ template<typename MembersType>
class TypeSupport class TypeSupport
{ {
public: public:
bool serializeROSmessage(const void * ros_message, cycser & ser, std::function<void(cycser&)> prefix = nullptr); bool serializeROSmessage(
bool deserializeROSmessage(cycdeser & deser, void * ros_message, std::function<void(cycdeser&)> prefix = nullptr); const void * ros_message, cycser & ser,
std::string getName (); std::function<void(cycser &)> prefix = nullptr);
bool deserializeROSmessage(
cycdeser & deser, void * ros_message,
std::function<void(cycdeser &)> prefix = nullptr);
std::string getName();
protected: protected:
TypeSupport(); TypeSupport();
void setName(const std::string& name); void setName(const std::string & name);
const MembersType * members_; const MembersType * members_;
std::string name; std::string name;
private: private:
bool serializeROSmessage(cycser & ser, const MembersType * members, const void * ros_message); 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 } // namespace rmw_cyclonedds_cpp

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

@ -109,13 +109,13 @@ rosidl_generator_c__void__Sequence__fini(rosidl_generator_c__void__Sequence * se
template<typename MembersType> template<typename MembersType>
TypeSupport<MembersType>::TypeSupport() TypeSupport<MembersType>::TypeSupport()
{ {
name = ""; name = "";
} }
template<typename MembersType> template<typename MembersType>
void TypeSupport<MembersType>::setName(const std::string& name) void TypeSupport<MembersType>::setName(const std::string & name)
{ {
this->name = std::string(name); this->name = std::string(name);
} }
static inline void * static inline void *
@ -502,7 +502,9 @@ inline void deserialize_field<std::string>(
deser >> cpp_string_vector; deser >> cpp_string_vector;
auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Sequence *>(field); auto & string_array_field = *reinterpret_cast<rosidl_generator_c__String__Sequence *>(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"); throw std::runtime_error("unable to initialize rosidl_generator_c__String array");
} }
@ -649,7 +651,7 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
template<typename MembersType> template<typename MembersType>
bool TypeSupport<MembersType>::serializeROSmessage( bool TypeSupport<MembersType>::serializeROSmessage(
const void * ros_message, cycser & ser, const void * ros_message, cycser & ser,
std::function<void(cycser&)> prefix) std::function<void(cycser &)> prefix)
{ {
assert(ros_message); assert(ros_message);
@ -669,7 +671,7 @@ bool TypeSupport<MembersType>::serializeROSmessage(
template<typename MembersType> template<typename MembersType>
bool TypeSupport<MembersType>::deserializeROSmessage( bool TypeSupport<MembersType>::deserializeROSmessage(
cycdeser & deser, void * ros_message, cycdeser & deser, void * ros_message,
std::function<void(cycdeser&)> prefix) std::function<void(cycdeser &)> prefix)
{ {
assert(ros_message); assert(ros_message);
@ -689,7 +691,7 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
} }
template<typename MembersType> template<typename MembersType>
std::string TypeSupport<MembersType>::getName () std::string TypeSupport<MembersType>::getName()
{ {
return name; return name;
} }

72
rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdata.hpp
View file

@ -11,47 +11,65 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#ifndef SERDATA_HPP #ifndef RMW_CYCLONEDDS_CPP__SERDATA_HPP_
#define SERDATA_HPP #define RMW_CYCLONEDDS_CPP__SERDATA_HPP_
#include <string>
#include <vector>
#include "dds/ddsi/ddsi_sertopic.h" #include "dds/ddsi/ddsi_sertopic.h"
#include "dds/ddsi/ddsi_serdata.h" #include "dds/ddsi/ddsi_serdata.h"
struct CddsTypeSupport { struct CddsTypeSupport
void *type_support_; {
const char *typesupport_identifier_; void * type_support_;
const char * typesupport_identifier_;
}; };
struct sertopic_rmw : ddsi_sertopic { struct sertopic_rmw : ddsi_sertopic
CddsTypeSupport type_support; {
bool is_request_header; CddsTypeSupport type_support;
std::string cpp_name; bool is_request_header;
std::string cpp_type_name; std::string cpp_name;
std::string cpp_name_type_name; std::string cpp_type_name;
std::string cpp_name_type_name;
}; };
struct serdata_rmw : ddsi_serdata { struct serdata_rmw : ddsi_serdata
/* first two bytes of data is CDR encoding {
second two bytes are encoding options */ /* first two bytes of data is CDR encoding
std::vector<unsigned char> data; second two bytes are encoding options */
std::vector<unsigned char> data;
}; };
typedef struct cdds_request_header { typedef struct cdds_request_header
uint64_t guid; {
int64_t seq; uint64_t guid;
int64_t seq;
} cdds_request_header_t; } cdds_request_header_t;
typedef struct cdds_request_wrapper { typedef struct cdds_request_wrapper
cdds_request_header_t header; {
void *data; cdds_request_header_t header;
void * data;
} cdds_request_wrapper_t; } cdds_request_wrapper_t;
void *create_message_type_support (const void *untyped_members, const char *typesupport_identifier); void * create_message_type_support(
void *create_request_type_support (const void *untyped_members, const char *typesupport_identifier); const void * untyped_members,
void *create_response_type_support (const void *untyped_members, const char *typesupport_identifier); 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_

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

@ -1,4 +1,4 @@
// Copyright 2018 ADLINK Technology // Copyright 2018 to 2019 ADLINK Technology
// //
// Licensed under the Apache License, Version 2.0 (the "License"); // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with 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 // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#ifndef SERDES_HPP #ifndef RMW_CYCLONEDDS_CPP__SERDES_HPP_
#define SERDES_HPP #define RMW_CYCLONEDDS_CPP__SERDES_HPP_
#include <string.h>
#include <array> #include <array>
#include <string> #include <string>
#include <vector> #include <vector>
#include <string.h>
#include <type_traits> #include <type_traits>
class cycser { class cycser
{
public: public:
cycser (std::vector<unsigned char>& dst_); explicit cycser(std::vector<unsigned char> & dst_);
cycser () = delete; cycser() = delete;
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;}
inline cycser& operator<< (int8_t 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<<(uint8_t x) {serialize(x); return *this;}
inline cycser& operator<< (int16_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<<(uint16_t x) {serialize(x); return *this;}
inline cycser& operator<< (int32_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<<(uint32_t x) {serialize(x); return *this;}
inline cycser& operator<< (int64_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<<(uint64_t x) {serialize(x); return *this;}
inline cycser& operator<< (float 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<<(double x) {serialize(x); return *this;}
inline cycser& operator<< (const char *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; } inline cycser & operator<<(const std::string & x) {serialize(x); return *this;}
template<class T> inline cycser& operator<< (const std::vector<T>& x) { serialize (x); return *this; } template<class T>
template<class T, size_t S> inline cycser& operator<< (const std::array<T, S>& x) { serialize (x); return *this; } inline cycser & operator<<(const std::vector<T> & x) {serialize(x); return *this;}
template<class T, size_t S>
inline cycser & operator<<(const std::array<T, S> & x) {serialize(x); return *this;}
#define SIMPLE(T) inline void serialize (T x) { \ #define SIMPLE(T) inline void serialize(T x) { \
if ((off % sizeof (T)) != 0) { \ if ((off % sizeof(T)) != 0) { \
off += sizeof (T) - (off % sizeof (T)); \ off += sizeof(T) - (off % sizeof(T)); \
} \ } \
resize (off + sizeof (T)); \ resize(off + sizeof(T)); \
*(T *) (data () + off) = x; \ *(reinterpret_cast<T *>(data() + off)) = x; \
off += sizeof (T); \ off += sizeof(T); \
} }
SIMPLE (char); SIMPLE(char);
SIMPLE (int8_t); SIMPLE(int8_t);
SIMPLE (uint8_t); SIMPLE(uint8_t);
SIMPLE (int16_t); SIMPLE(int16_t);
SIMPLE (uint16_t); SIMPLE(uint16_t);
SIMPLE (int32_t); SIMPLE(int32_t);
SIMPLE (uint32_t); SIMPLE(uint32_t);
SIMPLE (int64_t); SIMPLE(int64_t);
SIMPLE (uint64_t); SIMPLE(uint64_t);
SIMPLE (float); SIMPLE(float);
SIMPLE (double); SIMPLE(double);
#undef SIMPLE #undef SIMPLE
inline void serialize (bool x) { inline void serialize(bool x)
serialize (static_cast<unsigned char> (x)); {
} serialize(static_cast<unsigned char>(x));
inline void serialize (const char *x) }
{ inline void serialize(const char * x)
size_t sz = strlen (x) + 1; {
serialize (static_cast<uint32_t> (sz)); size_t sz = strlen(x) + 1;
resize (off + sz); serialize(static_cast<uint32_t>(sz));
memcpy (data () + off, x, sz); resize(off + sz);
off += 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 () + 1; {
serialize (static_cast<uint32_t> (sz)); size_t sz = x.size() + 1;
resize (off + sz); serialize(static_cast<uint32_t>(sz));
memcpy (data () + off, x.c_str (), sz); resize(off + sz);
off += sz; memcpy(data() + off, x.c_str(), sz);
} off += sz;
}
#define SIMPLEA(T) inline void serializeA (const T *x, size_t cnt) { \ #define SIMPLEA(T) inline void serializeA(const T * x, size_t cnt) { \
if (cnt > 0) { \ if (cnt > 0) { \
if ((off % sizeof (T)) != 0) { \ if ((off % sizeof(T)) != 0) { \
off += sizeof (T) - (off % sizeof (T)); \ off += sizeof(T) - (off % sizeof(T)); \
} \ } \
resize (off + cnt * sizeof (T)); \ resize(off + cnt * sizeof(T)); \
memcpy (data () + off, (void *) x, cnt * sizeof (T)); \ memcpy(data() + off, reinterpret_cast<const void *>(x), cnt * sizeof(T)); \
off += cnt * sizeof (T); \ off += cnt * sizeof(T); \
} \ } \
} }
SIMPLEA (char); SIMPLEA(char);
SIMPLEA (int8_t); SIMPLEA(int8_t);
SIMPLEA (uint8_t); SIMPLEA(uint8_t);
SIMPLEA (int16_t); SIMPLEA(int16_t);
SIMPLEA (uint16_t); SIMPLEA(uint16_t);
SIMPLEA (int32_t); SIMPLEA(int32_t);
SIMPLEA (uint32_t); SIMPLEA(uint32_t);
SIMPLEA (int64_t); SIMPLEA(int64_t);
SIMPLEA (uint64_t); SIMPLEA(uint64_t);
SIMPLEA (float); SIMPLEA(float);
SIMPLEA (double); SIMPLEA(double);
#undef SIMPLEA #undef SIMPLEA
template<class T> inline void serializeA (const T *x, size_t cnt) { template<class T>
for (size_t i = 0; i < cnt; i++) serialize (x[i]); inline void serializeA(const T * x, size_t cnt)
} {
for (size_t i = 0; i < cnt; i++) {serialize(x[i]);}
}
template<class T> inline void serialize (const std::vector<T>& x) template<class T>
{ inline void serialize(const std::vector<T> & x)
serialize (static_cast<uint32_t> (x.size ())); {
serializeA (x.data (), x.size ()); serialize(static_cast<uint32_t>(x.size()));
} serializeA(x.data(), x.size());
inline void serialize (const std::vector<bool>& x) { }
serialize (static_cast<uint32_t> (x.size ())); inline void serialize(const std::vector<bool> & x)
for (auto&& i : x) serialize (i); {
} serialize(static_cast<uint32_t>(x.size()));
for (auto && i : x) {serialize(i);}
}
template<class T> inline void serializeS (const T *x, size_t cnt) template<class T>
{ inline void serializeS(const T * x, size_t cnt)
serialize (static_cast<uint32_t> (cnt)); {
serializeA (x, cnt); serialize(static_cast<uint32_t>(cnt));
} serializeA(x, cnt);
}
private: private:
inline void resize (size_t n) { dst.resize (n + 4); } inline void resize(size_t n) {dst.resize(n + 4);}
inline unsigned char *data () { return dst.data () + 4; } inline unsigned char * data() {return dst.data() + 4;}
std::vector<unsigned char>& dst; std::vector<unsigned char> & dst;
size_t off; size_t off;
}; };
class cycdeser { class cycdeser
{
public: public:
// FIXME: byteswapping // FIXME: byteswapping
cycdeser (const void *data, size_t size); cycdeser(const void * data, size_t size);
cycdeser () = delete; cycdeser() = delete;
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;}
inline cycdeser& operator>> (int8_t& 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>>(uint8_t & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (int16_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>>(uint16_t & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (int32_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>>(uint32_t & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (int64_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>>(uint64_t & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (float& 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>>(double & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (char *& x) { deserialize (x); return *this; } inline cycdeser & operator>>(char * & x) {deserialize(x); return *this;}
inline cycdeser& operator>> (std::string& x) { deserialize (x); return *this; } inline cycdeser & operator>>(std::string & x) {deserialize(x); return *this;}
template<class T> inline cycdeser& operator>> (std::vector<T>& x) { deserialize (x); return *this; } template<class T>
template<class T, size_t S> inline cycdeser& operator>> (std::array<T, S>& x) { deserialize (x); return *this; } inline cycdeser & operator>>(std::vector<T> & x) {deserialize(x); return *this;}
template<class T, size_t S>
inline cycdeser & operator>>(std::array<T, S> & x) {deserialize(x); return *this;}
#define SIMPLE(T) inline void deserialize (T& x) { \ #define SIMPLE(T) inline void deserialize(T & x) { \
align (sizeof (x)); \ align(sizeof(x)); \
x = *reinterpret_cast<const T *> (data + pos); \ x = *reinterpret_cast<const T *>(data + pos); \
pos += sizeof (x); \ pos += sizeof(x); \
} }
SIMPLE (char); SIMPLE(char);
SIMPLE (int8_t); SIMPLE(int8_t);
SIMPLE (uint8_t); SIMPLE(uint8_t);
SIMPLE (int16_t); SIMPLE(int16_t);
SIMPLE (uint16_t); SIMPLE(uint16_t);
SIMPLE (int32_t); SIMPLE(int32_t);
SIMPLE (uint32_t); SIMPLE(uint32_t);
SIMPLE (int64_t); SIMPLE(int64_t);
SIMPLE (uint64_t); SIMPLE(uint64_t);
SIMPLE (float); SIMPLE(float);
SIMPLE (double); SIMPLE(double);
#undef SIMPLE #undef SIMPLE
inline void deserialize (bool& x) { inline void deserialize(bool & x)
unsigned char z; deserialize (z); x = (z != 0); {
} unsigned char z; deserialize(z); x = (z != 0);
inline uint32_t deserialize32 () { }
uint32_t sz; deserialize (sz); return sz; inline uint32_t deserialize32()
} {
inline void deserialize (char *& x) { uint32_t sz; deserialize(sz); return sz;
const uint32_t sz = deserialize32 (); }
x = (char *) malloc (sz); inline void deserialize(char * & x)
memcpy (x, data + pos, sz); {
pos += sz; const uint32_t sz = deserialize32();
} x = reinterpret_cast<char *>(malloc(sz));
inline void deserialize (std::string& x) { memcpy(x, data + pos, sz);
const uint32_t sz = deserialize32 (); pos += sz;
x = std::string (data + pos, sz-1); }
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) { \ #define SIMPLEA(T) inline void deserializeA(T * x, size_t cnt) { \
if (cnt > 0) { \ if (cnt > 0) { \
align (sizeof (T)); \ align(sizeof(T)); \
memcpy ((void *) x, (void *) (data + pos), (cnt) * sizeof (T)); \ memcpy(reinterpret_cast<void *>(x), reinterpret_cast<const void *>(data + pos), \
pos += (cnt) * sizeof (T); \ (cnt) * sizeof(T)); \
} \ pos += (cnt) * sizeof(T); \
} } \
SIMPLEA (char); }
SIMPLEA (int8_t); SIMPLEA(char);
SIMPLEA (uint8_t); SIMPLEA(int8_t);
SIMPLEA (int16_t); SIMPLEA(uint8_t);
SIMPLEA (uint16_t); SIMPLEA(int16_t);
SIMPLEA (int32_t); SIMPLEA(uint16_t);
SIMPLEA (uint32_t); SIMPLEA(int32_t);
SIMPLEA (int64_t); SIMPLEA(uint32_t);
SIMPLEA (uint64_t); SIMPLEA(int64_t);
SIMPLEA (float); SIMPLEA(uint64_t);
SIMPLEA (double); SIMPLEA(float);
SIMPLEA(double);
#undef SIMPLEA #undef SIMPLEA
template<class T> inline void deserializeA (T *x, size_t cnt) { template<class T>
for (size_t i = 0; i < cnt; i++) deserialize (x[i]); inline void deserializeA(T * x, size_t cnt)
} {
for (size_t i = 0; i < cnt; i++) {deserialize(x[i]);}
}
template<class T> inline void deserialize (std::vector<T>& x) { template<class T>
const uint32_t sz = deserialize32 (); inline void deserialize(std::vector<T> & x)
x.resize (sz); {
deserializeA (x.data (), sz); const uint32_t sz = deserialize32();
} x.resize(sz);
inline void deserialize (std::vector<bool>& x) { deserializeA(x.data(), sz);
const uint32_t sz = deserialize32 (); }
x.resize (sz); inline void deserialize(std::vector<bool> & x)
for (size_t i = 0; i < sz; i++) { {
x[i] = ((data + pos)[i] != 0); const uint32_t sz = deserialize32();
} x.resize(sz);
pos += sz; for (size_t i = 0; i < sz; i++) {
} x[i] = ((data + pos)[i] != 0);
template<class T, size_t S> inline void deserialize (std::array<T, S>& x) {
deserializeA (x.data (), x.size ());
} }
pos += sz;
}
template<class T, size_t S>
inline void deserialize(std::array<T, S> & x)
{
deserializeA(x.data(), x.size());
}
private: private:
inline void align (size_t a) inline void align(size_t a)
{ {
if ((pos % a) != 0) { if ((pos % a) != 0) {
pos += a - (pos % a); pos += a - (pos % a);
}
} }
}
const char *data; const char * data;
size_t pos; size_t pos;
size_t lim; // ignored for now ... better provide correct input size_t lim; // ignored for now ... better provide correct input
}; };
#endif #endif // RMW_CYCLONEDDS_CPP__SERDES_HPP_

3686
rmw_cyclonedds_cpp/src/rmw_node.cpp
View file

File diff suppressed because it is too large Load diff

612
rmw_cyclonedds_cpp/src/serdata.cpp
View file

@ -11,9 +11,12 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <string.h>
#include <vector> #include <vector>
#include <regex> #include <regex>
#include <sstream> #include <sstream>
#include <string>
#include "rmw/error_handling.h" #include "rmw/error_handling.h"
@ -26,356 +29,413 @@
#include "dds/ddsi/ddsi_iid.h" #include "dds/ddsi/ddsi_iid.h"
#include "dds/ddsi/q_radmin.h" #include "dds/ddsi/q_radmin.h"
using MessageTypeSupport_c = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>; using MessageTypeSupport_c =
using MessageTypeSupport_cpp = rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>; rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_c__MessageMembers>;
using RequestTypeSupport_c = rmw_cyclonedds_cpp::RequestTypeSupport<rosidl_typesupport_introspection_c__ServiceMembers, rosidl_typesupport_introspection_c__MessageMembers>; using MessageTypeSupport_cpp =
using RequestTypeSupport_cpp = rmw_cyclonedds_cpp::RequestTypeSupport<rosidl_typesupport_introspection_cpp::ServiceMembers, rosidl_typesupport_introspection_cpp::MessageMembers>; rmw_cyclonedds_cpp::MessageTypeSupport<rosidl_typesupport_introspection_cpp::MessageMembers>;
using ResponseTypeSupport_c = rmw_cyclonedds_cpp::ResponseTypeSupport<rosidl_typesupport_introspection_c__ServiceMembers, rosidl_typesupport_introspection_c__MessageMembers>; using RequestTypeSupport_c = rmw_cyclonedds_cpp::RequestTypeSupport<
using ResponseTypeSupport_cpp = rmw_cyclonedds_cpp::ResponseTypeSupport<rosidl_typesupport_introspection_cpp::ServiceMembers, rosidl_typesupport_introspection_cpp::MessageMembers>; 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)) { if (using_introspection_c_typesupport(typesupport_identifier)) {
auto members = static_cast<const rosidl_typesupport_introspection_c__MessageMembers *> (untyped_members); auto members =
return new MessageTypeSupport_c (members); static_cast<const rosidl_typesupport_introspection_c__MessageMembers *>(untyped_members);
} else if (using_introspection_cpp_typesupport (typesupport_identifier)) { return new MessageTypeSupport_c(members);
auto members = static_cast<const rosidl_typesupport_introspection_cpp::MessageMembers *> (untyped_members); } else if (using_introspection_cpp_typesupport(typesupport_identifier)) {
return new MessageTypeSupport_cpp (members); 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;
}
static uint32_t serdata_rmw_size(const struct ddsi_serdata * dcmn)
{
const struct serdata_rmw * d = static_cast<const struct serdata_rmw *>(dcmn);
return static_cast<uint32_t>(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 * 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"); fragchain = fragchain->nextfrag;
return nullptr; }
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)) { static_cast<void>(keyhash); // unused
auto members = static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *> (untyped_members); /* there is no key field, so from_keyhash is trivial */
return new RequestTypeSupport_c (members); return rmw_serdata_new(topic, SDK_KEY);
} 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); 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 = 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<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);
} }
RMW_SET_ERROR_MSG ("Unknown typesupport identifier"); } else {
return nullptr; /* 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. */
void *create_response_type_support (const void *untyped_members, const char *typesupport_identifier) 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;};
if (using_introspection_c_typesupport (typesupport_identifier)) { cycser sd(d->data);
auto members = static_cast<const rosidl_typesupport_introspection_c__ServiceMembers *> (untyped_members); if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) {
return new ResponseTypeSupport_c (members); auto typed_typesupport =
} else if (using_introspection_cpp_typesupport (typesupport_identifier)) { static_cast<MessageTypeSupport_c *>(topic->type_support.type_support_);
auto members = static_cast<const rosidl_typesupport_introspection_cpp::ServiceMembers *> (untyped_members); (void) typed_typesupport->serializeROSmessage(wrap->data, sd, prefix);
return new ResponseTypeSupport_cpp (members); } 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);
} }
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<const struct serdata_rmw *> (dcmn); const struct sertopic_rmw * topic = static_cast<const struct sertopic_rmw *>(topiccmn);
return static_cast<uint32_t>(d->data.size()); 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<const struct serdata_rmw *> (dcmn); const struct serdata_rmw * d = static_cast<const struct serdata_rmw *>(dcmn);
delete d; 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; const struct serdata_rmw * d = static_cast<const struct serdata_rmw *>(dcmn);
ddsi_serdata_init (sd, topic, kind); memcpy(buf, reinterpret_cast<const char *>(d->data.data()) + off, sz);
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) 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); const struct serdata_rmw * d = static_cast<const struct serdata_rmw *>(dcmn);
uint32_t off = 0; ref->iov_base = const_cast<char *>(reinterpret_cast<const char *>(d->data.data())) + off;
assert (fragchain->min == 0); ref->iov_len = (ddsrt_iov_len_t) sz;
assert (fragchain->maxp1 >= off); /* CDR header must be in first fragment */ return ddsi_serdata_ref(d);
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) static void serdata_rmw_to_ser_unref(struct ddsi_serdata * dcmn, const ddsrt_iovec_t * ref)
{ {
static_cast<void>(keyhash); // unused static_cast<void>(ref); // unused
/* there is no key field, so from_keyhash is trivial */ struct serdata_rmw * d = static_cast<struct serdata_rmw *>(dcmn);
return rmw_serdata_new (topic, SDK_KEY); 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<const struct sertopic_rmw *> (topiccmn); static_cast<void>(bufptr); // unused
struct serdata_rmw *d = rmw_serdata_new (topic, kind); static_cast<void>(buflim); // unused
if (kind != SDK_DATA) { const struct serdata_rmw * d = static_cast<const struct serdata_rmw *>(dcmn);
/* ROS2 doesn't do keys, so SDK_KEY is trivial */ const struct sertopic_rmw * topic = static_cast<const struct sertopic_rmw *>(d->topic);
} else if (!topic->is_request_header) { assert(bufptr == NULL);
cycser sd (d->data); assert(buflim == NULL);
if (using_introspection_c_typesupport (topic->type_support.typesupport_identifier_)) { if (d->kind != SDK_DATA) {
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);
}
}
/* 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<const struct sertopic_rmw *> (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<const struct serdata_rmw *> (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<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)
{
static_cast<void>(ref); // 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, void *buflim)
{
static_cast<void>(bufptr); // unused
static_cast<void>(buflim); // 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, const struct ddsi_serdata *dcmn, void *sample, void **bufptr, void *buflim)
{
static_cast<void>(topic);
static_cast<void>(dcmn);
static_cast<void>(sample);
static_cast<void>(bufptr);
static_cast<void>(buflim);
/* ROS2 doesn't do keys in a meaningful way yet */ /* ROS2 doesn't do keys in a meaningful way yet */
return true; } 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_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<void>(a); static_cast<void>(topic);
static_cast<void>(b); static_cast<void>(dcmn);
/* ROS2 doesn't do keys in a meaningful way yet */ static_cast<void>(sample);
return true; static_cast<void>(bufptr);
static_cast<void>(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<void>(a);
static_cast<void>(b);
/* ROS2 doesn't do keys in a meaningful way yet */
return true;
} }
static const struct ddsi_serdata_ops serdata_rmw_ops = { static const struct ddsi_serdata_ops serdata_rmw_ops = {
serdata_rmw_eqkey, serdata_rmw_eqkey,
serdata_rmw_size, serdata_rmw_size,
serdata_rmw_from_ser, serdata_rmw_from_ser,
serdata_rmw_from_keyhash, serdata_rmw_from_keyhash,
serdata_rmw_from_sample, serdata_rmw_from_sample,
serdata_rmw_to_ser, serdata_rmw_to_ser,
serdata_rmw_to_ser_ref, serdata_rmw_to_ser_ref,
serdata_rmw_to_ser_unref, serdata_rmw_to_ser_unref,
serdata_rmw_to_sample, serdata_rmw_to_sample,
serdata_rmw_to_topicless, serdata_rmw_to_topicless,
serdata_rmw_topicless_to_sample, serdata_rmw_topicless_to_sample,
serdata_rmw_free 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<struct sertopic_rmw *> (tpcmn); struct sertopic_rmw * tp = static_cast<struct sertopic_rmw *>(tpcmn);
delete tp; 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<void>(d); static_cast<void>(d);
static_cast<void>(samples); static_cast<void>(samples);
static_cast<void>(count); static_cast<void>(count);
/* Not using code paths that rely on the samples getting zero'd out */ /* 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<void>(ptrs); static_cast<void>(ptrs);
static_cast<void>(d); static_cast<void>(d);
static_cast<void>(old); static_cast<void>(old);
static_cast<void>(oldcount); static_cast<void>(oldcount);
static_cast<void>(count); static_cast<void>(count);
/* Not using code paths that rely on this (loans, dispose, unregister with instance handle, /* Not using code paths that rely on this (loans, dispose, unregister with instance handle,
content filters) */ content filters) */
abort (); 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<void>(d); // unused static_cast<void>(d); // unused
static_cast<void>(ptrs); // unused static_cast<void>(ptrs); // unused
static_cast<void>(count); // unused static_cast<void>(count); // unused
/* Not using code paths that rely on this (dispose, unregister with instance handle, content /* Not using code paths that rely on this (dispose, unregister with instance handle, content
filters) */ filters) */
assert (!(op & DDS_FREE_ALL_BIT)); assert(!(op & DDS_FREE_ALL_BIT));
(void) op; (void) op;
} }
static const struct ddsi_sertopic_ops sertopic_rmw_ops = { static const struct ddsi_sertopic_ops sertopic_rmw_ops = {
sertopic_rmw_free, sertopic_rmw_free,
sertopic_rmw_zero_samples, sertopic_rmw_zero_samples,
sertopic_rmw_realloc_samples, sertopic_rmw_realloc_samples,
sertopic_rmw_free_samples sertopic_rmw_free_samples
}; };
template<typename MembersType> template<typename MembersType>
ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL 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<const MembersType *> (untyped_members); auto members = static_cast<const MembersType *>(untyped_members);
if (!members) { if (!members) {
RMW_SET_ERROR_MSG("members handle is null"); RMW_SET_ERROR_MSG("members handle is null");
return ""; return "";
} }
std::ostringstream ss; 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 // Find and replace C namespace separator with C++, in case this is using C typesupport
message_namespace = std::regex_replace (message_namespace, std::regex("__"), "::"); message_namespace = std::regex_replace(message_namespace, std::regex("__"), "::");
std::string message_name (members->message_name_); std::string message_name(members->message_name_);
if (!message_namespace.empty ()) { if (!message_namespace.empty()) {
ss << message_namespace << "::"; ss << message_namespace << "::";
} }
ss << "dds_::" << message_name << "_"; 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)) { if (using_introspection_c_typesupport(type_support_identifier)) {
auto typed_typesupport = static_cast<MessageTypeSupport_c *> (type_support); auto typed_typesupport = static_cast<MessageTypeSupport_c *>(type_support);
return typed_typesupport->getName (); return typed_typesupport->getName();
} else if (using_introspection_cpp_typesupport (type_support_identifier)) { } else if (using_introspection_cpp_typesupport(type_support_identifier)) {
auto typed_typesupport = static_cast<MessageTypeSupport_cpp *> (type_support); auto typed_typesupport = static_cast<MessageTypeSupport_cpp *>(type_support);
return typed_typesupport->getName (); return typed_typesupport->getName();
} else { } else {
return "absent"; 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; struct sertopic_rmw * st = new struct sertopic_rmw;
memset (st, 0, sizeof (struct ddsi_sertopic)); memset(st, 0, sizeof(struct ddsi_sertopic));
st->cpp_name = std::string (topicname); st->cpp_name = std::string(topicname);
st->cpp_type_name = get_type_name (type_support_identifier, type_support); 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->cpp_name_type_name = st->cpp_name + std::string(";") + std::string(st->cpp_type_name);
st->ops = &sertopic_rmw_ops; st->ops = &sertopic_rmw_ops;
st->serdata_ops = &serdata_rmw_ops; st->serdata_ops = &serdata_rmw_ops;
st->serdata_basehash = ddsi_sertopic_compute_serdata_basehash (st->serdata_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_type_name = const_cast<char *>(st->cpp_name_type_name.c_str());
st->name = const_cast<char *> (st->cpp_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->type_name = const_cast<char *>(st->cpp_type_name.c_str());
st->iid = ddsi_iid_gen (); st->iid = ddsi_iid_gen();
ddsrt_atomic_st32 (&st->refc, 1); ddsrt_atomic_st32(&st->refc, 1);
st->type_support.typesupport_identifier_ = type_support_identifier; st->type_support.typesupport_identifier_ = type_support_identifier;
st->type_support.type_support_ = type_support; st->type_support.type_support_ = type_support;
st->is_request_header = is_request_header; st->is_request_header = is_request_header;
return st; return st;
} }

39
rmw_cyclonedds_cpp/src/serdes.cpp
View file

@ -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 <vector>
#include "rmw_cyclonedds_cpp/serdes.hpp" #include "rmw_cyclonedds_cpp/serdes.hpp"
cycser::cycser (std::vector<unsigned char>& dst_) cycser::cycser(std::vector<unsigned char> & dst_)
: dst (dst_) : dst(dst_),
, off (0) off(0)
{ {
dst.reserve (4); dst.reserve(4);
/* FIXME: hard code to little endian ... and ignoring endianness in deser */ /* FIXME: hard code to little endian ... and ignoring endianness in deser */
dst.push_back (0); dst.push_back(0);
dst.push_back (3); dst.push_back(3);
/* options: */ /* 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_) cycdeser::cycdeser(const void * data_, size_t size_)
: data (static_cast<const char *> (data_) + 4) : data(static_cast<const char *>(data_) + 4),
, pos (0) pos(0),
, lim (size_ - 4) lim(size_ - 4)
{ {
} }