From 3d48555597892d668aa9c9a482de08232b6bb040 Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 2 May 2019 11:32:13 -0500 Subject: [PATCH] Rmw preallocate (#428) * Proposola of changes for RMW_Preallocate. Related /ros2/rmw#160 Signed-off-by: Gonzalo de Pedro * Changed RCL interface Signed-off-by: Gonzalo de Pedro * Updates for allocation in serialize methods. Signed-off-by: Michael Carroll * Fix tests for new APIs. Signed-off-by: Michael Carroll --- rcl/include/rcl/publisher.h | 16 +++++++++++++--- rcl/include/rcl/subscription.h | 12 +++++++++--- rcl/src/rcl/logging_rosout.c | 2 +- rcl/src/rcl/publisher.c | 22 +++++++++++++++++----- rcl/src/rcl/subscription.c | 16 +++++++++++----- rcl/test/rcl/test_publisher.cpp | 8 ++++---- rcl/test/rcl/test_subscription.cpp | 8 ++++---- 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 +- 10 files changed, 63 insertions(+), 29 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 61b89e741..8f036582a 100644 --- a/rcl/include/rcl/publisher.h +++ b/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. /** diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 6bd3e5d24..567c782b2 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] 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. /** 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 4491612b5..4eec91903 100644 --- a/rcl/src/rcl/publisher.c +++ b/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) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 1d8762d55..43e2d770e 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) { @@ -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) { diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index c4c95d479..96d40bf44 100644 --- a/rcl/test/rcl/test_publisher.cpp +++ b/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; } 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)); } 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