From 8ef3b0d60db1a2bdcce1197dafc4f0b1ad5362e5 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 2 Dec 2020 15:26:40 +0800 Subject: [PATCH 1/4] to support a feature of content filtered topic Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 131 +++++++++++++++++++++ rcl/src/rcl/subscription.c | 175 ++++++++++++++++++++++++++++- rcl/test/rcl/test_subscription.cpp | 107 ++++++++++++++++++ 3 files changed, 411 insertions(+), 2 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index e0d2dce5f..764a4719f 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -25,6 +25,7 @@ extern "C" #include "rcl/macros.h" #include "rcl/node.h" #include "rcl/visibility_control.h" +#include "rcutils/types/string_array.h" #include "rmw/message_sequence.h" @@ -204,6 +205,136 @@ RCL_WARN_UNUSED rcl_subscription_options_t rcl_subscription_get_default_options(void); +/// Copy one rcl_subscription_options_t structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] src The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] dst A rcl_subscription_options_t structure to be copied into. + * \return `RCL_RET_OK` if the structure was copied successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if src is NULL, or + * if src allocator is invalid, or + * if dst is NULL, or + * if dst contains already allocated memory, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_copy( + const rcl_subscription_options_t * src, + rcl_subscription_options_t * dst +); + +/// Reclaim resources held inside rcl_subscription_options_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] option The structure which its resources have to be deallocated. + * \return `RCL_RET_OK` if the memory was successfully freed, or + * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL, or + * if its allocator is invalid and the structure contains initialized memory. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_fini(rcl_subscription_options_t * option); + +/// Check if the content filter topic feature is supported in the subscription. +/** + * Depending on the middleware and whether cft is supported in the subscription. + * this will return true if the middleware can support ContentFilteredTopic in the subscription. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool +rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); + +/// Set the filter expression and expression parameters for the subscription. +/** + * This function will set a filter expression and an array of expression parameters + * for the given subscription, but not to update the original rcl_subscription_options_t + * of subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] implementation defined, check the implementation documentation + * + * \param[in] subscription subscription the subscription object to inspect. + * \param[in] filter_expression An filter expression to set. + * \param[in] expression_parameters Array of expression parameters to set, + * it can be NULL if there is no placeholder in filter_expression. + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `filter_expression` is NULL, or + * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation + * identifier does not match this implementation, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_set_cft_expression_parameters( + const rcl_subscription_t * subscription, + const char * filter_expression, + const rcutils_string_array_t * expression_parameters +); + +/// Retrieve the filter expression of the subscription. +/** + * This function will return an filter expression by the given subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] implementation defined, check the implementation documentation + * + * \param[in] subscription subscription the subscription object to inspect. + * \param[out] filter_expression an filter expression, populated on success. + * It is up to the caller to deallocate the filter expression later on, + * using rcutils_get_default_allocator().deallocate(). + * \param[out] expression_parameters Array of expression parameters, populated on success. + * It is up to the caller to finalize this array later on, using rcutils_string_array_fini(). + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `filter_expression` is NULL, or + * \return `RMW_RET_INVALID_ARGUMENT` if `expression_parameters` is NULL, or + * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation + * identifier does not match this implementation, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_get_cft_expression_parameters( + const rcl_subscription_t * subscription, + char ** filter_expression, + rcutils_string_array_t * expression_parameters +); + /// Take a ROS message from a topic using a rcl subscription. /** * It is the job of the caller to ensure that the type of the ros_message diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index bd00c687b..2a6d210ed 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -24,6 +24,8 @@ extern "C" #include "rcl/error_handling.h" #include "rcl/node.h" #include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types/string_array.h" #include "rmw/error_handling.h" #include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" @@ -92,6 +94,7 @@ rcl_subscription_init( sizeof(rcl_subscription_impl_t), allocator->state); RCL_CHECK_FOR_NULL_WITH_MSG( subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + subscription->impl->options = rcl_subscription_get_default_options(); // Fill out the implemenation struct. // rmw_handle // TODO(wjwwood): pass allocator once supported in rmw api. @@ -116,8 +119,12 @@ rcl_subscription_init( } subscription->impl->actual_qos.avoid_ros_namespace_conventions = options->qos.avoid_ros_namespace_conventions; - // options - subscription->impl->options = *options; + ret = rcl_subscription_options_copy(options, &subscription->impl->options); + if (RCL_RET_OK != ret) { + ret = RCL_RET_ERROR; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACEPOINT( @@ -139,6 +146,12 @@ rcl_subscription_init( } } + ret = rcl_subscription_options_fini(&subscription->impl->options); + if (RCL_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + allocator->deallocate(subscription->impl, allocator->state); subscription->impl = NULL; } @@ -175,6 +188,13 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + rcl_ret_t rcl_ret = rcl_subscription_options_fini(&subscription->impl->options); + if (RCL_RET_OK != rcl_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; + } + allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; } @@ -194,6 +214,157 @@ rcl_subscription_get_default_options() return default_options; } +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_copy( + const rcl_subscription_options_t * src, + rcl_subscription_options_t * dst +) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &src->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + dst->qos = src->qos; + dst->allocator = src->allocator; + // copy rmw_subscription_options_t + dst->rmw_subscription_options.rmw_specific_subscription_payload = + src->rmw_subscription_options.rmw_specific_subscription_payload; + dst->rmw_subscription_options.ignore_local_publications = + src->rmw_subscription_options.ignore_local_publications; + if (src->rmw_subscription_options.filter_expression) { + dst->rmw_subscription_options.filter_expression = + rcutils_strdup(src->rmw_subscription_options.filter_expression, *allocator); + if (!dst->rmw_subscription_options.filter_expression) { + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + } + + if (src->rmw_subscription_options.expression_parameters) { + rcutils_string_array_t * parameters = + (rcutils_string_array_t *)allocator->allocate( + sizeof(rcutils_string_array_t), + allocator->state); + if (!parameters) { + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + + rcutils_ret_t ret = rcutils_string_array_init( + parameters, src->rmw_subscription_options.expression_parameters->size, allocator); + if (RCUTILS_RET_OK != ret) { + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + + for (size_t i = 0; i < src->rmw_subscription_options.expression_parameters->size; ++i) { + char * parameter = rcutils_strdup( + src->rmw_subscription_options.expression_parameters->data[i], *allocator); + if (!parameter) { + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + parameters->data[i] = parameter; + } + + dst->rmw_subscription_options.expression_parameters = parameters; + } + + return RCL_RET_OK; + +clean: + if (RCL_RET_OK != rcl_subscription_options_fini(dst)) { + RCL_SET_ERROR_MSG("Error while finalizing rcl subscription options due to another error"); + } + return ret; +} + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_fini(rcl_subscription_options_t * option) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(option, RCL_RET_INVALID_ARGUMENT); + // fini rmw_subscription_options_t + const rcl_allocator_t * allocator = &option->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (option->rmw_subscription_options.filter_expression) { + allocator->deallocate(option->rmw_subscription_options.filter_expression, allocator->state); + option->rmw_subscription_options.filter_expression = NULL; + } + + if (option->rmw_subscription_options.expression_parameters) { + rcutils_ret_t ret = rcutils_string_array_fini( + option->rmw_subscription_options.expression_parameters); + assert(ret == RCUTILS_RET_OK); + allocator->deallocate(option->rmw_subscription_options.expression_parameters, allocator->state); + option->rmw_subscription_options.expression_parameters = NULL; + } + return RCL_RET_OK; +} + +bool +rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return false; // error message already set + } + return subscription->impl->rmw_handle->is_cft_supported; +} + +rcl_ret_t +rcl_subscription_set_cft_expression_parameters( + const rcl_subscription_t * subscription, + const char * filter_expression, + const rcutils_string_array_t * expression_parameters +) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_subscription_set_cft_expression_parameters( + subscription->impl->rmw_handle, filter_expression, expression_parameters); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_subscription_get_cft_expression_parameters( + const rcl_subscription_t * subscription, + char ** filter_expression, + rcutils_string_array_t * expression_parameters +) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(expression_parameters, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_subscription_get_cft_expression_parameters( + subscription->impl->rmw_handle, filter_expression, expression_parameters); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + return RCL_RET_OK; +} + rcl_ret_t rcl_take( const rcl_subscription_t * subscription, diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 14a1863fd..bc925ca94 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1070,6 +1070,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_options(nullptr)); rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_is_cft_supported(nullptr)); + rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); rcl_reset_error(); @@ -1081,6 +1083,111 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_options(&subscription_zero_init)); rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_is_cft_supported(&subscription_zero_init)); + rcl_reset_error(); +} + +/* Test for all failure modes in subscription rcl_subscription_set_cft_expression_parameters function. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixtureInit, + RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_cft_expression_parameters) { + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_set_cft_expression_parameters(nullptr, nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_set_cft_expression_parameters(&subscription_zero_init, nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_set_cft_expression_parameters(&subscription, nullptr, nullptr)); + rcl_reset_error(); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_UNSUPPORTED); + std::string filter_expression = "(data_ MATCH '0')"; + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_set_cft_expression_parameters( + &subscription, filter_expression.c_str(), + nullptr)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_ERROR); + std::string filter_expression = "(data_ MATCH '0')"; + EXPECT_EQ( + RCL_RET_ERROR, + rcl_subscription_set_cft_expression_parameters( + &subscription, filter_expression.c_str(), + nullptr)); + rcl_reset_error(); + } +} + +/* Test for all failure modes in subscription rcl_subscription_get_cft_expression_parameters function. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixtureInit, + RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_cft_expression_parameters) { + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_cft_expression_parameters(nullptr, nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_cft_expression_parameters(&subscription_zero_init, nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_cft_expression_parameters(&subscription, nullptr, nullptr)); + rcl_reset_error(); + + char * filter_expression = NULL; + rcutils_string_array_t parameters; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_cft_expression_parameters(&subscription, &filter_expression, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_cft_expression_parameters(&subscription, nullptr, ¶meters)); + rcl_reset_error(); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_cft_expression_parameters, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_get_cft_expression_parameters( + &subscription, &filter_expression, + ¶meters)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_cft_expression_parameters, RMW_RET_ERROR); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_subscription_get_cft_expression_parameters( + &subscription, &filter_expression, + ¶meters)); + rcl_reset_error(); + } } TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail) From 13b0ddef2d0a5c43bdef7828ceadd9c9b6e29a10 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 3 Dec 2020 15:58:21 +0800 Subject: [PATCH 2/4] Update function description Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 764a4719f..669a8f096 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -276,9 +276,8 @@ rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); * Thread-Safe | No * Uses Atomics | Maybe [1] * Lock-Free | Maybe [1] - * [1] implementation defined, check the implementation documentation * - * \param[in] subscription subscription the subscription object to inspect. + * \param[in] subscription the subscription object to inspect. * \param[in] filter_expression An filter expression to set. * \param[in] expression_parameters Array of expression parameters to set, * it can be NULL if there is no placeholder in filter_expression. @@ -309,9 +308,8 @@ rcl_subscription_set_cft_expression_parameters( * Thread-Safe | No * Uses Atomics | Maybe [1] * Lock-Free | Maybe [1] - * [1] implementation defined, check the implementation documentation * - * \param[in] subscription subscription the subscription object to inspect. + * \param[in] subscription the subscription object to inspect. * \param[out] filter_expression an filter expression, populated on success. * It is up to the caller to deallocate the filter expression later on, * using rcutils_get_default_allocator().deallocate(). From 6ef26c8133da5e61360db13502be4fef62938723 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 2 Feb 2021 14:28:20 +0800 Subject: [PATCH 3/4] Nit. Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 2 ++ rcl/src/rcl/subscription.c | 11 +++++++---- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 669a8f096..20b48561c 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -286,6 +286,7 @@ rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); * \return `RCL_RET_INVALID_ARGUMENT` if `filter_expression` is NULL, or * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation * identifier does not match this implementation, or + * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC @@ -322,6 +323,7 @@ rcl_subscription_set_cft_expression_parameters( * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation * identifier does not match this implementation, or * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 2a6d210ed..49ba60258 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -254,6 +254,8 @@ rcl_subscription_options_copy( goto clean; } + dst->rmw_subscription_options.expression_parameters = parameters; + rcutils_ret_t ret = rcutils_string_array_init( parameters, src->rmw_subscription_options.expression_parameters->size, allocator); if (RCUTILS_RET_OK != ret) { @@ -270,8 +272,6 @@ rcl_subscription_options_copy( } parameters->data[i] = parameter; } - - dst->rmw_subscription_options.expression_parameters = parameters; } return RCL_RET_OK; @@ -300,7 +300,9 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option) if (option->rmw_subscription_options.expression_parameters) { rcutils_ret_t ret = rcutils_string_array_fini( option->rmw_subscription_options.expression_parameters); - assert(ret == RCUTILS_RET_OK); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini string array.\n"); + } allocator->deallocate(option->rmw_subscription_options.expression_parameters, allocator->state); option->rmw_subscription_options.expression_parameters = NULL; } @@ -311,7 +313,8 @@ bool rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription) { if (!rcl_subscription_is_valid(subscription)) { - return false; // error message already set + rcl_reset_error(); + return false; } return subscription->impl->rmw_handle->is_cft_supported; } From 938876c46c23d9e0ad159e22cc81e47b40484027 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 22 Feb 2021 11:05:04 +0800 Subject: [PATCH 4/4] Update based on review Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 10 ++---- rcl/src/rcl/subscription.c | 51 ++++++++++++++++-------------- rcl/test/rcl/test_subscription.cpp | 4 +-- 3 files changed, 32 insertions(+), 33 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 20b48561c..88a7b69cd 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -245,7 +245,7 @@ rcl_subscription_options_copy( * * \param[in] option The structure which its resources have to be deallocated. * \return `RCL_RET_OK` if the memory was successfully freed, or - * \return `RCL_RET_INVALID_ARGUMENT` if log_levels is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or * if its allocator is invalid and the structure contains initialized memory. */ RCL_PUBLIC @@ -278,14 +278,12 @@ rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); * Lock-Free | Maybe [1] * * \param[in] subscription the subscription object to inspect. - * \param[in] filter_expression An filter expression to set. + * \param[in] filter_expression A filter expression to set. * \param[in] expression_parameters Array of expression parameters to set, * it can be NULL if there is no placeholder in filter_expression. * \return `RCL_RET_OK` if the query was successful, or * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or * \return `RCL_RET_INVALID_ARGUMENT` if `filter_expression` is NULL, or - * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation - * identifier does not match this implementation, or * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ @@ -319,9 +317,7 @@ rcl_subscription_set_cft_expression_parameters( * \return `RCL_RET_OK` if the query was successful, or * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or * \return `RCL_RET_INVALID_ARGUMENT` if `filter_expression` is NULL, or - * \return `RMW_RET_INVALID_ARGUMENT` if `expression_parameters` is NULL, or - * \return `RCL_RET_INCORRECT_RMW_IMPLEMENTATION` if the `node` implementation - * identifier does not match this implementation, or + * \return `RCL_RET_INVALID_ARGUMENT` if `expression_parameters` is NULL, or * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or * \return `RCL_RET_ERROR` if an unspecified error occurs. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 49ba60258..6066b5b50 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -121,7 +121,6 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; ret = rcl_subscription_options_copy(options, &subscription->impl->options); if (RCL_RET_OK != ret) { - ret = RCL_RET_ERROR; goto fail; } @@ -239,38 +238,42 @@ rcl_subscription_options_copy( dst->rmw_subscription_options.filter_expression = rcutils_strdup(src->rmw_subscription_options.filter_expression, *allocator); if (!dst->rmw_subscription_options.filter_expression) { - ret = RCL_RET_BAD_ALLOC; - goto clean; - } - } - - if (src->rmw_subscription_options.expression_parameters) { - rcutils_string_array_t * parameters = - (rcutils_string_array_t *)allocator->allocate( - sizeof(rcutils_string_array_t), - allocator->state); - if (!parameters) { + RMW_SET_ERROR_MSG("failed to allocate memory for filter expression"); ret = RCL_RET_BAD_ALLOC; goto clean; } - dst->rmw_subscription_options.expression_parameters = parameters; + // set expression parameters only if filter expression is valid + if (src->rmw_subscription_options.expression_parameters) { + rcutils_string_array_t * parameters = + (rcutils_string_array_t *)allocator->allocate( + sizeof(rcutils_string_array_t), + allocator->state); + if (!parameters) { + RMW_SET_ERROR_MSG("failed to allocate memory for expression parameters"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } - rcutils_ret_t ret = rcutils_string_array_init( - parameters, src->rmw_subscription_options.expression_parameters->size, allocator); - if (RCUTILS_RET_OK != ret) { - ret = RCL_RET_BAD_ALLOC; - goto clean; - } + dst->rmw_subscription_options.expression_parameters = parameters; - for (size_t i = 0; i < src->rmw_subscription_options.expression_parameters->size; ++i) { - char * parameter = rcutils_strdup( - src->rmw_subscription_options.expression_parameters->data[i], *allocator); - if (!parameter) { + rcutils_ret_t ret = rcutils_string_array_init( + parameters, src->rmw_subscription_options.expression_parameters->size, allocator); + if (RCUTILS_RET_OK != ret) { ret = RCL_RET_BAD_ALLOC; goto clean; } - parameters->data[i] = parameter; + + for (size_t i = 0; i < src->rmw_subscription_options.expression_parameters->size; ++i) { + char * parameter = rcutils_strdup( + src->rmw_subscription_options.expression_parameters->data[i], *allocator); + if (!parameter) { + RMW_SET_ERROR_MSG("failed to allocate memory for expression parameter"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + parameters->data[i] = parameter; + } } } diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index bc925ca94..df030fabc 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1111,7 +1111,7 @@ TEST_F( { auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_UNSUPPORTED); - std::string filter_expression = "(data_ MATCH '0')"; + std::string filter_expression = "data MATCH '0'"; EXPECT_EQ( RMW_RET_UNSUPPORTED, rcl_subscription_set_cft_expression_parameters( @@ -1123,7 +1123,7 @@ TEST_F( { auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_ERROR); - std::string filter_expression = "(data_ MATCH '0')"; + std::string filter_expression = "data MATCH '0'"; EXPECT_EQ( RCL_RET_ERROR, rcl_subscription_set_cft_expression_parameters(