From b45823a910a18018c25cd272c410da2e9f895882 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Tue, 13 Aug 2019 00:26:00 -0700 Subject: [PATCH 1/9] allocate loaned message Signed-off-by: Karsten Knese --- rcl/include/rcl/publisher.h | 51 ++++++++++++++++++++++- rcl/src/rcl/logging_rosout.c | 2 +- rcl/src/rcl/publisher.c | 38 ++++++++++++++++- rcl/test/rcl/test_events.cpp | 6 +-- rcl/test/rcl/test_publisher.cpp | 8 ++-- rcl/test/rcl/test_subscription.cpp | 4 +- rcl_action/src/rcl_action/action_server.c | 4 +- rcl_lifecycle/src/com_interface.c | 2 +- 8 files changed, 99 insertions(+), 16 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 9f657c504..3ca3956ab 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -196,6 +196,43 @@ RCL_WARN_UNUSED rcl_publisher_options_t rcl_publisher_get_default_options(void); +/// Allocate memory for a loaned message. +/** + * The memory allocated for the ros message belongs to the middleware and must not be deallocated + * other than by a call to \sa rmw_deallocate_loaned_message. + * The given size has to be the exact size of the to be allocated message. + * + * \param[in] publisher Publisher to which the allocated message is associated. + * \param[in] type_support Typesupport to which the internal ros message is allocated. + * \param[in] message_size The amount of memory to be allocated. + * \return pointer to allocated memory. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +void * +rcl_allocate_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + size_t message_size); + +/// Destroy and deallocate a loaned message +/** + * Deallocates and destroys a previously loaned message. + * This function is the only allowed way of deallocating a loaned message. + * + * \param[in] publisher Publisher to which the loaned message is associated. + * \param[in] loaned_message Loaned message to be deallocated and destroyed. + * \return `RMW_RET_OK` if successful, or + * \return `RMW_RET_INVALID_ARGUMENT` if an argument is null, or + * \return `RMW_RET_ERROR` if an unexpected error occurs and no message can be initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_deallocate_loaned_message( + const rcl_publisher_t * publisher, + void * loaned_message); + /// Publish a ROS message on a topic using a publisher. /** * It is the job of the caller to ensure that the type of the ros_message @@ -248,6 +285,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) + * \param[in] is_Loaned flag indicating if ros messages was allocated in the middleware * \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 @@ -259,7 +297,8 @@ rcl_ret_t rcl_publish( const rcl_publisher_t * publisher, const void * ros_message, - rmw_publisher_allocation_t * allocation + rmw_publisher_allocation_t * allocation, + bool is_loaned ); /// Publish a serialized message on a topic using a publisher. @@ -528,6 +567,16 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher); + +/// Check if publisher instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ +RCL_PUBLIC +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 9c92ee9be..ea1b6e453 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -261,7 +261,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, NULL); + status = rcl_publish(&entry.publisher, log_message, NULL, false); 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 8cb7ce835..d51148afc 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -245,17 +245,42 @@ rcl_publisher_get_default_options() return default_options; } +void * +rcl_allocate_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + size_t message_size) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + return rmw_allocate_loaned_message(publisher->impl->rmw_handle, type_support, message_size); +} + +rcl_ret_t +rcl_deallocate_loaned_message( + const rcl_publisher_t * publisher, + void * loaned_message) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rmw_deallocate_loaned_message(publisher->impl->rmw_handle, loaned_message); +} + rcl_ret_t rcl_publish( const rcl_publisher_t * publisher, const void * ros_message, - rmw_publisher_allocation_t * allocation) + rmw_publisher_allocation_t * allocation, + bool is_loaned) { 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, allocation) != RMW_RET_OK) { + if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation, is_loaned) != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return RCL_RET_ERROR; } @@ -390,6 +415,15 @@ rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher) return &publisher->impl->actual_qos; } +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid(publisher)) { + return false; // error message already set + } + return publisher->impl->rmw_handle->can_loan_messages; +} + #ifdef __cplusplus } #endif diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 87321494c..6bce7c3df 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -355,7 +355,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_ 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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -419,7 +419,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis 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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -490,7 +490,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k 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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 96d40bf44..97c939d1a 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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); 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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); 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, nullptr); + ret = rcl_publish(&publisher, &msg_int, nullptr, false); 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, nullptr); + ret = rcl_publish(&publisher_in_namespace, &msg_string, nullptr, false); 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 2463f95a7..ca4224948 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -164,7 +164,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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -215,7 +215,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, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr, false); test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 9d17d170b..4ac003d03 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, NULL); + rcl_ret_t ret = rcl_publish(&action_server->impl->feedback_publisher, ros_feedback, NULL, false); 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, NULL); + rcl_ret_t ret = rcl_publish(&action_server->impl->status_publisher, status_message, NULL, false); 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 fc49f8d64..5a72447ba 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, NULL); + return rcl_publish(&com_interface->pub_transition_event, &msg, NULL, false); } #ifdef __cplusplus From 8adbfd3f1f3491051e6ccb4aa38d4b14880ee4a9 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sun, 15 Sep 2019 16:48:39 -0700 Subject: [PATCH 2/9] can_loan_messages for subscription Signed-off-by: Karsten Knese --- rcl/include/rcl/subscription.h | 9 +++++++++ rcl/src/rcl/publisher.c | 2 +- rcl/src/rcl/subscription.c | 9 +++++++++ 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 7de6ed8ff..414523f89 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -471,6 +471,15 @@ RCL_WARN_UNUSED const rmw_qos_profile_t * rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription); +/// Check if subscription instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ +RCL_PUBLIC +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d51148afc..c70c3ee31 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -252,7 +252,7 @@ rcl_allocate_loaned_message( size_t message_size) { if (!rcl_publisher_is_valid(publisher)) { - return RCL_RET_PUBLISHER_INVALID; // error already set + return NULL; // error already set } return rmw_allocate_loaned_message(publisher->impl->rmw_handle, type_support, message_size); } diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 45aee8389..d0a45da3c 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -378,6 +378,15 @@ rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription) return &subscription->impl->actual_qos; } +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return false; // error message already set + } + return subscription->impl->rmw_handle->can_loan_messages; +} + #ifdef __cplusplus } #endif From 0efd10ea9ced9ebcd024c799dceab074e3a08967 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 30 Sep 2019 00:05:49 -0700 Subject: [PATCH 3/9] first draft of loaned message sequence Signed-off-by: Karsten Knese --- rcl/include/rcl/subscription.h | 71 ++++++++++++++++++++++++++++++++++ rcl/src/rcl/subscription.c | 70 +++++++++++++++++++++++++++++++++ 2 files changed, 141 insertions(+) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 414523f89..e078b4465 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -311,6 +311,77 @@ rcl_take_serialized_message( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation); +/// Take a loaned message sequence from a topic using a rcl subscription. +/** + * Depending on the middleware, incoming messages can be loaned to the user's callback + * without further copying. + * The implicit contract here is that the middleware owns the memory allocated for these messages. + * The user must not destroy the messages, but rather has to return the sequence with a call to + * \sa rcl_return_loaned_message_sequence to the middleware. + * + * The amount of messages to fetch from the middleware is specified by the parameter `n`. + * If less than `n` messages are available, the middleware implementation shall fill the + * sequence with all available messages. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] loaned_message_sequence a pointer to the container for the loaned messages. + * \param[out] message_info_sequence rmw struct which contains meta-data for the messages. + * \param[in] n amount of messages to take from the middleware + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return `RCL_RET_OK` if the loaned message sequence was taken, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error + * occurred in the middleware, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_loaned_message_sequence( + const rcl_subscription_t * subscription, + rmw_loaned_message_sequence_t * loaned_message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t n, + rmw_subscription_allocation_t * allocation); + +/// Return a loaned message sequence from a topic using a rcl subscription. +/** + * If a loaned message sequence was previously obtained from the middleware with a call to + * \sa rcl_take_loaned_message_sequence, this sequence has to be returned to the middleware. + * The user must not delete the sequence. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[in] loaned_message_sequence a pointer to the container for the loaned messages. + * \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 + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_return_loaned_message_sequence( + const rcl_subscription_t * subscription, + rmw_loaned_message_sequence_t * loaned_message_sequence); + /// Get the topic name for the subscription. /** * This function returns the subscription's internal topic name string. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index d0a45da3c..80e778f16 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -310,6 +310,76 @@ rcl_take_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_take_loaned_message_sequence( + const rcl_subscription_t * subscription, + rmw_loaned_message_sequence_t * loaned_message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t n, + rmw_subscription_allocation_t * allocation +) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message sequence"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message_sequence, RCL_RET_INVALID_ARGUMENT); + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_sequence_t dummy_message_info_sequence; + rmw_message_info_sequence_t * message_info_local = + message_info_sequence ? message_info_sequence : &dummy_message_info_sequence; + // Call rmw_take_with_info. + bool taken = false; + rmw_ret_t ret = rmw_take_loaned_message_sequence_with_info( + subscription->impl->rmw_handle, + loaned_message_sequence, + n, + &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) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Subscription loaned message sequence take succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_return_loaned_message_sequence( + const rcl_subscription_t * subscription, + rmw_loaned_message_sequence_t * loaned_message_sequence +) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message sequence"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message_sequence, RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_return_loaned_message_sequence( + subscription->impl->rmw_handle, loaned_message_sequence); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + if (RMW_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Subscription loaned message sequence return succeeded"); + return RCL_RET_OK; +} + const char * rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) { From cfeec8e9f3b9551997bbbe806238d864e778aebd Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Mon, 30 Sep 2019 17:10:25 -0700 Subject: [PATCH 4/9] take loaned message Signed-off-by: Karsten Knese --- rcl/include/rcl/subscription.h | 65 ++++++++++++++++++++++++++++++++++ rcl/src/rcl/subscription.c | 50 ++++++++++++++++++++++++++ 2 files changed, 115 insertions(+) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index e078b4465..941cec03f 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -311,6 +311,71 @@ rcl_take_serialized_message( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation); +/// Take a loaned message from a topic using a rcl subscription. +/** + * Depending on the middleware, incoming messages can be loaned to the user's callback + * without further copying. + * The implicit contract here is that the middleware owns the memory allocated for this message. + * The user must not destroy the message, but rather has to return it with a call to + * \sa rcl_return_loaned_message to the middleware. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] loaned_message a pointer to the loaned messages. + * \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 loaned message sequence was taken, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or + * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error + * occurred in the middleware, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +/// Return a loaned message from a topic using a rcl subscription. +/** + * If a loaned message was previously obtained from the middleware with a call to + * \sa rcl_take_loaned_message, this message has to be returned to the middleware. + * The user must not delete the message. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[in] loaned_message a pointer to the loaned messages. + * \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 + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_return_loaned_message( + const rcl_subscription_t * subscription, + void * loaned_message); + /// Take a loaned message sequence from a topic using a rcl subscription. /** * Depending on the middleware, incoming messages can be loaned to the user's callback diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 80e778f16..7f450c1b0 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -310,6 +310,56 @@ rcl_take_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + if (loaned_message) { + RCL_SET_ERROR_MSG("loaned message is already initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_t dummy_message_info; + rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; + // Call rmw_take_with_info. + bool taken = false; + rmw_ret_t ret = rmw_take_loaned_message_with_info( + subscription->impl->rmw_handle, loaned_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) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription loaned take succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_return_loaned_message( + const rcl_subscription_t * subscription, + void * loaned_message) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription returning loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rmw_return_loaned_message(subscription->impl->rmw_handle, loaned_message); +} + rcl_ret_t rcl_take_loaned_message_sequence( const rcl_subscription_t * subscription, From 48e34015573ce0730cca367de87f93e17ae62624 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Wed, 2 Oct 2019 22:58:35 -0700 Subject: [PATCH 5/9] borrow/return & take/release Signed-off-by: Karsten Knese --- rcl/include/rcl/publisher.h | 51 +++++++++++++++------- rcl/include/rcl/subscription.h | 78 ++-------------------------------- rcl/src/rcl/publisher.c | 14 +++--- rcl/src/rcl/subscription.c | 76 ++------------------------------- 4 files changed, 50 insertions(+), 169 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 3ca3956ab..b71d93265 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -196,40 +196,61 @@ RCL_WARN_UNUSED rcl_publisher_options_t rcl_publisher_get_default_options(void); -/// Allocate memory for a loaned message. +/// Borrow a loaned message. /** * The memory allocated for the ros message belongs to the middleware and must not be deallocated - * other than by a call to \sa rmw_deallocate_loaned_message. - * The given size has to be the exact size of the to be allocated message. + * other than by a call to \sa rcl_return_loaned_message. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [0] the underlying middleware might allocate new memory or returns an existing chunk form a pool. + * The function in rcl however does not allocate any additional memory. * * \param[in] publisher Publisher to which the allocated message is associated. * \param[in] type_support Typesupport to which the internal ros message is allocated. - * \param[in] message_size The amount of memory to be allocated. - * \return pointer to allocated memory. + * \param[out] ros_message The pointer to be filled to a valid ros message by the middleware. + * \return RCL_RET_OK if the ros message was correctly initialized, or + * \return RCL_RET_INVALID_ARGUMENT if an argument other than the ros message is null, or + * \return RCL_RET_BAD_ALLOC if the ros message could not be correctly created, or + * \return RCL_RET_ERROR if an unexpected error occured. */ RCL_PUBLIC RCL_WARN_UNUSED -void * -rcl_allocate_loaned_message( +rcl_ret_t +rcl_borrow_loaned_message( const rcl_publisher_t * publisher, const rosidl_message_type_support_t * type_support, - size_t message_size); + void ** ros_message); -/// Destroy and deallocate a loaned message +/// Return a loaned message /** - * Deallocates and destroys a previously loaned message. - * This function is the only allowed way of deallocating a loaned message. + * The ownership of the passed in ros message will be transferred back to the middleware. + * The middleware might deallocate and destroy the message so that the pointer is no longer + * guarantueed to be valid after that call. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes * * \param[in] publisher Publisher to which the loaned message is associated. * \param[in] loaned_message Loaned message to be deallocated and destroyed. - * \return `RMW_RET_OK` if successful, or - * \return `RMW_RET_INVALID_ARGUMENT` if an argument is null, or - * \return `RMW_RET_ERROR` if an unexpected error occurs and no message can be initialized. + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if an argument is null, or + * \return `RCL_RET_ERROR` if an unexpected error occurs and no message can be initialized. */ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_deallocate_loaned_message( +rcl_return_loaned_message( const rcl_publisher_t * publisher, void * loaned_message); diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 941cec03f..662610493 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -348,10 +348,11 @@ rcl_take_loaned_message( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation); -/// Return a loaned message from a topic using a rcl subscription. +/// Release a loaned message from a topic using a rcl subscription. /** * If a loaned message was previously obtained from the middleware with a call to - * \sa rcl_take_loaned_message, this message has to be returned to the middleware. + * \sa rcl_take_loaned_message, this message has to be released to indicate to the middleware + * that the user no longer needs that memory. * The user must not delete the message. * *
@@ -372,81 +373,10 @@ rcl_take_loaned_message( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_return_loaned_message( +rcl_release_loaned_message( const rcl_subscription_t * subscription, void * loaned_message); -/// Take a loaned message sequence from a topic using a rcl subscription. -/** - * Depending on the middleware, incoming messages can be loaned to the user's callback - * without further copying. - * The implicit contract here is that the middleware owns the memory allocated for these messages. - * The user must not destroy the messages, but rather has to return the sequence with a call to - * \sa rcl_return_loaned_message_sequence to the middleware. - * - * The amount of messages to fetch from the middleware is specified by the parameter `n`. - * If less than `n` messages are available, the middleware implementation shall fill the - * sequence with all available messages. - * - *
- * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | No - * Thread-Safe | No - * Uses Atomics | No - * Lock-Free | Yes - * - * \param[in] subscription the handle to the subscription from which to take - * \param[inout] loaned_message_sequence a pointer to the container for the loaned messages. - * \param[out] message_info_sequence rmw struct which contains meta-data for the messages. - * \param[in] n amount of messages to take from the middleware - * \param[in] allocation structure pointer used for memory preallocation (may be NULL) - * \return `RCL_RET_OK` if the loaned message sequence was taken, or - * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or - * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or - * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or - * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error - * occurred in the middleware, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_take_loaned_message_sequence( - const rcl_subscription_t * subscription, - rmw_loaned_message_sequence_t * loaned_message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t n, - rmw_subscription_allocation_t * allocation); - -/// Return a loaned message sequence from a topic using a rcl subscription. -/** - * If a loaned message sequence was previously obtained from the middleware with a call to - * \sa rcl_take_loaned_message_sequence, this sequence has to be returned to the middleware. - * The user must not delete the sequence. - * - *
- * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | No - * Thread-Safe | No - * Uses Atomics | No - * Lock-Free | Yes - * - * \param[in] subscription the handle to the subscription from which to take - * \param[in] loaned_message_sequence a pointer to the container for the loaned messages. - * \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 - * \return `RCL_RET_ERROR` if an unspecified error occurs. - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_return_loaned_message_sequence( - const rcl_subscription_t * subscription, - rmw_loaned_message_sequence_t * loaned_message_sequence); - /// Get the topic name for the subscription. /** * This function returns the subscription's internal topic name string. diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index c70c3ee31..e0128f5f2 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -245,20 +245,20 @@ rcl_publisher_get_default_options() return default_options; } -void * -rcl_allocate_loaned_message( +rcl_ret_t +rcl_borrow_loaned_message( const rcl_publisher_t * publisher, const rosidl_message_type_support_t * type_support, - size_t message_size) + void ** ros_message) { if (!rcl_publisher_is_valid(publisher)) { - return NULL; // error already set + return RCL_RET_PUBLISHER_INVALID; // error already set } - return rmw_allocate_loaned_message(publisher->impl->rmw_handle, type_support, message_size); + return rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message); } rcl_ret_t -rcl_deallocate_loaned_message( +rcl_return_loaned_message( const rcl_publisher_t * publisher, void * loaned_message) { @@ -266,7 +266,7 @@ rcl_deallocate_loaned_message( return RCL_RET_PUBLISHER_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_deallocate_loaned_message(publisher->impl->rmw_handle, loaned_message); + return rmw_return_loaned_message(publisher->impl->rmw_handle, loaned_message); } rcl_ret_t diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 7f450c1b0..b7cc2329d 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -348,86 +348,16 @@ rcl_take_loaned_message( } rcl_ret_t -rcl_return_loaned_message( +rcl_release_loaned_message( const rcl_subscription_t * subscription, void * loaned_message) { - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription returning loaned message"); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription releasing loaned message"); if (!rcl_subscription_is_valid(subscription)) { return RCL_RET_SUBSCRIPTION_INVALID; // error already set } RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); - return rmw_return_loaned_message(subscription->impl->rmw_handle, loaned_message); -} - -rcl_ret_t -rcl_take_loaned_message_sequence( - const rcl_subscription_t * subscription, - rmw_loaned_message_sequence_t * loaned_message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t n, - rmw_subscription_allocation_t * allocation -) -{ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message sequence"); - if (!rcl_subscription_is_valid(subscription)) { - return RCL_RET_SUBSCRIPTION_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message_sequence, RCL_RET_INVALID_ARGUMENT); - // If message_info is NULL, use a place holder which can be discarded. - rmw_message_info_sequence_t dummy_message_info_sequence; - rmw_message_info_sequence_t * message_info_local = - message_info_sequence ? message_info_sequence : &dummy_message_info_sequence; - // Call rmw_take_with_info. - bool taken = false; - rmw_ret_t ret = rmw_take_loaned_message_sequence_with_info( - subscription->impl->rmw_handle, - loaned_message_sequence, - n, - &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) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, - "Subscription loaned message sequence take succeeded: %s", taken ? "true" : "false"); - if (!taken) { - return RCL_RET_SUBSCRIPTION_TAKE_FAILED; - } - return RCL_RET_OK; -} - -rcl_ret_t -rcl_return_loaned_message_sequence( - const rcl_subscription_t * subscription, - rmw_loaned_message_sequence_t * loaned_message_sequence -) -{ - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message sequence"); - if (!rcl_subscription_is_valid(subscription)) { - return RCL_RET_SUBSCRIPTION_INVALID; // error already set - } - RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message_sequence, RCL_RET_INVALID_ARGUMENT); - - rmw_ret_t ret = rmw_return_loaned_message_sequence( - subscription->impl->rmw_handle, loaned_message_sequence); - if (ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - if (RMW_RET_BAD_ALLOC == ret) { - return RCL_RET_BAD_ALLOC; - } - return RCL_RET_ERROR; - } - RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, - "Subscription loaned message sequence return succeeded"); - return RCL_RET_OK; + return rmw_release_loaned_message(subscription->impl->rmw_handle, loaned_message); } const char * From 96c03a101ce7e686e2038a648521ed31a088ee54 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 5 Oct 2019 11:26:43 -0700 Subject: [PATCH 6/9] introduce rmw_publish_loaned_message Signed-off-by: Karsten Knese --- rcl/include/rcl/publisher.h | 45 +++++++++++++++++++++-- rcl/src/rcl/logging_rosout.c | 2 +- rcl/src/rcl/publisher.c | 23 ++++++++++-- rcl/test/rcl/test_events.cpp | 6 +-- rcl/test/rcl/test_publisher.cpp | 8 ++-- rcl/test/rcl/test_subscription.cpp | 4 +- rcl_action/src/rcl_action/action_server.c | 4 +- rcl_lifecycle/src/com_interface.c | 2 +- 8 files changed, 75 insertions(+), 19 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index b71d93265..4e35c7a1c 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -306,7 +306,6 @@ rcl_return_loaned_message( * \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) - * \param[in] is_Loaned flag indicating if ros messages was allocated in the middleware * \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 @@ -318,8 +317,7 @@ rcl_ret_t rcl_publish( const rcl_publisher_t * publisher, const void * ros_message, - rmw_publisher_allocation_t * allocation, - bool is_loaned + rmw_publisher_allocation_t * allocation ); /// Publish a serialized message on a topic using a publisher. @@ -362,6 +360,47 @@ rcl_publish_serialized_message( rmw_publisher_allocation_t * allocation ); +/// Publish a loaned message on a topic using a publisher. +/** + * A previously borrowed loaned message can be sent via this call to `rcl_publish_loaned_message`. + * By calling this function, the ownership of the loaned message is getting transferred back + * to the middleware. + * The pointer to the `ros_message` is not guarantueed to be valid after as the middleware + * migth deallocate the memory for this message internally. + * It is thus recommended to call this function only in combination with + * \sa `rcl_borrow_loaned_message`. + * + * Apart from this, the `publish_loaned_message` function has the same behavior as `rcl_publish` + * expect that no serialization step is done. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [0] the middleware might deallocate the loaned message. + * The RCL function however does not allocate any memory. + * [1] for unique pairs of publishers and messages, see above for more + * + * \param[in] publisher handle to the publisher which will do the publishing + * \param[in] ros_message pointer to the previously borrow loaned 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 + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publish_loaned_message( + const rcl_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation +); + /// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) /** * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator of diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index ea1b6e453..9c92ee9be 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -261,7 +261,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, NULL, false); + 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 e0128f5f2..d4b7bcd19 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -273,14 +273,13 @@ rcl_ret_t rcl_publish( const rcl_publisher_t * publisher, const void * ros_message, - rmw_publisher_allocation_t * allocation, - bool is_loaned) + 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, allocation, is_loaned) != 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; } @@ -309,6 +308,24 @@ rcl_publish_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_publish_loaned_message( + 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); + rmw_ret_t ret = rmw_publish_loaned_message(publisher->impl->rmw_handle, ros_message, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + rcl_ret_t rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher) { diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index 6bce7c3df..87321494c 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -355,7 +355,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_ 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, nullptr, false); + ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -419,7 +419,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis 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, nullptr, false); + ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } @@ -490,7 +490,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k 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, nullptr, false); + ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; } diff --git a/rcl/test/rcl/test_publisher.cpp b/rcl/test/rcl/test_publisher.cpp index 97c939d1a..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, nullptr, false); + 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, nullptr, false); + 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, nullptr, false); + 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, nullptr, false); + 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 ca4224948..2463f95a7 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -164,7 +164,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, nullptr, false); + ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -215,7 +215,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, nullptr, false); + ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } diff --git a/rcl_action/src/rcl_action/action_server.c b/rcl_action/src/rcl_action/action_server.c index 4ac003d03..9d17d170b 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, NULL, false); + 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, NULL, false); + 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 5a72447ba..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, NULL, false); + return rcl_publish(&com_interface->pub_transition_event, &msg, NULL); } #ifdef __cplusplus From d89b7ad34b33676ea4014c2c081c6bdd69e39982 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Sat, 5 Oct 2019 14:57:44 -0700 Subject: [PATCH 7/9] const correct publish Signed-off-by: Karsten Knese --- rcl/include/rcl/publisher.h | 2 +- rcl/src/rcl/publisher.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 4e35c7a1c..081d04972 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -397,7 +397,7 @@ RCL_WARN_UNUSED rcl_ret_t rcl_publish_loaned_message( const rcl_publisher_t * publisher, - const void * ros_message, + void * ros_message, rmw_publisher_allocation_t * allocation ); diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index d4b7bcd19..3b02be76a 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -311,7 +311,7 @@ rcl_publish_serialized_message( rcl_ret_t rcl_publish_loaned_message( const rcl_publisher_t * publisher, - const void * ros_message, + void * ros_message, rmw_publisher_allocation_t * allocation) { if (!rcl_publisher_is_valid(publisher)) { From 8c03156db7f61a8f773060b8e0ed387ba79c4c3c Mon Sep 17 00:00:00 2001 From: Knese Karsten Date: Sun, 6 Oct 2019 13:36:35 -0700 Subject: [PATCH 8/9] fix typo Signed-off-by: Knese Karsten --- rcl/src/rcl/subscription.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index b7cc2329d..506a5d855 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -321,7 +321,7 @@ rcl_take_loaned_message( if (!rcl_subscription_is_valid(subscription)) { return RCL_RET_SUBSCRIPTION_INVALID; // error already set } - if (loaned_message) { + if (*loaned_message) { RCL_SET_ERROR_MSG("loaned message is already initialized"); return RCL_RET_INVALID_ARGUMENT; } From 13b757046f17a1443b0c0b09d6bdd7b3f2838602 Mon Sep 17 00:00:00 2001 From: Karsten Knese Date: Fri, 18 Oct 2019 10:34:25 -0700 Subject: [PATCH 9/9] address review Signed-off-by: Karsten Knese --- rcl/include/rcl/publisher.h | 17 ++++++++++------- rcl/include/rcl/subscription.h | 2 ++ 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 081d04972..fb7aacc0c 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -214,10 +214,11 @@ rcl_publisher_get_default_options(void); * \param[in] publisher Publisher to which the allocated message is associated. * \param[in] type_support Typesupport to which the internal ros message is allocated. * \param[out] ros_message The pointer to be filled to a valid ros message by the middleware. - * \return RCL_RET_OK if the ros message was correctly initialized, or - * \return RCL_RET_INVALID_ARGUMENT if an argument other than the ros message is null, or - * \return RCL_RET_BAD_ALLOC if the ros message could not be correctly created, or - * \return RCL_RET_ERROR if an unexpected error occured. + * \return `RCL_RET_OK` if the ros message was correctly initialized, or + * \return `RCL_RET_INVALID_ARGUMENT` if an argument other than the ros message is null, or + * \return `RCL_RET_BAD_ALLOC` if the ros message could not be correctly created, or + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or + * \return `RCL_RET_ERROR` if an unexpected error occured. */ RCL_PUBLIC RCL_WARN_UNUSED @@ -231,7 +232,7 @@ rcl_borrow_loaned_message( /** * The ownership of the passed in ros message will be transferred back to the middleware. * The middleware might deallocate and destroy the message so that the pointer is no longer - * guarantueed to be valid after that call. + * guaranteed to be valid after that call. * *
* Attribute | Adherence @@ -245,6 +246,7 @@ rcl_borrow_loaned_message( * \param[in] loaned_message Loaned message to be deallocated and destroyed. * \return `RCL_RET_OK` if successful, or * \return `RCL_RET_INVALID_ARGUMENT` if an argument is null, or + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or * \return `RCL_RET_ERROR` if an unexpected error occurs and no message can be initialized. */ RCL_PUBLIC @@ -365,13 +367,13 @@ rcl_publish_serialized_message( * A previously borrowed loaned message can be sent via this call to `rcl_publish_loaned_message`. * By calling this function, the ownership of the loaned message is getting transferred back * to the middleware. - * The pointer to the `ros_message` is not guarantueed to be valid after as the middleware + * The pointer to the `ros_message` is not guaranteed to be valid after as the middleware * migth deallocate the memory for this message internally. * It is thus recommended to call this function only in combination with * \sa `rcl_borrow_loaned_message`. * * Apart from this, the `publish_loaned_message` function has the same behavior as `rcl_publish` - * expect that no serialization step is done. + * except that no serialization step is done. * *
* Attribute | Adherence @@ -390,6 +392,7 @@ rcl_publish_serialized_message( * \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 + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 662610493..430eea4f7 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -337,6 +337,7 @@ rcl_take_serialized_message( * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or * \return `RCL_RET_SUBSCRIPTION_TAKE_FAILED` if take failed but no error * occurred in the middleware, or + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC @@ -368,6 +369,7 @@ rcl_take_loaned_message( * \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 + * \return `RCL_RET_UNIMPLEMENTED` if the middleware does not support that feature, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC