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

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

rcl/include/rcl/publisher.h

@@ -43,6 +43,7 @@ typedef struct rcl_publisher_options_t
   /// Custom allocator for the publisher, used for incidental allocations.
   /** For default behavior (malloc/free), use: rcl_get_default_allocator() */
   rcl_allocator_t allocator;
+  bool preallocate;
 } rcl_publisher_options_t;
 
 /// Return a rcl_publisher_t struct with members set to `NULL`.

rcl/src/rcl/publisher.c

@@ -37,6 +37,7 @@ typedef struct rcl_publisher_impl_t
   rcl_publisher_options_t options;
   rcl_context_t * context;
   rmw_publisher_t * rmw_handle;
+  rmw_publisher_allocation_t * allocation;
 } rcl_publisher_impl_t;
 
 rcl_publisher_t
@@ -73,6 +74,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();
@@ -161,6 +164,15 @@ 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);
+
+  publisher->impl->allocation = NULL;
+  if(options->preallocate){
+    publisher->impl->allocation = (rmw_publisher_allocation_t *)allocator->allocate(
+      sizeof(rmw_publisher_allocation_t), allocator->state);
+    RCL_CHECK_FOR_NULL_WITH_MSG(
+      publisher->impl->allocation, "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
@@ -176,11 +188,29 @@ rcl_publisher_init(
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized");
   // context
   publisher->impl->context = node->context;
-  goto cleanup;
+
+  if(options->preallocate){
+    rmw_ret = rmw_init_publisher_allocation(
+        type_support, NULL, publisher->impl->allocation
+    );
+    if (rmw_ret != RMW_RET_OK) {
+      RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+      ret = RCL_RET_ERROR;
+      goto fail;
+    }
+  }
+
+goto cleanup;
 fail:
-  if (publisher->impl) {
+ if(options->preallocate){
+   if (NULL != publisher->impl->allocation) {
+     allocator->deallocate(publisher->impl->allocation, allocator->state);
+   }
+ }
+ if (publisher->impl) {
     allocator->deallocate(publisher->impl, allocator->state);
   }
+
   ret = fail_ret;
   // Fall through to cleanup
 cleanup:
@@ -215,6 +245,9 @@ rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node)
       RCL_SET_ERROR_MSG(rmw_get_error_string().str);
       result = RCL_RET_ERROR;
     }
+    if(publisher->impl->allocation){
+      allocator.deallocate(publisher->impl->allocation, allocator.state);
+    }
     allocator.deallocate(publisher->impl, allocator.state);
   }
   RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized");
@@ -229,6 +262,7 @@ rcl_publisher_get_default_options()
   // Must set the allocator and qos after because they are not a compile time constant.
   default_options.qos = rmw_qos_profile_default;
   default_options.allocator = rcl_get_default_allocator();
+  default_options.preallocate = false;
   return default_options;
 }
 
@@ -239,7 +273,7 @@ rcl_publish(const rcl_publisher_t * publisher, const void * ros_message)
     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, NULL) != RMW_RET_OK) {
     RCL_SET_ERROR_MSG(rmw_get_error_string().str);
     return RCL_RET_ERROR;
   }