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

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)