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

Add support for printing messages to DDSI trace

Browse source 
NOTE: not tested against versions of Cyclone that don't have the feature

Signed-off-by: Erik Boasson <eb@ilities.com>
This commit is contained in:
Erik Boasson 2019-09-01 15:32:04 +02:00 • committed by eboasson
parent 888235e96e
commit fb8c08bd7f
5 changed files with 348 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -118,6 +118,9 @@ public:
bool deserializeROSmessage(
cycdeser & deser, void * ros_message,
std::function<void(cycdeser &)> prefix = nullptr);
bool printROSmessage(
cycprint & deser,
std::function<void(cycprint &)> 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

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

@ -776,6 +776,106 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
return true;
}
template<typename M, typename T>
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<typename MembersType>
bool TypeSupport<MembersType>::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<typename MembersType>
bool TypeSupport<MembersType>::serializeROSmessage(
const void * ros_message, cycser & ser,
@ -818,6 +918,32 @@ bool TypeSupport<MembersType>::deserializeROSmessage(
return true;
}
template<typename MembersType>
bool TypeSupport<MembersType>::printROSmessage(
cycprint & prt,
std::function<void(cycprint &)> 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<typename MembersType>
std::string TypeSupport<MembersType>::getName()
{

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

@ -16,6 +16,8 @@
#define RMW_CYCLONEDDS_CPP__SERDES_HPP_
#include <string.h>
#include <inttypes.h>
#include <stdarg.h>
#include <array>
#include <string>
@ -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<uint32_t>(sz));
resize(off + sz * sizeof(wchar_t));
memcpy(data() + off, reinterpret_cast<const char *>(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<const wchar_t *>(data + pos), sz - 1);
x = std::wstring(reinterpret_cast<const wchar_t *>(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<class T>
inline cycprint & operator>>(std::vector<T> & x) {print(x); return *this;}
template<class T, size_t S>
inline cycprint & operator>>(std::array<T, S> & x) {print(x); return *this;}
#define SIMPLE(T, F) inline void print(T & x) { \
align(sizeof(x)); \
x = *reinterpret_cast<const T *>(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<void>(x);
unsigned char z; print(z);
}
inline uint32_t get32()
{
uint32_t sz;
align(sizeof(sz));
sz = *reinterpret_cast<const uint32_t *>(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<int>(sz - 1);
static_cast<void>(x);
prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast<const char *>(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<int>(sz - 1);
static_cast<void>(x);
prtf(&buf, &bufsize, "\"%*.*s\"", len, len, static_cast<const char *>(data + pos));
pos += sz;
}
inline void print(std::wstring & x)
{
const uint32_t sz = get32();
x = std::wstring(reinterpret_cast<const wchar_t *>(data + pos), sz);
prtf(&buf, &bufsize, "\"%ls\"", x.c_str());
pos += sz * sizeof(wchar_t);
}
template<class T>
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<class T>
inline void print(std::vector<T> & x)
{
const uint32_t sz = get32();
printA(x.data(), sz);
}
template<class T, size_t S>
inline void print(std::array<T, S> & 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_

54
rmw_cyclonedds_cpp/src/serdata.cpp
View file

@ -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<MessageTypeSupport_c *>(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<MessageTypeSupport_cpp *>(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<MessageTypeSupport_c *>(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<MessageTypeSupport_cpp *>(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<const struct serdata_rmw *>(dcmn);
const struct sertopic_rmw * topic = static_cast<const struct sertopic_rmw *>(tpcmn);
if (d->kind != SDK_DATA) {
/* ROS2 doesn't do keys in a meaningful way yet */
return static_cast<size_t>(snprintf(buf, bufsize, ":k:{}"));
} else if (!topic->is_request_header) {
cycprint sd(buf, bufsize, 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->printROSmessage(sd);
} 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->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<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->printROSmessage(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_);
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)

9
rmw_cyclonedds_cpp/src/serdes.cpp
View file

@ -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<const char *>(data_) + 4),
pos(0),
buf(buf_),
bufsize(bufsize_)
{
static_cast<void>(size_);
}