From 1eb546711ba437ce350ba6444d1a7e3095e77b3d Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Wed, 6 Feb 2019 18:41:49 -0300 Subject: [PATCH 1/4] Proposola of changes for RMW_Preallocate. Related /ros2/rmw#160 Signed-off-by: Gonzalo de Pedro --- rcl/include/rcl/publisher.h | 1 + rcl/src/rcl/publisher.c | 40 ++++++++++++++++++++++++++++++--- rcl/test/rcl/test_publisher.cpp | 1 + 3 files changed, 39 insertions(+), 3 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 61b89e741..054443b64 100644 --- a/rcl/include/rcl/publisher.h +++ b/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`. diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 4491612b5..32e2b2f60 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -38,6 +38,7 @@ 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 @@ -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,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 @@ -188,11 +200,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: @@ -227,6 +257,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"); @@ -241,6 +274,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; } @@ -251,7 +285,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; } diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index c4c95d479..c6f815c66 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -83,6 +83,7 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin const char * topic_name = "chatter"; const char * expected_topic_name = "/chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.preallocate = true; ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ From fc155f1093b9ef877aa3ce2523d8955e031077a9 Mon Sep 17 00:00:00 2001 From: Gonzalo de Pedro Date: Thu, 14 Feb 2019 10:58:56 -0300 Subject: [PATCH 2/4] Changed RCL interface Signed-off-by: Gonzalo de Pedro --- rcl/include/rcl/publisher.h | 11 ++++++--- rcl/include/rcl/subscription.h | 8 ++++-- rcl/src/rcl/logging_rosout.c | 2 +- rcl/src/rcl/publisher.c | 45 ++++++++-------------------------- rcl/src/rcl/subscription.c | 10 +++++--- 5 files changed, 32 insertions(+), 44 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 054443b64..0f734a244 100644 --- a/rcl/include/rcl/publisher.h +++ b/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. /** diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 6bd3e5d24..e161a3051 100644 --- a/rcl/include/rcl/subscription.h +++ b/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. /** diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 0f4ed0d8e..f7fa8baed 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/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); diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 32e2b2f60..e6a49c2e0 100644 --- a/rcl/src/rcl/publisher.c +++ b/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,14 +166,6 @@ 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 @@ -181,7 +173,8 @@ 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 @@ -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; } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 1d8762d55..d1431dc9c 100644 --- a/rcl/src/rcl/subscription.c +++ b/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) { From f611286c86b79fcee4ce7dc0fecb28826536b18b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 1 May 2019 11:29:50 -0500 Subject: [PATCH 3/4] Updates for allocation in serialize methods. Signed-off-by: Michael Carroll --- rcl/include/rcl/publisher.h | 8 ++++++-- rcl/include/rcl/subscription.h | 6 ++++-- rcl/src/rcl/publisher.c | 13 ++++++++----- rcl/src/rcl/subscription.c | 6 ++++-- rcl_action/src/rcl_action/action_client.c | 2 +- rcl_action/src/rcl_action/action_server.c | 4 ++-- rcl_lifecycle/src/com_interface.c | 2 +- 7 files changed, 26 insertions(+), 15 deletions(-) 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 From 7126b4987f28e26572d5610a237febcb2a4a70c4 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 1 May 2019 11:50:52 -0500 Subject: [PATCH 4/4] Fix tests for new APIs. Signed-off-by: Michael Carroll --- rcl/test/rcl/test_publisher.cpp | 9 ++++----- rcl/test/rcl/test_subscription.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 9 deletions(-) diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index c6f815c66..96d40bf44 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/rcl/test/rcl/test_publisher.cpp @@ -83,7 +83,6 @@ TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nomin const char * topic_name = "chatter"; const char * expected_topic_name = "/chatter"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - publisher_options.preallocate = true; ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -94,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; } @@ -117,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; } @@ -161,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; } diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 261b24eda..03cd5de87 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/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)); }