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 <eb@ilities.com>
This commit is contained in:
Erik Boasson
eboasson
parent
888235e96e
commit
fb8c08bd7f
5 changed files with 348 additions and 7 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
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_
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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_);
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue