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) {