Rmw preallocate (#428)

* Proposola of changes for RMW_Preallocate. Related /ros2/rmw#160

Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>

* Changed RCL interface

Signed-off-by: Gonzalo de Pedro <gonzalo@depedro.com.ar>

* Updates for allocation in serialize methods.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

* Fix tests for new APIs.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

rcl/include/rcl/publisher.h

@@ -150,7 +150,8 @@ rcl_publisher_init(
   const rcl_node_t * node,
   const rosidl_message_type_support_t * type_support,
   const char * topic_name,
-  const rcl_publisher_options_t * options);
+  const rcl_publisher_options_t * options
+);
 
 /// Finalize a rcl_publisher_t.
 /**
@@ -244,6 +245,7 @@ rcl_publisher_get_default_options(void);
  *
  * \param[in] publisher handle to the publisher which will do the publishing
  * \param[in] ros_message type-erased pointer to the ROS 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
@@ -252,7 +254,11 @@ rcl_publisher_get_default_options(void);
 RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
-rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
+rcl_publish(
+  const rcl_publisher_t * publisher,
+  const void * ros_message,
+  rmw_publisher_allocation_t * allocation
+);
 
 /// Publish a serialized message on a topic using a publisher.
 /**
@@ -279,6 +285,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message);
  *
  * \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
+ * \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
@@ -288,7 +295,10 @@ RCL_PUBLIC
 RCL_WARN_UNUSED
 rcl_ret_t
 rcl_publish_serialized_message(
-  const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message);
+  const rcl_publisher_t * publisher,
+  const rcl_serialized_message_t * serialized_message,
+  rmw_publisher_allocation_t * allocation
+);
 
 /// Get the topic name for the publisher.
 /**

rcl/include/rcl/subscription.h

@@ -153,7 +153,8 @@ rcl_subscription_init(
   const rcl_node_t * node,
   const rosidl_message_type_support_t * type_support,
   const char * topic_name,
-  const rcl_subscription_options_t * options);
+  const rcl_subscription_options_t * options
+);
 
 /// Finalize a rcl_subscription_t.
 /**
@@ -246,6 +247,7 @@ rcl_subscription_get_default_options(void);
  * \param[in] subscription the handle to the subscription from which to take
  * \param[inout] ros_message type-erased ptr to a allocated ROS message
  * \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 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
@@ -260,7 +262,9 @@ rcl_ret_t
 rcl_take(
   const rcl_subscription_t * subscription,
   void * ros_message,
-  rmw_message_info_t * message_info);
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation
+);
 
 /// Take a serialized raw message from a topic using a rcl subscription.
 /**
@@ -289,6 +293,7 @@ rcl_take(
  * \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
+ * \param[in] allocation structure pointer used for memory preallocation (may be NULL)
  * \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
@@ -303,7 +308,8 @@ rcl_ret_t
 rcl_take_serialized_message(
   const rcl_subscription_t * subscription,
   rcl_serialized_message_t * serialized_message,
-  rmw_message_info_t * message_info);
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation);
 
 /// Get the topic name for the subscription.
 /**

rcl/src/rcl/logging_rosout.c

@@ -260,7 +260,7 @@ void rcl_logging_rosout_output_handler(
         rosidl_generator_c__String__assign(&log_message->msg, msg_array.buffer);
         rosidl_generator_c__String__assign(&log_message->file, location->file_name);
         rosidl_generator_c__String__assign(&log_message->function, location->function_name);
-        status = rcl_publish(&entry.publisher, log_message);
+        status = rcl_publish(&entry.publisher, log_message, NULL);
         if (RCL_RET_OK != status) {
           RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to publish log message to rosout: ");
           RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str);

rcl/src/rcl/publisher.c

@@ -53,7 +53,8 @@ rcl_publisher_init(
   const rcl_node_t * node,
   const rosidl_message_type_support_t * type_support,
   const char * topic_name,
-  const rcl_publisher_options_t * options)
+  const rcl_publisher_options_t * options
+)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
 
@@ -74,6 +75,8 @@ rcl_publisher_init(
   RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT);
   RCUTILS_LOG_DEBUG_NAMED(
     ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name);
+
+
   // Expand the given topic name.
   rcutils_allocator_t rcutils_allocator = *allocator;  // implicit conversion to rcutils version
   rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map();
@@ -162,6 +165,7 @@ rcl_publisher_init(
     sizeof(rcl_publisher_impl_t), allocator->state);
   RCL_CHECK_FOR_NULL_WITH_MSG(
     publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup);
+
   // Fill out implementation struct.
   // rmw handle (create rmw publisher)
   // TODO(wjwwood): pass along the allocator to rmw when it supports it
@@ -188,11 +192,13 @@ rcl_publisher_init(
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
   // context
   publisher->impl->context = node->context;
+
   goto cleanup;
 fail:
   if (publisher->impl) {
     allocator->deallocate(publisher->impl, allocator->state);
   }
+
   ret = fail_ret;
   // Fall through to cleanup
 cleanup:
@@ -245,13 +251,16 @@ rcl_publisher_get_default_options()
 }
 
 rcl_ret_t
-rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
+rcl_publish(
+  const rcl_publisher_t * publisher,
+  const 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);
-  if (rmw_publish(publisher->impl->rmw_handle, ros_message) != RMW_RET_OK) {
+  if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     return RCL_RET_ERROR;
   }
@@ -260,13 +269,16 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
 
 rcl_ret_t
 rcl_publish_serialized_message(
-  const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message)
+  const rcl_publisher_t * publisher,
+  const rcl_serialized_message_t * serialized_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(serialized_message, RCL_RET_INVALID_ARGUMENT);
-  rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message);
+  rmw_ret_t ret = rmw_publish_serialized_message(publisher->impl->rmw_handle, serialized_message,
+      allocation);
   if (ret != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     if (ret == RMW_RET_BAD_ALLOC) {

rcl/src/rcl/subscription.c

@@ -49,7 +49,8 @@ rcl_subscription_init(
   const rcl_node_t * node,
   const rosidl_message_type_support_t * type_support,
   const char * topic_name,
-  const rcl_subscription_options_t * options)
+  const rcl_subscription_options_t * options
+)
 {
   rcl_ret_t fail_ret = RCL_RET_ERROR;
 
@@ -235,7 +236,9 @@ rcl_ret_t
 rcl_take(
   const rcl_subscription_t * subscription,
   void * ros_message,
-  rmw_message_info_t * message_info)
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation
+)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message");
   if (!rcl_subscription_is_valid(subscription)) {
@@ -249,7 +252,8 @@ rcl_take(
   // Call rmw_take_with_info.
   bool taken = false;
   rmw_ret_t ret =
-    rmw_take_with_info(subscription->impl->rmw_handle, ros_message, &taken, message_info_local);
+    rmw_take_with_info(subscription->impl->rmw_handle, ros_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) {
@@ -269,7 +273,9 @@ rcl_ret_t
 rcl_take_serialized_message(
   const rcl_subscription_t * subscription,
   rcl_serialized_message_t * serialized_message,
-  rmw_message_info_t * message_info)
+  rmw_message_info_t * message_info,
+  rmw_subscription_allocation_t * allocation
+)
 {
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message");
   if (!rcl_subscription_is_valid(subscription)) {
@@ -282,7 +288,7 @@ rcl_take_serialized_message(
   // 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);
+    subscription->impl->rmw_handle, serialized_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) {

rcl/test/rcl/test_publisher.cpp

@@ -93,7 +93,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin
   test_msgs__msg__BasicTypes msg;
   test_msgs__msg__BasicTypes__init(&msg);
   msg.int64_value = 42;
-  ret = rcl_publish(&publisher, &msg);
+  ret = rcl_publish(&publisher, &msg, nullptr);
   test_msgs__msg__BasicTypes__fini(&msg);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 }
@@ -116,7 +116,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin
   test_msgs__msg__Strings msg;
   test_msgs__msg__Strings__init(&msg);
   ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.string_value, "testing"));
-  ret = rcl_publish(&publisher, &msg);
+  ret = rcl_publish(&publisher, &msg, nullptr);
   test_msgs__msg__Strings__fini(&msg);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 }
@@ -160,14 +160,14 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publishers_diff
   test_msgs__msg__BasicTypes msg_int;
   test_msgs__msg__BasicTypes__init(&msg_int);
   msg_int.int64_value = 42;
-  ret = rcl_publish(&publisher, &msg_int);
+  ret = rcl_publish(&publisher, &msg_int, nullptr);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   test_msgs__msg__BasicTypes__fini(&msg_int);
 
   test_msgs__msg__Strings msg_string;
   test_msgs__msg__Strings__init(&msg_string);
   ASSERT_TRUE(rosidl_generator_c__String__assign(&msg_string.string_value, "testing"));
-  ret = rcl_publish(&publisher_in_namespace, &msg_string);
+  ret = rcl_publish(&publisher_in_namespace, &msg_string, nullptr);
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 }
 

rcl/test/rcl/test_subscription.cpp

@@ -163,7 +163,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     test_msgs__msg__BasicTypes msg;
     test_msgs__msg__BasicTypes__init(&msg);
     msg.int64_value = 42;
-    ret = rcl_publish(&publisher, &msg);
+    ret = rcl_publish(&publisher, &msg, nullptr);
     test_msgs__msg__BasicTypes__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
@@ -176,7 +176,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
       test_msgs__msg__BasicTypes__fini(&msg);
     });
-    ret = rcl_take(&subscription, &msg, nullptr);
+    ret = rcl_take(&subscription, &msg, nullptr, nullptr);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     ASSERT_EQ(42, msg.int64_value);
   }
@@ -214,7 +214,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     test_msgs__msg__Strings msg;
     test_msgs__msg__Strings__init(&msg);
     ASSERT_TRUE(rosidl_generator_c__String__assign(&msg.string_value, test_string));
-    ret = rcl_publish(&publisher, &msg);
+    ret = rcl_publish(&publisher, &msg, nullptr);
     test_msgs__msg__Strings__fini(&msg);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
@@ -227,7 +227,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
       test_msgs__msg__Strings__fini(&msg);
     });
-    ret = rcl_take(&subscription, &msg, nullptr);
+    ret = rcl_take(&subscription, &msg, nullptr, nullptr);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
     ASSERT_EQ(std::string(test_string), std::string(msg.string_value.data, msg.string_value.size));
   }

rcl_action/src/rcl_action/action_client.c

@@ -390,7 +390,7 @@ rcl_action_take_cancel_response(
   RCL_CHECK_ARGUMENT_FOR_NULL(ros_ ## Type, RCL_RET_INVALID_ARGUMENT); \
   rmw_message_info_t message_info; /* ignored */ \
   rcl_ret_t ret = rcl_take( \
-    &action_client->impl->Type ## _subscription, ros_ ## Type, &message_info); \
+    &action_client->impl->Type ## _subscription, ros_ ## Type, &message_info, NULL); \
   if (RCL_RET_OK != ret) { \
     if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret) { \
       return RCL_RET_ACTION_CLIENT_TAKE_FAILED; \

rcl_action/src/rcl_action/action_server.c

@@ -491,7 +491,7 @@ rcl_action_publish_feedback(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(ros_feedback, RCL_RET_INVALID_ARGUMENT);
 
-  rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback);
+  rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback, NULL);
   if (RCL_RET_OK != ret) {
     return RCL_RET_ERROR;  // error already set
   }
@@ -559,7 +559,7 @@ rcl_action_publish_status(
   }
   RCL_CHECK_ARGUMENT_FOR_NULL(status_message, RCL_RET_INVALID_ARGUMENT);
 
-  rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message);
+  rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message, NULL);
   if (RCL_RET_OK != ret) {
     return RCL_RET_ERROR;  // error already set
   }

rcl_lifecycle/src/com_interface.c

@@ -255,7 +255,7 @@ rcl_lifecycle_com_interface_publish_notification(
   msg.goal_state.id = goal->id;
   rosidl_generator_c__String__assign(&msg.goal_state.label, goal->label);
 
-  return rcl_publish(&com_interface->pub_transition_event, &msg);
+  return rcl_publish(&com_interface->pub_transition_event, &msg, NULL);
 }
 
 #ifdef __cplusplus