diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 9f657c5..fb7aacc 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -196,6 +196,66 @@ RCL_WARN_UNUSED rcl_publisher_options_t rcl_publisher_get_default_options(void); +/// Borrow a loaned message. +/** + * The memory allocated for the ros message belongs to the middleware and must not be deallocated + * other than by a call to \sa rcl_return_loaned_message. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [0] the underlying middleware might allocate new memory or returns an existing chunk form a pool. + * The function in rcl however does not allocate any additional memory. + * + * \param[in] publisher Publisher to which the allocated message is associated. + * \param[in] type_support Typesupport to which the internal ros message is allocated. + * \param[out] ros_message The pointer to be filled to a valid ros message by the middleware. + * \return `RCL_RET_OK` if the ros message was correctly initialized, or + * \return `RCL_RET_INVALID_ARGUMENT` if an argument other than the ros message is null, or + * \return `RCL_RET_BAD_ALLOC` if the ros message could not be correctly created, or + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unexpected error occured. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_borrow_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message); + +/// Return a loaned message +/** + * The ownership of the passed in ros message will be transferred back to the middleware. + * The middleware might deallocate and destroy the message so that the pointer is no longer + * guaranteed to be valid after that call. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher Publisher to which the loaned message is associated. + * \param[in] loaned_message Loaned message to be deallocated and destroyed. + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if an argument is null, or + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unexpected error occurs and no message can be initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_return_loaned_message( + const rcl_publisher_t * publisher, + void * loaned_message); + /// Publish a ROS message on a topic using a publisher. /** * It is the job of the caller to ensure that the type of the ros_message @@ -302,6 +362,48 @@ rcl_publish_serialized_message( rmw_publisher_allocation_t * allocation ); +/// Publish a loaned message on a topic using a publisher. +/** + * A previously borrowed loaned message can be sent via this call to `rcl_publish_loaned_message`. + * By calling this function, the ownership of the loaned message is getting transferred back + * to the middleware. + * The pointer to the `ros_message` is not guaranteed to be valid after as the middleware + * migth deallocate the memory for this message internally. + * It is thus recommended to call this function only in combination with + * \sa `rcl_borrow_loaned_message`. + * + * Apart from this, the `publish_loaned_message` function has the same behavior as `rcl_publish` + * except that no serialization step is done. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [0] the middleware might deallocate the loaned message. + * The RCL function however does not allocate any memory. + * [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] ros_message pointer to the previously borrow loaned message + * \param[in] allocation structure pointer, used for memory preallocation (may be NULL) + * \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_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publish_loaned_message( + const rcl_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation +); + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) /** * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator of @@ -528,6 +630,16 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher); + +/// Check if publisher instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ +RCL_PUBLIC +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher); + #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 7de6ed8..430eea4 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -311,6 +311,74 @@ rcl_take_serialized_message( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation); +/// Take a loaned message from a topic using a rcl subscription. +/** + * Depending on the middleware, incoming messages can be loaned to the user's callback + * without further copying. + * The implicit contract here is that the middleware owns the memory allocated for this message. + * The user must not destroy the message, but rather has to return it with a call to + * \sa rcl_return_loaned_message to the middleware. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] loaned_message a pointer to the loaned messages. + * \param[out] message_info rmw struct which contains meta-data for the message. + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return `RCL_RET_OK` if the loaned message sequence was taken, 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_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +/// Release a loaned message from a topic using a rcl subscription. +/** + * If a loaned message was previously obtained from the middleware with a call to + * \sa rcl_take_loaned_message, this message has to be released to indicate to the middleware + * that the user no longer needs that memory. + * The user must not delete the message. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[in] loaned_message a pointer to the loaned messages. + * \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_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_release_loaned_message( + const rcl_subscription_t * subscription, + void * loaned_message); + /// Get the topic name for the subscription. /** * This function returns the subscription's internal topic name string. @@ -471,6 +539,15 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription); +/// Check if subscription instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ +RCL_PUBLIC +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 8cb7ce8..3b02be7 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -245,6 +245,30 @@ rcl_publisher_get_default_options() return default_options; } +rcl_ret_t +rcl_borrow_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + return rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message); +} + +rcl_ret_t +rcl_return_loaned_message( + const rcl_publisher_t * publisher, + void * loaned_message) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rmw_return_loaned_message(publisher->impl->rmw_handle, loaned_message); +} + rcl_ret_t rcl_publish( const rcl_publisher_t * publisher, @@ -284,6 +308,24 @@ rcl_publish_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_publish_loaned_message( + const rcl_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_publish_loaned_message(publisher->impl->rmw_handle, ros_message, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + rcl_ret_t rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher) { @@ -390,6 +432,15 @@ rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher) return &publisher->impl->actual_qos; } +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid(publisher)) { + return false; // error message already set + } + return publisher->impl->rmw_handle->can_loan_messages; +} + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 45aee83..506a5d8 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -310,6 +310,56 @@ rcl_take_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + if (*loaned_message) { + RCL_SET_ERROR_MSG("loaned message is already initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + // 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_loaned_message_with_info( + subscription->impl->rmw_handle, loaned_message, &taken, message_info_local, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + if (RMW_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription loaned take succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_release_loaned_message( + const rcl_subscription_t * subscription, + void * loaned_message) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription releasing loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rmw_release_loaned_message(subscription->impl->rmw_handle, loaned_message); +} + const char * rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) { @@ -378,6 +428,15 @@ rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription) return &subscription->impl->actual_qos; } +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return false; // error message already set + } + return subscription->impl->rmw_handle->can_loan_messages; +} + #ifdef __cplusplus } #endif