diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h
index 44b9c50..3aadd1e 100644
--- a/rcl/include/rcl/publisher.h
+++ b/rcl/include/rcl/publisher.h
@@ -252,6 +252,42 @@ RCL_WARN_UNUSED
rcl_ret_t
rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
+/// Publish a serialized message on a topic using a publisher.
+/**
+ * It is the job of the caller to ensure that the type of the serialized message
+ * parameter and the type associate with the publisher (via the type support)
+ * match.
+ * Even though this call to publish takes an already serialized serialized message,
+ * the publisher has to register its type as a ROS known message type.
+ * Passing a serialized message from a different type leads to undefined behavior on the subscriber side.
+ * The publish call might be able to send any abitrary serialized message, it is however
+ * not garantueed that the subscriber side successfully deserializes this byte stream.
+ *
+ * Apart from this, the `publish_serialized` function has the same behavior as `rcl_publish`
+ * expect that no serialization step is done.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | No
+ * Thread-Safe | Yes [1]
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ * [1] for unique pairs of publishers and messages, see above for more
+ *
+ * \param[in] publisher handle to the publisher which will do the publishing
+ * \param[in] serialized_message pointer to the already serialized message in raw form
+ * \return `RCL_RET_OK` if the message was published successfully, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_publish_serialized_message(
+ const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message);
+
/// Get the topic name for the publisher.
/**
* This function returns the publisher's internal topic name string.
diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h
index 507245b..252949b 100644
--- a/rcl/include/rcl/subscription.h
+++ b/rcl/include/rcl/subscription.h
@@ -256,6 +256,49 @@ rcl_take(
void * ros_message,
rmw_message_info_t * message_info);
+/// Take a serialized raw message from a topic using a rcl subscription.
+/**
+ * In contrast to `rcl_take`, this function stores the taken message in
+ * its raw binary representation.
+ * It is the job of the caller to ensure that the type associate with the subscription
+ * matches, and can optionally be deserialized into its ROS message via, the correct
+ * type support.
+ * If the `serialized_message` parameter contains enough preallocated memory, the incoming
+ * message can be taken without any additional memory allocation.
+ * If not, the function will dynamically allocate enough memory for the message.
+ * Passing a different type to rcl_take produces undefined behavior and cannot
+ * be checked by this function and therefore no deliberate error will occur.
+ *
+ * Apart from the differences above, this function behaves like `rcl_take`.
+ *
+ *
+ * Attribute | Adherence
+ * ------------------ | -------------
+ * Allocates Memory | Maybe [1]
+ * Thread-Safe | No
+ * Uses Atomics | No
+ * Lock-Free | Yes
+ * [1] only if storage in the serialized_message is insufficient
+ *
+ * \param[in] subscription the handle to the subscription from which to take
+ * \param[inout] serialized_message pointer to a (pre-allocated) serialized message.
+ * \param[out] message_info rmw struct which contains meta-data for the message
+ * \return `RCL_RET_OK` if the message was published, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or
+ * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
+ * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error
+ * occurred in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_PUBLIC
+RCL_WARN_UNUSED
+rcl_ret_t
+rcl_take_serialized_message(
+ const rcl_subscription_t * subscription,
+ rcl_serialized_message_t * serialized_message,
+ rmw_message_info_t * message_info);
+
/// Get the topic name for the subscription.
/**
* This function returns the subscription's internal topic name string.
diff --git a/rcl/include/rcl/types.h b/rcl/include/rcl/types.h
index 0e6bfd7..4825bf2 100644
--- a/rcl/include/rcl/types.h
+++ b/rcl/include/rcl/types.h
@@ -97,4 +97,7 @@ typedef rmw_ret_t rcl_ret_t;
/// Argument is not a valid log level rule
#define RCL_RET_INVALID_LOG_LEVEL_RULE 1020
+/// typedef for rmw_serialized_message_t;
+typedef rmw_serialized_message_t rcl_serialized_message_t;
+
#endif // RCL__TYPES_H_
diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c
index 1cbabb8..ef0e5cd 100644
--- a/rcl/src/rcl/publisher.c
+++ b/rcl/src/rcl/publisher.c
@@ -244,6 +244,24 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
return RCL_RET_OK;
}
+rcl_ret_t
+rcl_publish_serialized_message(
+ const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message)
+{
+ if (!rcl_publisher_is_valid(publisher, NULL)) {
+ return RCL_RET_PUBLISHER_INVALID;
+ }
+ rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message);
+ if (ret != RMW_RET_OK) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), rcl_get_default_allocator());
+ if (ret == RMW_RET_BAD_ALLOC) {
+ return RCL_RET_BAD_ALLOC;
+ }
+ return RMW_RET_ERROR;
+ }
+ return RCL_RET_OK;
+}
+
const char *
rcl_publisher_get_topic_name(const rcl_publisher_t * publisher)
{
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c
index 2a8837b..319419f 100644
--- a/rcl/src/rcl/subscription.c
+++ b/rcl/src/rcl/subscription.c
@@ -241,18 +241,12 @@ rcl_take(
rmw_message_info_t * message_info)
{
RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message")
- RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator());
- const rcl_subscription_options_t * options = rcl_subscription_get_options(subscription);
- if (!options) {
- return RCL_RET_SUBSCRIPTION_INVALID;
+ rcl_allocator_t error_allocator = rcl_get_default_allocator();
+ if (!rcl_subscription_is_valid(subscription, &error_allocator)) {
+ return RCL_RET_SUBSCRIPTION_INVALID; // error message already set
}
- RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT, options->allocator);
- RCL_CHECK_FOR_NULL_WITH_MSG(
- subscription->impl, "subscription is invalid",
- return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
- RCL_CHECK_FOR_NULL_WITH_MSG(
- subscription->impl->rmw_handle,
- "subscription is invalid", return RCL_RET_SUBSCRIPTION_INVALID, options->allocator);
+ RCL_CHECK_ARGUMENT_FOR_NULL(
+ ros_message, RCL_RET_INVALID_ARGUMENT, error_allocator);
// If message_info is NULL, use a place holder which can be discarded.
rmw_message_info_t dummy_message_info;
rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
@@ -261,7 +255,7 @@ rcl_take(
rmw_ret_t ret =
rmw_take_with_info(subscription->impl->rmw_handle, ros_message, &taken, message_info_local);
if (ret != RMW_RET_OK) {
- RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), options->allocator);
+ RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), error_allocator);
return RCL_RET_ERROR;
}
RCUTILS_LOG_DEBUG_NAMED(
@@ -272,6 +266,41 @@ rcl_take(
return RCL_RET_OK;
}
+rcl_ret_t
+rcl_take_serialized_message(
+ const rcl_subscription_t * subscription,
+ rcl_serialized_message_t * serialized_message,
+ rmw_message_info_t * message_info)
+{
+ RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message")
+ rcl_allocator_t error_allocator = rcl_get_default_allocator();
+ if (!rcl_subscription_is_valid(subscription, &error_allocator)) {
+ return RCL_RET_SUBSCRIPTION_INVALID; // error message already set
+ }
+ RCL_CHECK_ARGUMENT_FOR_NULL(
+ serialized_message, RCL_RET_INVALID_ARGUMENT, error_allocator);
+ // If message_info is NULL, use a place holder which can be discarded.
+ rmw_message_info_t dummy_message_info;
+ rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info;
+ // Call rmw_take_with_info.
+ bool taken = false;
+ rmw_ret_t ret = rmw_take_serialized_message_with_info(
+ subscription->impl->rmw_handle, serialized_message, &taken, message_info_local);
+ if (ret != RMW_RET_OK) {
+ RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), error_allocator);
+ if (ret == RMW_RET_BAD_ALLOC) {
+ return RCL_RET_BAD_ALLOC;
+ }
+ return RCL_RET_ERROR;
+ }
+ RCUTILS_LOG_DEBUG_NAMED(
+ ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false")
+ if (!taken) {
+ return RCL_RET_SUBSCRIPTION_TAKE_FAILED;
+ }
+ return RCL_RET_OK;
+}
+
const char *
rcl_subscription_get_topic_name(const rcl_subscription_t * subscription)
{