From fb8c08bd7ff1452c09ff0b9922c1e991afe0697b Mon Sep 17 00:00:00 2001 From: Erik Boasson Date: Sun, 1 Sep 2019 15:32:04 +0200 Subject: [PATCH] Add support for printing messages to DDSI trace NOTE: not tested against versions of Cyclone that don't have the feature Signed-off-by: Erik Boasson --- .../rmw_cyclonedds_cpp/TypeSupport.hpp | 5 + .../rmw_cyclonedds_cpp/TypeSupport_impl.hpp | 126 ++++++++++++++ .../include/rmw_cyclonedds_cpp/serdes.hpp | 161 +++++++++++++++++- rmw_cyclonedds_cpp/src/serdata.cpp | 54 +++++- rmw_cyclonedds_cpp/src/serdes.cpp | 9 + 5 files changed, 348 insertions(+), 7 deletions(-) diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp index cf2b92b..2e817e4 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport.hpp @@ -118,6 +118,9 @@ public: bool deserializeROSmessage( cycdeser & deser, void * ros_message, std::function prefix = nullptr); + bool printROSmessage( + cycprint & deser, + std::function prefix = nullptr); std::string getName(); protected: @@ -133,6 +136,8 @@ private: bool deserializeROSmessage( cycdeser & deser, const MembersType * members, void * ros_message, bool call_new); + bool printROSmessage( + cycprint & deser, const MembersType * members); }; } // namespace rmw_cyclonedds_cpp diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp index 3957982..b963ab9 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/TypeSupport_impl.hpp @@ -776,6 +776,106 @@ bool TypeSupport::deserializeROSmessage( return true; } +template +void print_field(const M * member, cycprint & deser, T & dummy) +{ + if (!member->is_array_) { + deser >> dummy; + } else { + deser.print_constant("{"); + if (member->array_size_ && !member->is_upper_bound_) { + deser.printA(&dummy, member->array_size_); + } else { + int32_t dsize = deser.get32(); + deser.printA(&dummy, dsize); + } + deser.print_constant("}"); + } +} + +template +bool TypeSupport::printROSmessage( + cycprint & deser, const MembersType * members) +{ + assert(members); + + deser.print_constant("{"); + for (uint32_t i = 0; i < members->member_count_; ++i) { + if (i != 0) { + deser.print_constant(","); + } + const auto * member = members->members_ + i; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + {bool dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + {uint8_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + {char dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + {float dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + {double dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + {int16_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + {uint16_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + {int32_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + {uint32_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + {int64_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + {uint64_t dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + {std::string dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + {std::wstring dummy; print_field(member, deser, dummy);} + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = (const MembersType *)member->members_->data; + if (!member->is_array_) { + printROSmessage(deser, sub_members); + } else { + size_t array_size = 0; + if (member->array_size_ && !member->is_upper_bound_) { + array_size = member->array_size_; + } else { + array_size = deser.get32(); + } + deser.print_constant("{"); + for (size_t index = 0; index < array_size; ++index) { + printROSmessage(deser, sub_members); + } + deser.print_constant("}"); + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + deser.print_constant("}"); + + return true; +} + template bool TypeSupport::serializeROSmessage( const void * ros_message, cycser & ser, @@ -818,6 +918,32 @@ bool TypeSupport::deserializeROSmessage( return true; } +template +bool TypeSupport::printROSmessage( + cycprint & prt, + std::function prefix) +{ + if (prefix) { + prt.print_constant("{"); + prefix(prt); + prt.print_constant(","); + } + + if (members_->member_count_ != 0) { + TypeSupport::printROSmessage(prt, members_); + } else { + uint8_t dump = 0; + prt >> dump; + (void)dump; + } + + if (prefix) { + prt.print_constant("}"); + } + + return true; +} + template std::string TypeSupport::getName() { diff --git a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp index 8018448..4c4ae99 100644 --- a/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp +++ b/rmw_cyclonedds_cpp/include/rmw_cyclonedds_cpp/serdes.hpp @@ -16,6 +16,8 @@ #define RMW_CYCLONEDDS_CPP__SERDES_HPP_ #include +#include +#include #include #include @@ -91,7 +93,7 @@ public: } inline void serialize(const std::wstring & x) { - size_t sz = x.size() + 1; + size_t sz = x.size(); serialize(static_cast(sz)); resize(off + sz * sizeof(wchar_t)); memcpy(data() + off, reinterpret_cast(x.c_str()), sz * sizeof(wchar_t)); @@ -216,13 +218,17 @@ public: inline void deserialize(std::string & x) { const uint32_t sz = deserialize32(); - x = std::string(data + pos, sz - 1); + if (sz == 0) { + x = std::string(""); + } else { + x = std::string(data + pos, sz - 1); + } pos += sz; } inline void deserialize(std::wstring & x) { const uint32_t sz = deserialize32(); - x = std::wstring(reinterpret_cast(data + pos), sz - 1); + x = std::wstring(reinterpret_cast(data + pos), sz); pos += sz * sizeof(wchar_t); } @@ -287,4 +293,153 @@ private: size_t lim; // ignored for now ... better provide correct input }; +class cycprint +{ +private: + static bool prtf(char * __restrict * buf, size_t * __restrict bufsize, const char * fmt, ...) + { + va_list ap; + if (*bufsize == 0) { + return false; + } + va_start(ap, fmt); + int n = vsnprintf(*buf, *bufsize, fmt, ap); + va_end(ap); + if (n < 0) { + **buf = 0; + return false; + } else if ((size_t) n <= *bufsize) { + *buf += (size_t) n; + *bufsize -= (size_t) n; + return *bufsize > 0; + } else { + *buf += *bufsize; + *bufsize = 0; + return false; + } + } + +public: + // FIXME: byteswapping + cycprint(char * buf, size_t bufsize, const void * data, size_t size); + cycprint() = delete; + + void print_constant(const char * x) + { + prtf(&buf, &bufsize, "%s", x); + } + + inline cycprint & operator>>(bool & x) {print(x); return *this;} + inline cycprint & operator>>(char & x) {print(x); return *this;} + inline cycprint & operator>>(int8_t & x) {print(x); return *this;} + inline cycprint & operator>>(uint8_t & x) {print(x); return *this;} + inline cycprint & operator>>(int16_t & x) {print(x); return *this;} + inline cycprint & operator>>(uint16_t & x) {print(x); return *this;} + inline cycprint & operator>>(int32_t & x) {print(x); return *this;} + inline cycprint & operator>>(uint32_t & x) {print(x); return *this;} + inline cycprint & operator>>(int64_t & x) {print(x); return *this;} + inline cycprint & operator>>(uint64_t & x) {print(x); return *this;} + inline cycprint & operator>>(float & x) {print(x); return *this;} + inline cycprint & operator>>(double & x) {print(x); return *this;} + inline cycprint & operator>>(char * & x) {print(x); return *this;} + inline cycprint & operator>>(std::string & x) {print(x); return *this;} + inline cycprint & operator>>(std::wstring & x) {print(x); return *this;} + template + inline cycprint & operator>>(std::vector & x) {print(x); return *this;} + template + inline cycprint & operator>>(std::array & x) {print(x); return *this;} + +#define SIMPLE(T, F) inline void print(T & x) { \ + align(sizeof(x)); \ + x = *reinterpret_cast(data + pos); \ + prtf(&buf, &bufsize, F, x); \ + pos += sizeof(x); \ +} + SIMPLE(char, "'%c'"); + SIMPLE(int8_t, "%" PRId8); + SIMPLE(uint8_t, "%" PRIu8); + SIMPLE(int16_t, "%" PRId16); + SIMPLE(uint16_t, "%" PRIu16); + SIMPLE(int32_t, "%" PRId32); + SIMPLE(uint32_t, "%" PRIu32); + SIMPLE(int64_t, "%" PRId64); + SIMPLE(uint64_t, "%" PRIu64); + SIMPLE(float, "%f"); + SIMPLE(double, "%f"); +#undef SIMPLE + + inline void print(bool & x) + { + static_cast(x); + unsigned char z; print(z); + } + inline uint32_t get32() + { + uint32_t sz; + align(sizeof(sz)); + sz = *reinterpret_cast(data + pos); + pos += sizeof(sz); + return sz; + } + inline void print(char * & x) + { + const uint32_t sz = get32(); + const int len = (sz == 0) ? 0 : (sz > INT32_MAX) ? INT32_MAX : static_cast(sz - 1); + static_cast(x); + prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast(data + pos)); + pos += sz; + } + inline void print(std::string & x) + { + const uint32_t sz = get32(); + const int len = (sz == 0) ? 0 : (sz > INT32_MAX) ? INT32_MAX : static_cast(sz - 1); + static_cast(x); + prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast(data + pos)); + pos += sz; + } + inline void print(std::wstring & x) + { + const uint32_t sz = get32(); + x = std::wstring(reinterpret_cast(data + pos), sz); + prtf(&buf, &bufsize, "\"%ls\"", x.c_str()); + pos += sz * sizeof(wchar_t); + } + + template + inline void printA(T * x, size_t cnt) + { + prtf(&buf, &bufsize, "{"); + for (size_t i = 0; i < cnt; i++) { + if (i != 0) {prtf(&buf, &bufsize, ",");} + print(*x); + } + prtf(&buf, &bufsize, "}"); + } + + template + inline void print(std::vector & x) + { + const uint32_t sz = get32(); + printA(x.data(), sz); + } + template + inline void print(std::array & x) + { + printA(x.data(), x.size()); + } + +private: + inline void align(size_t a) + { + if ((pos % a) != 0) { + pos += a - (pos % a); + } + } + + const char * data; + size_t pos; + char * buf; + size_t bufsize; +}; + #endif // RMW_CYCLONEDDS_CPP__SERDES_HPP_ diff --git a/rmw_cyclonedds_cpp/src/serdata.cpp b/rmw_cyclonedds_cpp/src/serdata.cpp index 75d187f..bc6f9d3 100644 --- a/rmw_cyclonedds_cpp/src/serdata.cpp +++ b/rmw_cyclonedds_cpp/src/serdata.cpp @@ -175,11 +175,11 @@ static struct ddsi_serdata * serdata_rmw_from_sample( if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { auto typed_typesupport = static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd, nullptr); + (void) typed_typesupport->serializeROSmessage(sample, sd); } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { auto typed_typesupport = static_cast(topic->type_support.type_support_); - (void) typed_typesupport->serializeROSmessage(sample, sd, nullptr); + (void) typed_typesupport->serializeROSmessage(sample, sd); } } else { /* The "prefix" lambda is there to inject the service invocation header data into the CDR @@ -270,11 +270,11 @@ static bool serdata_rmw_to_sample( if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { auto typed_typesupport = static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, sample, nullptr); + return typed_typesupport->deserializeROSmessage(sd, sample); } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { auto typed_typesupport = static_cast(topic->type_support.type_support_); - return typed_typesupport->deserializeROSmessage(sd, sample, nullptr); + return typed_typesupport->deserializeROSmessage(sd, sample); } } else { /* The "prefix" lambda is there to inject the service invocation header data into the CDR @@ -318,6 +318,49 @@ static bool serdata_rmw_eqkey(const struct ddsi_serdata * a, const struct ddsi_s return true; } +#if DDSI_SERDATA_HAS_PRINT +static size_t serdata_rmw_print( + const struct ddsi_sertopic * tpcmn, const struct ddsi_serdata * dcmn, char * buf, size_t bufsize) +{ + const struct serdata_rmw * d = static_cast(dcmn); + const struct sertopic_rmw * topic = static_cast(tpcmn); + if (d->kind != SDK_DATA) { + /* ROS2 doesn't do keys in a meaningful way yet */ + return static_cast(snprintf(buf, bufsize, ":k:{}")); + } else if (!topic->is_request_header) { + cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd); + } + } 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 wrap; + auto prefix = [&wrap](cycprint & ser) { + ser >> wrap.header.guid; ser.print_constant(","); ser >> wrap.header.seq; + }; + cycprint sd(buf, bufsize, static_cast(d->data.data()), d->data.size()); + if (using_introspection_c_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd, prefix); + } else if (using_introspection_cpp_typesupport(topic->type_support.typesupport_identifier_)) { + auto typed_typesupport = + static_cast(topic->type_support.type_support_); + return typed_typesupport->printROSmessage(sd, prefix); + } + } + return false; +} +#endif + static const struct ddsi_serdata_ops serdata_rmw_ops = { serdata_rmw_eqkey, serdata_rmw_size, @@ -331,6 +374,9 @@ static const struct ddsi_serdata_ops serdata_rmw_ops = { serdata_rmw_to_topicless, serdata_rmw_topicless_to_sample, serdata_rmw_free +#if DDSI_SERDATA_HAS_PRINT + , serdata_rmw_print +#endif }; static void sertopic_rmw_free(struct ddsi_sertopic * tpcmn) diff --git a/rmw_cyclonedds_cpp/src/serdes.cpp b/rmw_cyclonedds_cpp/src/serdes.cpp index 29b2b1e..ab531c4 100644 --- a/rmw_cyclonedds_cpp/src/serdes.cpp +++ b/rmw_cyclonedds_cpp/src/serdes.cpp @@ -34,3 +34,12 @@ cycdeser::cycdeser(const void * data_, size_t size_) lim(size_ - 4) { } + +cycprint::cycprint(char * buf_, size_t bufsize_, const void * data_, size_t size_) +: data(static_cast(data_) + 4), + pos(0), + buf(buf_), + bufsize(bufsize_) +{ + static_cast(size_); +}