Zero copy api (#506)

* allocate loaned message

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* can_loan_messages for subscription

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* first draft of loaned message sequence

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* take loaned message

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* borrow/return & take/release

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* introduce rmw_publish_loaned_message

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* const correct publish

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* fix typo

Signed-off-by: Knese Karsten <karsten@openrobotics.org>

* address review

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

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.
+ *
+ * <hr>
+ * 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.
+ *
+ * <hr>
+ * 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.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No [0]
+ * Thread-Safe        | Yes [1]
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ * <i>[0] the middleware might deallocate the loaned message.
+ * The RCL function however does not allocate any memory.</i>
+ * <i>[1] for unique pairs of publishers and messages, see above for more</i>
+ *
+ * \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

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.
+ *
+ * <hr>
+ * 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.
+ *
+ * <hr>
+ * 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

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

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