Changed RCL interface

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

rcl/include/rcl/publisher.h

@@ -43,7 +43,6 @@ 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`.
@@ -151,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.
 /**
@@ -245,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] publihser allocation structure pointer, used for memory preallocation
  * \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
@@ -253,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.
 /**

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] subscription allocation structure pointer used for memory preallocation
  * \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.
 /**

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

@@ -38,7 +38,6 @@ typedef struct rcl_publisher_impl_t
  rmw_qos_profile_t actual_qos;
  rcl_context_t * context;
  rmw_publisher_t * rmw_handle;
- rmw_publisher_allocation_t * allocation;
 } rcl_publisher_impl_t;
 
 rcl_publisher_t
@@ -54,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;
 
@@ -166,22 +166,15 @@ rcl_publisher_init(
  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
  publisher->impl->rmw_handle = rmw_create_publisher(
  rcl_node_get_rmw_handle(node),
  type_support,
  remapped_topic_name,
- &(options->qos));
+ &(options->qos)
+ );
  RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl->rmw_handle,
  rmw_get_error_string().str, goto fail);
  // get actual qos, and store it
@@ -201,25 +194,9 @@ rcl_publisher_init(
  // context
  publisher->impl->context = node->context;
 
- 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;
+ goto cleanup;
 fail:
- if(options->preallocate){
- if (NULL != publisher->impl->allocation) {
- allocator->deallocate(publisher->impl->allocation, allocator->state);
- }
- }
- if (publisher->impl) {
+ if (publisher->impl) {
  allocator->deallocate(publisher->impl, allocator->state);
  }
 
@@ -257,9 +234,6 @@ 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");
@@ -274,18 +248,19 @@ 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;
 }
 
 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, NULL) != 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;
  }

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