diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 0f734a244..8f036582a 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -245,7 +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 + * \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 @@ -285,6 +285,7 @@ rcl_publish( * * \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 @@ -294,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. /** diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index e161a3051..567c782b2 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -247,7 +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 + * \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 @@ -293,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 @@ -307,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. /** diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index e6a49c2e0..4eec91903 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -173,8 +173,7 @@ rcl_publisher_init( 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 @@ -253,7 +252,8 @@ rcl_publisher_get_default_options() rcl_ret_t rcl_publish( - const rcl_publisher_t * publisher, const void * ros_message, + const rcl_publisher_t * publisher, + const void * ros_message, rmw_publisher_allocation_t * allocation) { if (!rcl_publisher_is_valid(publisher)) { @@ -269,13 +269,16 @@ rcl_publish( 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) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index d1431dc9c..43e2d770e 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -273,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)) { @@ -286,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) { diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index ae97bed64..c02726ecf 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/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; \ diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 114f734f3..271d19563 100644 --- a/rcl_action/src/rcl_action/action_server.c +++ b/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 } diff --git a/rcl_lifecycle/src/com_interface.c b/rcl_lifecycle/src/com_interface.c index 149f4fde7..fc49f8d64 100644 --- a/rcl_lifecycle/src/com_interface.c +++ b/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