From 5998e52aac7ab1717be6bb050b8bf63b0cee5e20 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 2 Dec 2020 15:26:40 +0800 Subject: [PATCH 01/20] 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 09d6e6bc4..18610a610 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -27,6 +27,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" @@ -208,6 +209,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 fcea1e767..271dc10ca 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. @@ -115,8 +118,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( @@ -138,6 +145,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; } @@ -174,6 +187,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; } @@ -193,6 +213,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 adedd7bb9..c5dbc990a 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1087,6 +1087,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(); @@ -1098,6 +1100,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 a24bc82421a0317fe18e288bf4fe47fc8f08c2fc Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 3 Dec 2020 15:58:21 +0800 Subject: [PATCH 02/20] 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 18610a610..eca89df6d 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -280,9 +280,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. @@ -313,9 +312,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 6c668db517ca5b1caa085cfc24937ff64306a2ad Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 2 Feb 2021 14:28:20 +0800 Subject: [PATCH 03/20] 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 eca89df6d..291245c38 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -290,6 +290,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 @@ -326,6 +327,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 271dc10ca..667b1ff56 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -253,6 +253,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) { @@ -269,8 +271,6 @@ rcl_subscription_options_copy( } parameters->data[i] = parameter; } - - dst->rmw_subscription_options.expression_parameters = parameters; } return RCL_RET_OK; @@ -299,7 +299,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; } @@ -310,7 +312,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 f723e36c41a109a31d6e51ac2d07c76d0ccc8be1 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 22 Feb 2021 11:05:04 +0800 Subject: [PATCH 04/20] 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 291245c38..8e5f51bc1 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -249,7 +249,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 @@ -282,14 +282,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. */ @@ -323,9 +321,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 667b1ff56..efb56fc00 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -120,7 +120,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; } @@ -238,38 +237,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) { + RMW_SET_ERROR_MSG("failed to allocate memory for 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; - } - - 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 c5dbc990a..b2b58c58c 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1128,7 +1128,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( @@ -1140,7 +1140,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( From c21dd52f8f01689f020a0c276c054b72b2391a05 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 Feb 2021 15:27:47 +0800 Subject: [PATCH 05/20] Not to reset error if subscrption is invalid. Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 1 - 1 file changed, 1 deletion(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index efb56fc00..56bbf3a5b 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -315,7 +315,6 @@ bool rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription) { if (!rcl_subscription_is_valid(subscription)) { - rcl_reset_error(); return false; } return subscription->impl->rmw_handle->is_cft_supported; From 9d5b62ecbbf13cbcf048a8deaa0d916f53e3affd Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 Feb 2021 18:30:58 +0800 Subject: [PATCH 06/20] remove copy function for subscription_options Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 28 ------------ rcl/src/rcl/subscription.c | 78 +--------------------------------- 2 files changed, 1 insertion(+), 105 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 8e5f51bc1..4e605650c 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -209,34 +209,6 @@ 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. /** *
diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 56bbf3a5b..ec6e40f3b 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -118,10 +118,7 @@ rcl_subscription_init( } subscription->impl->actual_qos.avoid_ros_namespace_conventions = options->qos.avoid_ros_namespace_conventions; - ret = rcl_subscription_options_copy(options, &subscription->impl->options); - if (RCL_RET_OK != ret) { - goto fail; - } + subscription->impl->options = *options; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -212,79 +209,6 @@ 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) { - RMW_SET_ERROR_MSG("failed to allocate memory for filter expression"); - ret = RCL_RET_BAD_ALLOC; - goto clean; - } - - // 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; - } - - 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) { - 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) { - RMW_SET_ERROR_MSG("failed to allocate memory for expression parameter"); - ret = RCL_RET_BAD_ALLOC; - goto clean; - } - parameters->data[i] = parameter; - } - } - } - - 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 From ba6f619ad93ef198a106b08d8f939589640e58db Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 25 Feb 2021 17:07:51 +0800 Subject: [PATCH 07/20] update comments to make linelength <= 100 Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index b2b58c58c..ba74e73fe 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1104,7 +1104,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip rcl_reset_error(); } -/* Test for all failure modes in subscription rcl_subscription_set_cft_expression_parameters function. +/* Test for all failure modes in rcl_subscription_set_cft_expression_parameters function. */ TEST_F( CLASSNAME( @@ -1150,7 +1150,7 @@ TEST_F( } } -/* Test for all failure modes in subscription rcl_subscription_get_cft_expression_parameters function. +/* Test for all failure modes in rcl_subscription_get_cft_expression_parameters function. */ TEST_F( CLASSNAME( From c6fb320f0a5265b5068d9ec996fa602963ba43f4 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 11 Mar 2021 16:53:12 +0800 Subject: [PATCH 08/20] Update comments Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 4e605650c..1a82444d6 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -229,7 +229,7 @@ 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. +/// Check if the content filtered 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. @@ -254,9 +254,15 @@ 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 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. + * \param[in] filter_expression A filter_expression is a string that specifies the criteria + * to select the data samples of interest. It is similar to the WHERE part of an SQL clause. + * Using an empty("") string can reset/clean content filtered topic for the subscription. + * \param[in] expression_parameters An expression_parameters is an array of strings that + * give values to the ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * The number of supplied parameters must fit with the requested values in the filter_expression. + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * The maximun size allowance depends on concrete DDS vendor. + * (i.e., it cannot be greater than 100 on RTI_Connext.) * \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 From 1048602ecb14a2735d2522679d35862dc15865de Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 12 Mar 2021 13:09:39 +0800 Subject: [PATCH 09/20] add test for filter data by all cft interfaces with rcl_take Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 383 +++++++++++++++++++++++++++++ 1 file changed, 383 insertions(+) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index ba74e73fe..ea098a15e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -23,6 +23,7 @@ #include "rmw/rmw.h" #include "rmw/validate_full_topic_name.h" +#include "rcutils/strdup.h" #include "rcutils/testing/fault_injection.h" #include "test_msgs/msg/basic_types.h" #include "test_msgs/msg/strings.h" @@ -847,6 +848,388 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_return_l rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str; } + +/* A subscription with a content filtered topic setting. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_content_filtered) { + bool is_vendor_support_cft + = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); + + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_test_subscription_content_filtered_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.rmw_subscription_options.filter_expression + = rcutils_strdup("string_value MATCH 'FilteredData'", subscription_options.allocator); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + if (is_vendor_support_cft) { + ASSERT_TRUE(rcl_subscription_is_cft_supported(&subscription)); + } else { + ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + } + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + constexpr char test_string[] = "NotFilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_vendor_support_cft) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + 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)); + } + + constexpr char test_filtered_string[] = "FilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // set filter + { + rcutils_string_array_t expression_parameters; + expression_parameters = rcutils_get_zero_initialized_string_array(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &expression_parameters, 1, &allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_ret) << rcl_get_error_string().str; + expression_parameters.data[0] = rcutils_strdup("'FilteredOtherData'", allocator); + + ret = rcl_subscription_set_cft_expression_parameters( + &subscription, "string_value MATCH %0", &expression_parameters); + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + rcutils_ret = rcutils_string_array_fini(&expression_parameters); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_ret) << rcl_get_error_string().str; + } + + // publish FilteredData again + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_vendor_support_cft) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + constexpr char test_filtered_other_string[] = "FilteredOtherData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_other_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_other_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // get filter + char * filter_expression = nullptr; + rcutils_string_array_t expression_parameters; + expression_parameters = rcutils_get_zero_initialized_string_array(); + ret = rcl_subscription_get_cft_expression_parameters( + &subscription, &filter_expression, &expression_parameters); + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string("string_value MATCH %0"), + std::string(filter_expression)); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + allocator.deallocate(filter_expression, allocator.state); + ASSERT_EQ(static_cast(1), expression_parameters.size); + ASSERT_EQ( + std::string("'FilteredOtherData'"), + std::string(expression_parameters.data[0])); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&expression_parameters)); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + // reset filter + { + ret = rcl_subscription_set_cft_expression_parameters( + &subscription, "", NULL); + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(wait_for_established_subscription(&publisher, 50, 100)); + ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + } + + // publish no filtered data again + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // it might take 'FilteredData' again, if so, contine to take data + std::string data = std::string(msg.string_value.data, msg.string_value.size); + if (is_vendor_support_cft) { + const int try_total_num = 3; + int i = 0; + while (data != test_string && i++ < try_total_num) { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + data = std::string(msg.string_value.data, msg.string_value.size); + } + } + ASSERT_EQ(std::string(test_string), data); + } +} + +/* A subscription without a content filtered topic setting at beginning. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_begin_content_filtered) { + bool is_vendor_support_cft + = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); + + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_test_subscription_not_begin_content_filtered_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + // not to set filter expression + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + + // publish no filtered data + constexpr char test_string[] = "NotFilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + 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)); + } + + // set filter + { + ret = rcl_subscription_set_cft_expression_parameters( + &subscription, "string_value MATCH 'FilteredData'", NULL); + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + } + + if (is_vendor_support_cft) { + ASSERT_TRUE(rcl_subscription_is_cft_supported(&subscription)); + } else { + ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + } + // publish no filtered data again + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_vendor_support_cft) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + 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)); + } + + // publish filtered data + constexpr char test_filtered_string[] = "FilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } +} + TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { rcl_ret_t ret; const rosidl_message_type_support_t * ts = From ece3bdc2be851d7d0193666b22d5b31382503785 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 11 Oct 2021 14:40:39 +0800 Subject: [PATCH 10/20] update interface Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 123 ++++++++++++----- rcl/src/rcl/subscription.c | 215 +++++++++++++++++++++++++---- rcl/test/CMakeLists.txt | 6 + rcl/test/rcl/test_subscription.cpp | 208 ++++++++++++++++++---------- 4 files changed, 420 insertions(+), 132 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 1a82444d6..d91dabc30 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -27,7 +27,6 @@ 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" @@ -53,6 +52,16 @@ typedef struct rcl_subscription_options_s rmw_subscription_options_t rmw_subscription_options; } rcl_subscription_options_t; +typedef struct rcl_subscription_content_filtered_topic_options_s +{ + /// Custom allocator for the subscription, used for incidental allocations. + /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ + rcl_allocator_t allocator; + /// rmw specific subscription content filtered topic options + rmw_subscription_content_filtered_topic_options_t * + rmw_subscription_content_filtered_topic_options; +} rcl_subscription_content_filtered_topic_options_t; + /// Return a rcl_subscription_t struct with members set to `NULL`. /** * Should be called to get a null rcl_subscription_t before passing to @@ -209,6 +218,69 @@ RCL_WARN_UNUSED rcl_subscription_options_t rcl_subscription_get_default_options(void); +// TODO. comments +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_fini(rcl_subscription_options_t * option); + +// TODO. comments +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_set_content_filtered_topic_options( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_options_t * options); + +/// Return the default subscription content filtered topic options. +/** + * The defaults are: + * + * - allocator = rcl_get_default_allocator() + * - rmw_subscription_content_filtered_topic_options = NULL; + * + * \return A structure containing the default options for a subscription. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_subscription_content_filtered_topic_options_t +rcl_subscription_get_default_content_filtered_topic_options(void); + +/// Allocate. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * 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 option is NULL, or + * if its allocator is invalid and the structure contains initialized memory. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_content_filtered_topic_options_init( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filtered_topic_options_t * options); + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_content_filtered_topic_options_set( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filtered_topic_options_t * options); + /// Reclaim resources held inside rcl_subscription_options_t structure. /** *
@@ -227,23 +299,25 @@ rcl_subscription_get_default_options(void); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_options_fini(rcl_subscription_options_t * option); +rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filtered_topic_options_t * options); -/// Check if the content filtered topic feature is supported in the subscription. +/// Check if the content filtered topic feature is enabled 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. + * Depending on the middleware and whether cft is enabled in the subscription. + * + * It will return true if the middleware can support ContentFilteredTopic in the subscription + * and the cft is enabled. */ RCL_PUBLIC RCL_WARN_UNUSED bool -rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); +rcl_subscription_is_cft_enabled(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. + * for the given subscription. * *
* Attribute | Adherence @@ -253,19 +327,11 @@ rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription); * Uses Atomics | Maybe [1] * Lock-Free | Maybe [1] * - * \param[in] subscription the subscription object to inspect. - * \param[in] filter_expression A filter_expression is a string that specifies the criteria - * to select the data samples of interest. It is similar to the WHERE part of an SQL clause. - * Using an empty("") string can reset/clean content filtered topic for the subscription. - * \param[in] expression_parameters An expression_parameters is an array of strings that - * give values to the ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. - * The number of supplied parameters must fit with the requested values in the filter_expression. - * It can be NULL if there is no "%n" tokens placeholder in filter_expression. - * The maximun size allowance depends on concrete DDS vendor. - * (i.e., it cannot be greater than 100 on RTI_Connext.) + * \param[in] subscription The subscription to set content filtered topic options. + * \param[in] options The rcl content filtered topic options. * \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_INVALID_ARGUMENT` if `rcl_content_filtered_topic_options` is NULL, or * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ @@ -274,8 +340,7 @@ 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 + const rcl_subscription_content_filtered_topic_options_t * options ); /// Retrieve the filter expression of the subscription. @@ -290,16 +355,13 @@ rcl_subscription_set_cft_expression_parameters( * Uses Atomics | Maybe [1] * Lock-Free | Maybe [1] * - * \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(). - * \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(). + * \param[in] subscription The subscription object to inspect. + * \param[out] options The rcl content filtered topic options. + * It is up to the caller to finalize this options later on, using + * rcl_content_filtered_topic_options_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 `RCL_RET_INVALID_ARGUMENT` if `expression_parameters` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `options` 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. @@ -309,8 +371,7 @@ 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 + rcl_subscription_content_filtered_topic_options_t * options ); /// Take a ROS message from a topic using a rcl subscription. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index ec6e40f3b..8f3499628 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -27,6 +27,7 @@ extern "C" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" #include "rmw/error_handling.h" +#include "rmw/subscription_content_filtered_topic_options.h" #include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" @@ -90,11 +91,10 @@ rcl_subscription_init( ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); // Allocate memory for the implementation struct. - subscription->impl = (rcl_subscription_impl_t *)allocator->allocate( - sizeof(rcl_subscription_impl_t), allocator->state); + subscription->impl = (rcl_subscription_impl_t *)allocator->zero_allocate( + 1, 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. @@ -118,6 +118,7 @@ rcl_subscription_init( } subscription->impl->actual_qos.avoid_ros_namespace_conventions = options->qos.avoid_ros_namespace_conventions; + // options subscription->impl->options = *options; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); @@ -209,8 +210,6 @@ rcl_subscription_get_default_options() return default_options; } -RCL_PUBLIC -RCL_WARN_UNUSED rcl_ret_t rcl_subscription_options_fini(rcl_subscription_options_t * option) { @@ -218,37 +217,186 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option) // 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); + if (option->rmw_subscription_options.content_filtered_topic_options) { + rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_fini( + option->rmw_subscription_options.content_filtered_topic_options, allocator); if (RCUTILS_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini string array.\n"); + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini content filtered topic options.\n"); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + allocator->deallocate( + option->rmw_subscription_options.content_filtered_topic_options, allocator->state); + option->rmw_subscription_options.content_filtered_topic_options = NULL; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_subscription_options_set_content_filtered_topic_options( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + allocator->allocate( + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + if (!content_filtered_topic_options) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + + *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + + rmw_ret_t rmw_ret = rmw_subscription_content_filtered_topic_options_set( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + content_filtered_topic_options + ); + + if (rmw_ret != RMW_RET_OK) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto failed; + } + + if (options->rmw_subscription_options.content_filtered_topic_options) { + rmw_ret = rmw_subscription_content_filtered_topic_options_fini( + options->rmw_subscription_options.content_filtered_topic_options, + allocator + ); + + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } - allocator->deallocate(option->rmw_subscription_options.expression_parameters, allocator->state); - option->rmw_subscription_options.expression_parameters = NULL; + + allocator->deallocate( + options->rmw_subscription_options.content_filtered_topic_options, allocator->state); + options->rmw_subscription_options.content_filtered_topic_options = NULL; + } + options->rmw_subscription_options.content_filtered_topic_options = content_filtered_topic_options; + + return RCL_RET_OK; + +failed: + allocator->deallocate(content_filtered_topic_options, allocator->state); + return ret; +} + +rcl_subscription_content_filtered_topic_options_t +rcl_subscription_get_default_content_filtered_topic_options() +{ + static rcl_subscription_content_filtered_topic_options_t default_options; + default_options.allocator = rcl_get_default_allocator(); + default_options.rmw_subscription_content_filtered_topic_options = NULL; + return default_options; +} + +rcl_ret_t +rcl_subscription_content_filtered_topic_options_init( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filtered_topic_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + allocator->allocate( + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + if (!content_filtered_topic_options) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + + rmw_ret_t rmw_ret = rmw_subscription_content_filtered_topic_options_init( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + content_filtered_topic_options + ); + if (rmw_ret != RMW_RET_OK) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto failed; + } + options->rmw_subscription_content_filtered_topic_options = content_filtered_topic_options; + + return RCL_RET_OK; + +failed: + allocator->deallocate(content_filtered_topic_options, allocator->state); + return ret; +} + +rcl_ret_t +rcl_subscription_content_filtered_topic_options_set( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filtered_topic_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_set( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + options->rmw_subscription_content_filtered_topic_options + ); + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +rcl_ret_t +rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filtered_topic_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_fini( + options->rmw_subscription_content_filtered_topic_options, + allocator + ); + + if (ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(ret); } + + allocator->deallocate(options->rmw_subscription_content_filtered_topic_options, allocator->state); return RCL_RET_OK; } bool -rcl_subscription_is_cft_supported(const rcl_subscription_t * subscription) +rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription) { if (!rcl_subscription_is_valid(subscription)) { return false; } - return subscription->impl->rmw_handle->is_cft_supported; + return subscription->impl->rmw_handle->is_cft_enabled; } 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 + const rcl_subscription_content_filtered_topic_options_t * options ) { RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); @@ -257,22 +405,37 @@ rcl_subscription_set_cft_expression_parameters( if (!rcl_subscription_is_valid(subscription)) { return RCL_RET_SUBSCRIPTION_INVALID; } - RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + + rcl_allocator_t * allocator = (rcl_allocator_t *)&subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); rmw_ret_t ret = rmw_subscription_set_cft_expression_parameters( - subscription->impl->rmw_handle, filter_expression, expression_parameters); + subscription->impl->rmw_handle, + options->rmw_subscription_content_filtered_topic_options); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); return rcl_convert_rmw_ret_to_rcl_ret(ret); } + + // copy options into subscription_options + ret = rmw_subscription_content_filtered_topic_options_copy( + options->rmw_subscription_content_filtered_topic_options, + allocator, + subscription->impl->options.rmw_subscription_options.content_filtered_topic_options); + if (RMW_RET_OK != ret) { + 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 + rcl_subscription_content_filtered_topic_options_t * options ) { RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); @@ -281,10 +444,10 @@ rcl_subscription_get_cft_expression_parameters( 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); + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); rmw_ret_t ret = rmw_subscription_get_cft_expression_parameters( - subscription->impl->rmw_handle, filter_expression, expression_parameters); + subscription->impl->rmw_handle, + &options->allocator, options->rmw_subscription_content_filtered_topic_options); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 6ce895f04..5d0d909f8 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -433,3 +433,9 @@ rcl_add_custom_gtest(test_log_level LIBRARIES ${PROJECT_NAME} mimick AMENT_DEPENDENCIES "osrf_testing_tools_cpp" ) + +rcl_add_custom_gtest(test_subscription_content_filtered_topic_options + SRCS rcl/test_subscription_content_filtered_topic_options.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} +) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index ea098a15e..15f6c6e80 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -858,6 +858,7 @@ TEST_F( bool is_vendor_support_cft = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); + const char * filter_expression1 = "string_value MATCH 'FilteredData'"; rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = @@ -873,8 +874,13 @@ TEST_F( }); rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - subscription_options.rmw_subscription_options.filter_expression - = rcutils_strdup("string_value MATCH 'FilteredData'", subscription_options.allocator); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filtered_topic_options( + filter_expression1, 0, nullptr, &subscription_options) + ); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -883,9 +889,9 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); if (is_vendor_support_cft) { - ASSERT_TRUE(rcl_subscription_is_cft_supported(&subscription)); + ASSERT_TRUE(rcl_subscription_is_cft_enabled(&subscription)); } else { - ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); constexpr char test_string[] = "NotFilteredData"; @@ -943,25 +949,33 @@ TEST_F( } // set filter + const char * filter_expression2 = "string_value MATCH %0"; + const char * expression_parameters2[] = {"'FilteredOtherData'"}; + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char*); { - rcutils_string_array_t expression_parameters; - expression_parameters = rcutils_get_zero_initialized_string_array(); - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &expression_parameters, 1, &allocator); - ASSERT_EQ(RCUTILS_RET_OK, rcutils_ret) << rcl_get_error_string().str; - expression_parameters.data[0] = rcutils_strdup("'FilteredOtherData'", allocator); + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); ret = rcl_subscription_set_cft_expression_parameters( - &subscription, "string_value MATCH %0", &expression_parameters); + &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } - rcutils_ret = rcutils_string_array_fini(&expression_parameters); - ASSERT_EQ(RCUTILS_RET_OK, rcutils_ret) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &options) + ); } // publish FilteredData again @@ -1019,38 +1033,64 @@ TEST_F( } // get filter - char * filter_expression = nullptr; - rcutils_string_array_t expression_parameters; - expression_parameters = rcutils_get_zero_initialized_string_array(); - ret = rcl_subscription_get_cft_expression_parameters( - &subscription, &filter_expression, &expression_parameters); - if (is_vendor_support_cft) { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ( - std::string("string_value MATCH %0"), - std::string(filter_expression)); - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - allocator.deallocate(filter_expression, allocator.state); - ASSERT_EQ(static_cast(1), expression_parameters.size); - ASSERT_EQ( - std::string("'FilteredOtherData'"), - std::string(expression_parameters.data[0])); - ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&expression_parameters)); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + { + rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = + rcl_subscription_get_default_content_filtered_topic_options(); + + ret = rcl_subscription_get_cft_expression_parameters( + &subscription, &content_filtered_topic_options); + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rmw_subscription_content_filtered_topic_options_t * options = + content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; + ASSERT_NE(nullptr, options); + ASSERT_STREQ(filter_expression2, options->filter_expression); + ASSERT_NE(nullptr, options->expression_parameters); + ASSERT_EQ(expression_parameters2_count, options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters2_count; ++i) { + EXPECT_STREQ( + options->expression_parameters->data[i], + expression_parameters2[i]); + } + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &content_filtered_topic_options) + ); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } } + // reset filter { + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + "", 0, nullptr, + &options) + ); + ret = rcl_subscription_set_cft_expression_parameters( - &subscription, "", NULL); + &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ASSERT_TRUE(wait_for_established_subscription(&publisher, 50, 100)); - ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &options) + ); } // publish no filtered data again @@ -1101,7 +1141,7 @@ TEST_F( */ TEST_F( CLASSNAME( - TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_begin_content_filtered) { + TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { bool is_vendor_support_cft = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); @@ -1128,7 +1168,7 @@ TEST_F( rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); // publish no filtered data @@ -1159,21 +1199,35 @@ TEST_F( } // set filter + const char * filter_expression2 = "string_value MATCH %0"; + const char * expression_parameters2[] = {"'FilteredData'"}; + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char*); { + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); + ret = rcl_subscription_set_cft_expression_parameters( - &subscription, "string_value MATCH 'FilteredData'", NULL); + &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } - } - if (is_vendor_support_cft) { - ASSERT_TRUE(rcl_subscription_is_cft_supported(&subscription)); - } else { - ASSERT_FALSE(rcl_subscription_is_cft_supported(&subscription)); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &options) + ); } + // publish no filtered data again { test_msgs__msg__Strings msg; @@ -1470,7 +1524,7 @@ 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)); + EXPECT_FALSE(rcl_subscription_is_cft_enabled(nullptr)); rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); @@ -1483,7 +1537,7 @@ 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)); + EXPECT_FALSE(rcl_subscription_is_cft_enabled(&subscription_zero_init)); rcl_reset_error(); } @@ -1495,40 +1549,56 @@ TEST_F( 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_subscription_set_cft_expression_parameters(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_subscription_set_cft_expression_parameters(&subscription_zero_init, nullptr, nullptr)); + rcl_subscription_set_cft_expression_parameters(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, - rcl_subscription_set_cft_expression_parameters(&subscription, nullptr, nullptr)); + rcl_subscription_set_cft_expression_parameters(&subscription, nullptr)); rcl_reset_error(); + // an options used later + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + "data MATCH '0'", + 0, + nullptr, + &options + ) + ); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini(&options) + ); + }); + { 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)); + &subscription, &options)); 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, + RMW_RET_ERROR, rcl_subscription_set_cft_expression_parameters( - &subscription, filter_expression.c_str(), - nullptr)); + &subscription, &options)); rcl_reset_error(); } } @@ -1541,31 +1611,21 @@ TEST_F( 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_subscription_get_cft_expression_parameters(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_subscription_get_cft_expression_parameters(&subscription_zero_init, 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_subscription_get_cft_expression_parameters(&subscription, nullptr)); rcl_reset_error(); - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_get_cft_expression_parameters(&subscription, nullptr, ¶meters)); - rcl_reset_error(); + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); { auto mock = mocking_utils::patch_and_return( @@ -1573,8 +1633,7 @@ TEST_F( EXPECT_EQ( RMW_RET_UNSUPPORTED, rcl_subscription_get_cft_expression_parameters( - &subscription, &filter_expression, - ¶meters)); + &subscription, &options)); rcl_reset_error(); } @@ -1582,10 +1641,9 @@ TEST_F( auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_get_cft_expression_parameters, RMW_RET_ERROR); EXPECT_EQ( - RCL_RET_ERROR, + RMW_RET_ERROR, rcl_subscription_get_cft_expression_parameters( - &subscription, &filter_expression, - ¶meters)); + &subscription, &options)); rcl_reset_error(); } } From 13966e169ec9148282b10274e0248bc915955dac Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 13 Oct 2021 17:52:16 +0800 Subject: [PATCH 11/20] update test Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 20 +++++++++-- rcl/src/rcl/subscription.c | 57 ++++++++++++++++++++++-------- rcl/test/rcl/test_subscription.cpp | 38 +++++++++++--------- 3 files changed, 80 insertions(+), 35 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index d91dabc30..0a3a18df6 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -218,7 +218,21 @@ RCL_WARN_UNUSED rcl_subscription_options_t rcl_subscription_get_default_options(void); -// TODO. comments +/// 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 option is NULL, or + * if its allocator is invalid and the structure contains initialized memory. + */ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t @@ -281,7 +295,7 @@ rcl_subscription_content_filtered_topic_options_set( const char * expression_parameter_argv[], rcl_subscription_content_filtered_topic_options_t * options); -/// Reclaim resources held inside rcl_subscription_options_t structure. +/// Reclaim rcl_subscription_content_filtered_topic_options_t structure. /** *
* Attribute | Adherence @@ -291,7 +305,7 @@ rcl_subscription_content_filtered_topic_options_set( * Uses Atomics | No * Lock-Free | Yes * - * \param[in] option The structure which its resources have to be deallocated. + * \param[in] options 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 option is NULL, or * if its allocator is invalid and the structure contains initialized memory. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 8f3499628..7132ec12e 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -420,16 +420,21 @@ rcl_subscription_set_cft_expression_parameters( } // copy options into subscription_options - ret = rmw_subscription_content_filtered_topic_options_copy( - options->rmw_subscription_content_filtered_topic_options, - allocator, - subscription->impl->options.rmw_subscription_options.content_filtered_topic_options); - if (RMW_RET_OK != ret) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return rcl_convert_rmw_ret_to_rcl_ret(ret); - } - - return RCL_RET_OK; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + options->rmw_subscription_content_filtered_topic_options; + size_t expression_parameters_size = 0; + const char ** expression_parameters = NULL; + if (content_filtered_topic_options->expression_parameters) { + expression_parameters_size = content_filtered_topic_options->expression_parameters->size; + expression_parameters = + (const char **)content_filtered_topic_options->expression_parameters->data; + } + return rcl_subscription_options_set_content_filtered_topic_options( + content_filtered_topic_options->filter_expression, + expression_parameters_size, + expression_parameters, + &subscription->impl->options + ); } rcl_ret_t @@ -445,15 +450,37 @@ rcl_subscription_get_cft_expression_parameters( return RCL_RET_SUBSCRIPTION_INVALID; } RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - rmw_ret_t ret = rmw_subscription_get_cft_expression_parameters( + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + allocator->allocate( + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + if (!content_filtered_topic_options) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + + rmw_ret_t rmw_ret = rmw_subscription_get_cft_expression_parameters( subscription->impl->rmw_handle, - &options->allocator, options->rmw_subscription_content_filtered_topic_options); + &options->allocator, content_filtered_topic_options); - if (ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return rcl_convert_rmw_ret_to_rcl_ret(ret); + if (rmw_ret != RMW_RET_OK) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto failed; } + + options->rmw_subscription_content_filtered_topic_options = content_filtered_topic_options; + return RCL_RET_OK; + +failed: + allocator->deallocate(content_filtered_topic_options, allocator->state); + return ret; + + } rcl_ret_t diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 15f6c6e80..28af04fbf 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -856,7 +856,9 @@ TEST_F( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_content_filtered) { bool is_vendor_support_cft - = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); + = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 + ); const char * filter_expression1 = "string_value MATCH 'FilteredData'"; rcl_ret_t ret; @@ -893,7 +895,7 @@ TEST_F( } else { ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } - ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); constexpr char test_string[] = "NotFilteredData"; { test_msgs__msg__Strings msg; @@ -905,9 +907,9 @@ TEST_F( } if (is_vendor_support_cft) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -932,7 +934,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { test_msgs__msg__Strings msg; @@ -989,9 +991,9 @@ TEST_F( } if (is_vendor_support_cft) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -1016,7 +1018,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { test_msgs__msg__Strings msg; @@ -1080,7 +1082,7 @@ TEST_F( &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(wait_for_established_subscription(&publisher, 50, 100)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); @@ -1103,7 +1105,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { test_msgs__msg__Strings msg; @@ -1121,7 +1123,7 @@ TEST_F( const int try_total_num = 3; int i = 0; while (data != test_string && i++ < try_total_num) { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -1143,7 +1145,9 @@ TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { bool is_vendor_support_cft - = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0); + = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 + ); rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); @@ -1169,7 +1173,7 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); - ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); // publish no filtered data constexpr char test_string[] = "NotFilteredData"; @@ -1182,7 +1186,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { test_msgs__msg__Strings msg; @@ -1239,9 +1243,9 @@ TEST_F( } if (is_vendor_support_cft) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -1267,7 +1271,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { test_msgs__msg__Strings msg; From ee7b0d5b0c9a91f17a65cf69ae2575501422fffd Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 09:36:23 +0800 Subject: [PATCH 12/20] add lost test file Signed-off-by: Chen Lihui --- ...ription_content_filtered_topic_options.cpp | 267 ++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp diff --git a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp new file mode 100644 index 000000000..d18a5738d --- /dev/null +++ b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp @@ -0,0 +1,267 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/error_handling.h" +#include "rcl/subscription.h" + +TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_failure) { + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filtered_topic_options( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filtered_topic_options( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filtered_topic_options( + filter_expression1, 1, nullptr, &subscription_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_fini( + nullptr) + ); + rcl_reset_error(); +} + +TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) +{ + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filtered_topic_options( + filter_expression1, 0, nullptr, &subscription_options) + ); + + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options + = subscription_options.rmw_subscription_options.content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression1, content_filtered_topic_options->filter_expression); + EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "p1", "p2", "q1", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filtered_topic_options( + filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_options) + ); + + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options + = subscription_options.rmw_subscription_options.content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); + ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); + ASSERT_EQ(expression_parameters_count2, + content_filtered_topic_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filtered_topic_options->expression_parameters->data[i], + expression_parameters2[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_fini(&subscription_options) + ); +} + +TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options_failure) { + rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = + rcl_subscription_get_default_content_filtered_topic_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_init( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_init( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_init( + filter_expression1, 1, nullptr, &content_filtered_topic_options) + ); + rcl_reset_error(); + + // set + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_set( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_set( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_set( + filter_expression1, 1, nullptr, &content_filtered_topic_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filtered_topic_options_fini( + nullptr) + ); + rcl_reset_error(); +} + +TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options_success) +{ + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options; + const char * filter_expression1 = "filter=1"; + const char * filter_expression1_update = "filter=2"; + + rcl_subscription_content_filtered_topic_options_t subscription_content_filtered_topic_options = + rcl_subscription_get_default_content_filtered_topic_options(); + { + // init with filter_expression1 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + filter_expression1, 0, nullptr, &subscription_content_filtered_topic_options) + ); + + content_filtered_topic_options = + subscription_content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression1, content_filtered_topic_options->filter_expression); + EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); + + // set with filter_expression1_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_set( + filter_expression1_update, 0, nullptr, &subscription_content_filtered_topic_options) + ); + + content_filtered_topic_options = + subscription_content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression1_update, content_filtered_topic_options->filter_expression); + EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "p1", "p2", "q1", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); + + const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; + const char * expression_parameters2_update[] = { + "p11", "p22", "q11", + }; + size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char*); + + rcl_subscription_content_filtered_topic_options_t subscription_content_filtered_topic_options2 = + rcl_subscription_get_default_content_filtered_topic_options(); + { + // init with filter_expression2 and expression_parameters2 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_init( + filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_content_filtered_topic_options2) + ); + + content_filtered_topic_options = + subscription_content_filtered_topic_options2.rmw_subscription_content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); + ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); + ASSERT_EQ(expression_parameters_count2, + content_filtered_topic_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filtered_topic_options->expression_parameters->data[i], + expression_parameters2[i]); + } + + // set with filter_expression2_update and expression_parameters2_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_set( + filter_expression2_update, expression_parameters_count2_update, + expression_parameters2_update, &subscription_content_filtered_topic_options2) + ); + + content_filtered_topic_options = + subscription_content_filtered_topic_options2.rmw_subscription_content_filtered_topic_options; + ASSERT_NE(nullptr, content_filtered_topic_options); + EXPECT_STREQ(filter_expression2_update, content_filtered_topic_options->filter_expression); + ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); + ASSERT_EQ(expression_parameters_count2_update, + content_filtered_topic_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2_update; ++i) { + EXPECT_STREQ( + content_filtered_topic_options->expression_parameters->data[i], + expression_parameters2_update[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &subscription_content_filtered_topic_options) + ); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filtered_topic_options_fini( + &subscription_content_filtered_topic_options2) + ); +} From 55c02c285bd6a5564a28291d65040880e73c73cf Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:24:10 +0800 Subject: [PATCH 13/20] update test case Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 17 +++++++++++++---- rcl/src/rcl/subscription.c | 8 +++----- rcl/test/rcl/test_subscription.cpp | 28 ++++++++++++++++++++-------- 3 files changed, 36 insertions(+), 17 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 0a3a18df6..3adcc67a4 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -54,7 +54,7 @@ typedef struct rcl_subscription_options_s typedef struct rcl_subscription_content_filtered_topic_options_s { - /// Custom allocator for the subscription, used for incidental allocations. + /// Custom allocator for the options, used for incidental allocations. /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ rcl_allocator_t allocator; /// rmw specific subscription content filtered topic options @@ -231,14 +231,23 @@ rcl_subscription_get_default_options(void); * \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 option is NULL, or - * if its allocator is invalid and the structure contains initialized memory. + * \returns `RCL_RET_BAD_ALLOC` if deallocating memory fails. */ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_subscription_options_fini(rcl_subscription_options_t * option); -// TODO. comments +/// Set the content filtered topic options for the given subscription options. +/** + * \param[in] filter_expression The filter expression. + * \param[in] expression_parameters_argc The expression parameters argc. + * \param[in] expression_parameter_argv The expression parameters argv. + * \param[out] options The subscription options to be set. + * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or + * \returns `RCL_RET_BAD_ALLOC` if allocating memory fails. + */ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t @@ -372,7 +381,7 @@ rcl_subscription_set_cft_expression_parameters( * \param[in] subscription The subscription object to inspect. * \param[out] options The rcl content filtered topic options. * It is up to the caller to finalize this options later on, using - * rcl_content_filtered_topic_options_fini(). + * rcl_subscription_content_filtered_topic_options_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 `options` is NULL, or diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 7132ec12e..58a0ba036 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -247,7 +247,7 @@ rcl_subscription_options_set_content_filtered_topic_options( rcl_ret_t ret; rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); if (!content_filtered_topic_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; @@ -314,7 +314,7 @@ rcl_subscription_content_filtered_topic_options_init( rcl_ret_t ret; rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); if (!content_filtered_topic_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; @@ -456,7 +456,7 @@ rcl_subscription_get_cft_expression_parameters( rcl_ret_t ret; rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); + sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); if (!content_filtered_topic_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; @@ -479,8 +479,6 @@ rcl_subscription_get_cft_expression_parameters( failed: allocator->deallocate(content_filtered_topic_options, allocator->state); return ret; - - } rcl_ret_t diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 28af04fbf..5e1aa0c21 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -855,8 +855,8 @@ TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_content_filtered) { - bool is_vendor_support_cft - = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + bool is_vendor_support_cft = + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 ); @@ -896,6 +896,8 @@ TEST_F( ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); + + // publish with a non-filtered data constexpr char test_string[] = "NotFilteredData"; { test_msgs__msg__Strings msg; @@ -1065,7 +1067,6 @@ TEST_F( } } - // reset filter { rcl_subscription_content_filtered_topic_options_t options = @@ -1095,7 +1096,7 @@ TEST_F( ); } - // publish no filtered data again + // publish with a non-filtered data again { test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -1144,8 +1145,8 @@ TEST_F( TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { - bool is_vendor_support_cft - = (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + bool is_vendor_support_cft = + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 ); @@ -1173,9 +1174,20 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); + + // failed to get filter + { + rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = + rcl_subscription_get_default_content_filtered_topic_options(); + + ret = rcl_subscription_get_cft_expression_parameters( + &subscription, &content_filtered_topic_options); + ASSERT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); - // publish no filtered data + // publish with a non-filtered data constexpr char test_string[] = "NotFilteredData"; { test_msgs__msg__Strings msg; @@ -1205,7 +1217,7 @@ TEST_F( // set filter const char * filter_expression2 = "string_value MATCH %0"; const char * expression_parameters2[] = {"'FilteredData'"}; - size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char*); + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); { rcl_subscription_content_filtered_topic_options_t options = rcl_subscription_get_default_content_filtered_topic_options(); From 1868d1a426c1e7a6a18fdad6c3251725eab0b5db Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:32:44 +0800 Subject: [PATCH 14/20] nit Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 4 ++-- .../test_subscription_content_filtered_topic_options.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 3adcc67a4..768a3e24c 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -231,7 +231,7 @@ rcl_subscription_get_default_options(void); * \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 option is NULL, or - * \returns `RCL_RET_BAD_ALLOC` if deallocating memory fails. + * \return `RCL_RET_BAD_ALLOC` if deallocating memory fails. */ RCL_PUBLIC RCL_WARN_UNUSED @@ -246,7 +246,7 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option); * \param[out] options The subscription options to be set. * \return `RCL_RET_OK` if set options successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or - * \returns `RCL_RET_BAD_ALLOC` if allocating memory fails. + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. */ RCL_PUBLIC RCL_WARN_UNUSED diff --git a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp index d18a5738d..67b74bbbf 100644 --- a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp @@ -72,7 +72,7 @@ TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; const char * expression_parameters2[] = { - "p1", "p2", "q1", + "'p1'", "'p2'", "'q1'", }; size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); @@ -198,13 +198,13 @@ TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; const char * expression_parameters2[] = { - "p1", "p2", "q1", + "'p1'", "'p2'", "'q1'", }; size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; const char * expression_parameters2_update[] = { - "p11", "p22", "q11", + "'p11'", "'p22'", "'q11'", }; size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char*); From 06902914e5540f34fa49336d8bcdb7cfe200e626 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:42:52 +0800 Subject: [PATCH 15/20] fix for unsupported cft and unscrutify Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 3 +-- rcl/test/rcl/test_subscription.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 768a3e24c..06c25afe6 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -329,8 +329,7 @@ rcl_subscription_content_filtered_topic_options_fini( /** * Depending on the middleware and whether cft is enabled in the subscription. * - * It will return true if the middleware can support ContentFilteredTopic in the subscription - * and the cft is enabled. + * \return `true` if the content filtered topic of `subscription` is enabled, otherwise `false` */ RCL_PUBLIC RCL_WARN_UNUSED diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 5e1aa0c21..4cfb35e3c 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -856,7 +856,7 @@ TEST_F( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_content_filtered) { bool is_vendor_support_cft = - (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 ); @@ -955,7 +955,7 @@ TEST_F( // set filter const char * filter_expression2 = "string_value MATCH %0"; const char * expression_parameters2[] = {"'FilteredOtherData'"}; - size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char*); + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); { rcl_subscription_content_filtered_topic_options_t options = rcl_subscription_get_default_content_filtered_topic_options(); @@ -1146,7 +1146,7 @@ TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { bool is_vendor_support_cft = - (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 ); @@ -1182,7 +1182,11 @@ TEST_F( ret = rcl_subscription_get_cft_expression_parameters( &subscription, &content_filtered_topic_options); - ASSERT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + if (is_vendor_support_cft) { + ASSERT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret) << rcl_get_error_string().str; + } } ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); From 80d1e33f259733042d71834ab3ae42aced21db03 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:50:51 +0800 Subject: [PATCH 16/20] fix unscrutify Signed-off-by: Chen Lihui --- ...ription_content_filtered_topic_options.cpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp index 67b74bbbf..207d38008 100644 --- a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp @@ -63,8 +63,8 @@ TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) filter_expression1, 0, nullptr, &subscription_options) ); - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options - = subscription_options.rmw_subscription_options.content_filtered_topic_options; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + subscription_options.rmw_subscription_options.content_filtered_topic_options; ASSERT_NE(nullptr, content_filtered_topic_options); EXPECT_STREQ(filter_expression1, content_filtered_topic_options->filter_expression); EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); @@ -74,7 +74,7 @@ TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) const char * expression_parameters2[] = { "'p1'", "'p2'", "'q1'", }; - size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); { EXPECT_EQ( @@ -84,12 +84,13 @@ TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) expression_parameters2, &subscription_options) ); - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options - = subscription_options.rmw_subscription_options.content_filtered_topic_options; + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + subscription_options.rmw_subscription_options.content_filtered_topic_options; ASSERT_NE(nullptr, content_filtered_topic_options); EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ(expression_parameters_count2, + ASSERT_EQ( + expression_parameters_count2, content_filtered_topic_options->expression_parameters->size); for (size_t i = 0; i < expression_parameters_count2; ++i) { EXPECT_STREQ( @@ -200,13 +201,13 @@ TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options const char * expression_parameters2[] = { "'p1'", "'p2'", "'q1'", }; - size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char*); + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; const char * expression_parameters2_update[] = { "'p11'", "'p22'", "'q11'", }; - size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char*); + size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char *); rcl_subscription_content_filtered_topic_options_t subscription_content_filtered_topic_options2 = rcl_subscription_get_default_content_filtered_topic_options(); @@ -224,7 +225,8 @@ TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options ASSERT_NE(nullptr, content_filtered_topic_options); EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ(expression_parameters_count2, + ASSERT_EQ( + expression_parameters_count2, content_filtered_topic_options->expression_parameters->size); for (size_t i = 0; i < expression_parameters_count2; ++i) { EXPECT_STREQ( @@ -245,7 +247,8 @@ TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options ASSERT_NE(nullptr, content_filtered_topic_options); EXPECT_STREQ(filter_expression2_update, content_filtered_topic_options->filter_expression); ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ(expression_parameters_count2_update, + ASSERT_EQ( + expression_parameters_count2_update, content_filtered_topic_options->expression_parameters->size); for (size_t i = 0; i < expression_parameters_count2_update; ++i) { EXPECT_STREQ( From a9b226b6b06839c9a6c8ca792df8c2f10be9a22b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 26 Oct 2021 14:24:17 +0800 Subject: [PATCH 17/20] rename Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 53 ++-- rcl/src/rcl/subscription.c | 130 ++++----- rcl/test/CMakeLists.txt | 4 +- rcl/test/rcl/test_subscription.cpp | 104 +++---- ...st_subscription_content_filter_options.cpp | 270 ++++++++++++++++++ ...ription_content_filtered_topic_options.cpp | 270 ------------------ 6 files changed, 415 insertions(+), 416 deletions(-) create mode 100644 rcl/test/rcl/test_subscription_content_filter_options.cpp delete mode 100644 rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 06c25afe6..732624453 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -52,15 +52,14 @@ typedef struct rcl_subscription_options_s rmw_subscription_options_t rmw_subscription_options; } rcl_subscription_options_t; -typedef struct rcl_subscription_content_filtered_topic_options_s +typedef struct rcl_subscription_content_filter_options_s { /// Custom allocator for the options, used for incidental allocations. /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ rcl_allocator_t allocator; - /// rmw specific subscription content filtered topic options - rmw_subscription_content_filtered_topic_options_t * - rmw_subscription_content_filtered_topic_options; -} rcl_subscription_content_filtered_topic_options_t; + /// rmw specific subscription content filter options + rmw_subscription_content_filter_options_t * rmw_subscription_content_filter_options; +} rcl_subscription_content_filter_options_t; /// Return a rcl_subscription_t struct with members set to `NULL`. /** @@ -238,7 +237,7 @@ RCL_WARN_UNUSED rcl_ret_t rcl_subscription_options_fini(rcl_subscription_options_t * option); -/// Set the content filtered topic options for the given subscription options. +/// Set the content filter options for the given subscription options. /** * \param[in] filter_expression The filter expression. * \param[in] expression_parameters_argc The expression parameters argc. @@ -251,25 +250,25 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_options_set_content_filtered_topic_options( +rcl_subscription_options_set_content_filter_options( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], rcl_subscription_options_t * options); -/// Return the default subscription content filtered topic options. +/// Return the default subscription content filter options. /** * The defaults are: * * - allocator = rcl_get_default_allocator() - * - rmw_subscription_content_filtered_topic_options = NULL; + * - rmw_subscription_content_filter_options = NULL; * * \return A structure containing the default options for a subscription. */ RCL_PUBLIC RCL_WARN_UNUSED -rcl_subscription_content_filtered_topic_options_t -rcl_subscription_get_default_content_filtered_topic_options(void); +rcl_subscription_content_filter_options_t +rcl_subscription_get_default_content_filter_options(void); /// Allocate. /** @@ -289,22 +288,22 @@ rcl_subscription_get_default_content_filtered_topic_options(void); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_content_filtered_topic_options_init( +rcl_subscription_content_filter_options_init( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], - rcl_subscription_content_filtered_topic_options_t * options); + rcl_subscription_content_filter_options_t * options); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_content_filtered_topic_options_set( +rcl_subscription_content_filter_options_set( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], - rcl_subscription_content_filtered_topic_options_t * options); + rcl_subscription_content_filter_options_t * options); -/// Reclaim rcl_subscription_content_filtered_topic_options_t structure. +/// Reclaim rcl_subscription_content_filter_options_t structure. /** *
* Attribute | Adherence @@ -322,8 +321,8 @@ rcl_subscription_content_filtered_topic_options_set( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_content_filtered_topic_options_fini( - rcl_subscription_content_filtered_topic_options_t * options); +rcl_subscription_content_filter_options_fini( + rcl_subscription_content_filter_options_t * options); /// Check if the content filtered topic feature is enabled in the subscription. /** @@ -349,20 +348,20 @@ rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription); * Uses Atomics | Maybe [1] * Lock-Free | Maybe [1] * - * \param[in] subscription The subscription to set content filtered topic options. - * \param[in] options The rcl content filtered topic options. + * \param[in] subscription The subscription to set content filter options. + * \param[in] options The rcl content filter options. * \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 `rcl_content_filtered_topic_options` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `rcl_content_filter_options` is NULL, 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 RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_set_cft_expression_parameters( +rcl_subscription_set_content_filter( const rcl_subscription_t * subscription, - const rcl_subscription_content_filtered_topic_options_t * options + const rcl_subscription_content_filter_options_t * options ); /// Retrieve the filter expression of the subscription. @@ -378,9 +377,9 @@ rcl_subscription_set_cft_expression_parameters( * Lock-Free | Maybe [1] * * \param[in] subscription The subscription object to inspect. - * \param[out] options The rcl content filtered topic options. + * \param[out] options The rcl content filter options. * It is up to the caller to finalize this options later on, using - * rcl_subscription_content_filtered_topic_options_fini(). + * rcl_subscription_content_filter_options_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 `options` is NULL, or @@ -391,9 +390,9 @@ rcl_subscription_set_cft_expression_parameters( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_subscription_get_cft_expression_parameters( +rcl_subscription_get_content_filter( const rcl_subscription_t * subscription, - rcl_subscription_content_filtered_topic_options_t * options + rcl_subscription_content_filter_options_t * options ); /// Take a ROS message from a topic using a rcl subscription. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 58a0ba036..2580289b4 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -27,7 +27,7 @@ extern "C" #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" #include "rmw/error_handling.h" -#include "rmw/subscription_content_filtered_topic_options.h" +#include "rmw/subscription_content_filter_options.h" #include "rmw/validate_full_topic_name.h" #include "tracetools/tracetools.h" @@ -218,22 +218,22 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option) 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.content_filtered_topic_options) { - rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_fini( - option->rmw_subscription_options.content_filtered_topic_options, allocator); + if (option->rmw_subscription_options.content_filter_options) { + rmw_ret_t ret = rmw_subscription_content_filter_options_fini( + option->rmw_subscription_options.content_filter_options, allocator); if (RCUTILS_RET_OK != ret) { - RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini content filtered topic options.\n"); + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini content filter options.\n"); return rcl_convert_rmw_ret_to_rcl_ret(ret); } allocator->deallocate( - option->rmw_subscription_options.content_filtered_topic_options, allocator->state); - option->rmw_subscription_options.content_filtered_topic_options = NULL; + option->rmw_subscription_options.content_filter_options, allocator->state); + option->rmw_subscription_options.content_filter_options = NULL; } return RCL_RET_OK; } rcl_ret_t -rcl_subscription_options_set_content_filtered_topic_options( +rcl_subscription_options_set_content_filter_options( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], @@ -245,22 +245,22 @@ rcl_subscription_options_set_content_filtered_topic_options( RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret; - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + rmw_subscription_content_filter_options_t * content_filter_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); - if (!content_filtered_topic_options) { + sizeof(rmw_subscription_content_filter_options_t), allocator->state); + if (!content_filter_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; } - *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + *content_filter_options = rmw_get_zero_initialized_content_filter_options(); - rmw_ret_t rmw_ret = rmw_subscription_content_filtered_topic_options_set( + rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_set( filter_expression, expression_parameters_argc, expression_parameter_argv, allocator, - content_filtered_topic_options + content_filter_options ); if (rmw_ret != RMW_RET_OK) { @@ -268,9 +268,9 @@ rcl_subscription_options_set_content_filtered_topic_options( goto failed; } - if (options->rmw_subscription_options.content_filtered_topic_options) { - rmw_ret = rmw_subscription_content_filtered_topic_options_fini( - options->rmw_subscription_options.content_filtered_topic_options, + if (options->rmw_subscription_options.content_filter_options) { + rmw_ret = rmw_subscription_content_filter_options_fini( + options->rmw_subscription_options.content_filter_options, allocator ); @@ -279,100 +279,100 @@ rcl_subscription_options_set_content_filtered_topic_options( } allocator->deallocate( - options->rmw_subscription_options.content_filtered_topic_options, allocator->state); - options->rmw_subscription_options.content_filtered_topic_options = NULL; + options->rmw_subscription_options.content_filter_options, allocator->state); + options->rmw_subscription_options.content_filter_options = NULL; } - options->rmw_subscription_options.content_filtered_topic_options = content_filtered_topic_options; + options->rmw_subscription_options.content_filter_options = content_filter_options; return RCL_RET_OK; failed: - allocator->deallocate(content_filtered_topic_options, allocator->state); + allocator->deallocate(content_filter_options, allocator->state); return ret; } -rcl_subscription_content_filtered_topic_options_t -rcl_subscription_get_default_content_filtered_topic_options() +rcl_subscription_content_filter_options_t +rcl_subscription_get_default_content_filter_options() { - static rcl_subscription_content_filtered_topic_options_t default_options; + static rcl_subscription_content_filter_options_t default_options; default_options.allocator = rcl_get_default_allocator(); - default_options.rmw_subscription_content_filtered_topic_options = NULL; + default_options.rmw_subscription_content_filter_options = NULL; return default_options; } rcl_ret_t -rcl_subscription_content_filtered_topic_options_init( +rcl_subscription_content_filter_options_init( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], - rcl_subscription_content_filtered_topic_options_t * options) + rcl_subscription_content_filter_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); const rcl_allocator_t * allocator = &options->allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret; - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + rmw_subscription_content_filter_options_t * content_filter_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); - if (!content_filtered_topic_options) { + sizeof(rmw_subscription_content_filter_options_t), allocator->state); + if (!content_filter_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; } - *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + *content_filter_options = rmw_get_zero_initialized_content_filter_options(); - rmw_ret_t rmw_ret = rmw_subscription_content_filtered_topic_options_init( + rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init( filter_expression, expression_parameters_argc, expression_parameter_argv, allocator, - content_filtered_topic_options + content_filter_options ); if (rmw_ret != RMW_RET_OK) { ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); goto failed; } - options->rmw_subscription_content_filtered_topic_options = content_filtered_topic_options; + options->rmw_subscription_content_filter_options = content_filter_options; return RCL_RET_OK; failed: - allocator->deallocate(content_filtered_topic_options, allocator->state); + allocator->deallocate(content_filter_options, allocator->state); return ret; } rcl_ret_t -rcl_subscription_content_filtered_topic_options_set( +rcl_subscription_content_filter_options_set( const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], - rcl_subscription_content_filtered_topic_options_t * options) + rcl_subscription_content_filter_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); const rcl_allocator_t * allocator = &options->allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_set( + rmw_ret_t ret = rmw_subscription_content_filter_options_set( filter_expression, expression_parameters_argc, expression_parameter_argv, allocator, - options->rmw_subscription_content_filtered_topic_options + options->rmw_subscription_content_filter_options ); return rcl_convert_rmw_ret_to_rcl_ret(ret); } rcl_ret_t -rcl_subscription_content_filtered_topic_options_fini( - rcl_subscription_content_filtered_topic_options_t * options) +rcl_subscription_content_filter_options_fini( + rcl_subscription_content_filter_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); const rcl_allocator_t * allocator = &options->allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); - rmw_ret_t ret = rmw_subscription_content_filtered_topic_options_fini( - options->rmw_subscription_content_filtered_topic_options, + rmw_ret_t ret = rmw_subscription_content_filter_options_fini( + options->rmw_subscription_content_filter_options, allocator ); @@ -380,7 +380,7 @@ rcl_subscription_content_filtered_topic_options_fini( return rcl_convert_rmw_ret_to_rcl_ret(ret); } - allocator->deallocate(options->rmw_subscription_content_filtered_topic_options, allocator->state); + allocator->deallocate(options->rmw_subscription_content_filter_options, allocator->state); return RCL_RET_OK; } @@ -394,9 +394,9 @@ rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription) } rcl_ret_t -rcl_subscription_set_cft_expression_parameters( +rcl_subscription_set_content_filter( const rcl_subscription_t * subscription, - const rcl_subscription_content_filtered_topic_options_t * options + const rcl_subscription_content_filter_options_t * options ) { RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); @@ -410,9 +410,9 @@ rcl_subscription_set_cft_expression_parameters( RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - rmw_ret_t ret = rmw_subscription_set_cft_expression_parameters( + rmw_ret_t ret = rmw_subscription_set_content_filter( subscription->impl->rmw_handle, - options->rmw_subscription_content_filtered_topic_options); + options->rmw_subscription_content_filter_options); if (ret != RMW_RET_OK) { RCL_SET_ERROR_MSG(rmw_get_error_string().str); @@ -420,17 +420,17 @@ rcl_subscription_set_cft_expression_parameters( } // copy options into subscription_options - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = - options->rmw_subscription_content_filtered_topic_options; + rmw_subscription_content_filter_options_t * content_filter_options = + options->rmw_subscription_content_filter_options; size_t expression_parameters_size = 0; const char ** expression_parameters = NULL; - if (content_filtered_topic_options->expression_parameters) { - expression_parameters_size = content_filtered_topic_options->expression_parameters->size; + if (content_filter_options->expression_parameters) { + expression_parameters_size = content_filter_options->expression_parameters->size; expression_parameters = - (const char **)content_filtered_topic_options->expression_parameters->data; + (const char **)content_filter_options->expression_parameters->data; } - return rcl_subscription_options_set_content_filtered_topic_options( - content_filtered_topic_options->filter_expression, + return rcl_subscription_options_set_content_filter_options( + content_filter_options->filter_expression, expression_parameters_size, expression_parameters, &subscription->impl->options @@ -438,9 +438,9 @@ rcl_subscription_set_cft_expression_parameters( } rcl_ret_t -rcl_subscription_get_cft_expression_parameters( +rcl_subscription_get_content_filter( const rcl_subscription_t * subscription, - rcl_subscription_content_filtered_topic_options_t * options + rcl_subscription_content_filter_options_t * options ) { RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); @@ -454,30 +454,30 @@ rcl_subscription_get_cft_expression_parameters( RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret; - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + rmw_subscription_content_filter_options_t * content_filter_options = allocator->allocate( - sizeof(rmw_subscription_content_filtered_topic_options_t), allocator->state); - if (!content_filtered_topic_options) { + sizeof(rmw_subscription_content_filter_options_t), allocator->state); + if (!content_filter_options) { RCL_SET_ERROR_MSG("allocating memory failed"); return RCL_RET_BAD_ALLOC; } - *content_filtered_topic_options = rmw_get_zero_initialized_content_filtered_topic_options(); + *content_filter_options = rmw_get_zero_initialized_content_filter_options(); - rmw_ret_t rmw_ret = rmw_subscription_get_cft_expression_parameters( + rmw_ret_t rmw_ret = rmw_subscription_get_content_filter( subscription->impl->rmw_handle, - &options->allocator, content_filtered_topic_options); + &options->allocator, content_filter_options); if (rmw_ret != RMW_RET_OK) { ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); goto failed; } - options->rmw_subscription_content_filtered_topic_options = content_filtered_topic_options; + options->rmw_subscription_content_filter_options = content_filter_options; return RCL_RET_OK; failed: - allocator->deallocate(content_filtered_topic_options, allocator->state); + allocator->deallocate(content_filter_options, allocator->state); return ret; } diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 5d0d909f8..d65f72691 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -434,8 +434,8 @@ rcl_add_custom_gtest(test_log_level AMENT_DEPENDENCIES "osrf_testing_tools_cpp" ) -rcl_add_custom_gtest(test_subscription_content_filtered_topic_options - SRCS rcl/test_subscription_content_filtered_topic_options.cpp +rcl_add_custom_gtest(test_subscription_content_filter_options + SRCS rcl/test_subscription_content_filter_options.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} ) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 4cfb35e3c..8c4f2a703 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -879,7 +879,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_options_set_content_filtered_topic_options( + rcl_subscription_options_set_content_filter_options( filter_expression1, 0, nullptr, &subscription_options) ); @@ -957,17 +957,17 @@ TEST_F( const char * expression_parameters2[] = {"'FilteredOtherData'"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); { - rcl_subscription_content_filtered_topic_options_t options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( + rcl_subscription_content_filter_options_init( filter_expression2, expression_parameters2_count, expression_parameters2, &options) ); - ret = rcl_subscription_set_cft_expression_parameters( + ret = rcl_subscription_set_content_filter( &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -977,7 +977,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1038,16 +1038,16 @@ TEST_F( // get filter { - rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t content_filter_options = + rcl_subscription_get_default_content_filter_options(); - ret = rcl_subscription_get_cft_expression_parameters( - &subscription, &content_filtered_topic_options); + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - rmw_subscription_content_filtered_topic_options_t * options = - content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; + rmw_subscription_content_filter_options_t * options = + content_filter_options.rmw_subscription_content_filter_options; ASSERT_NE(nullptr, options); ASSERT_STREQ(filter_expression2, options->filter_expression); ASSERT_NE(nullptr, options->expression_parameters); @@ -1059,8 +1059,8 @@ TEST_F( } EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( - &content_filtered_topic_options) + rcl_subscription_content_filter_options_fini( + &content_filter_options) ); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); @@ -1069,17 +1069,17 @@ TEST_F( // reset filter { - rcl_subscription_content_filtered_topic_options_t options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( + rcl_subscription_content_filter_options_init( "", 0, nullptr, &options) ); - ret = rcl_subscription_set_cft_expression_parameters( + ret = rcl_subscription_set_content_filter( &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -1091,7 +1091,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1177,11 +1177,11 @@ TEST_F( // failed to get filter { - rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t content_filter_options = + rcl_subscription_get_default_content_filter_options(); - ret = rcl_subscription_get_cft_expression_parameters( - &subscription, &content_filtered_topic_options); + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; } else { @@ -1223,17 +1223,17 @@ TEST_F( const char * expression_parameters2[] = {"'FilteredData'"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); { - rcl_subscription_content_filtered_topic_options_t options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( + rcl_subscription_content_filter_options_init( filter_expression2, expression_parameters2_count, expression_parameters2, &options) ); - ret = rcl_subscription_set_cft_expression_parameters( + ret = rcl_subscription_set_content_filter( &subscription, &options); if (is_vendor_support_cft) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -1243,7 +1243,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1561,33 +1561,33 @@ TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscrip rcl_reset_error(); } -/* Test for all failure modes in rcl_subscription_set_cft_expression_parameters function. +/* Test for all failure modes in rcl_subscription_set_content_filter function. */ TEST_F( CLASSNAME( TestSubscriptionFixtureInit, - RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_cft_expression_parameters) { + RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_content_filter) { EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_subscription_set_cft_expression_parameters(nullptr, nullptr)); + rcl_subscription_set_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_subscription_set_cft_expression_parameters(&subscription_zero_init, nullptr)); + rcl_subscription_set_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, - rcl_subscription_set_cft_expression_parameters(&subscription, nullptr)); + rcl_subscription_set_content_filter(&subscription, nullptr)); rcl_reset_error(); // an options used later - rcl_subscription_content_filtered_topic_options_t options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( + rcl_subscription_content_filter_options_init( "data MATCH '0'", 0, nullptr, @@ -1598,71 +1598,71 @@ TEST_F( { EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini(&options) + rcl_subscription_content_filter_options_fini(&options) ); }); { auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_UNSUPPORTED); + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); EXPECT_EQ( RMW_RET_UNSUPPORTED, - rcl_subscription_set_cft_expression_parameters( + rcl_subscription_set_content_filter( &subscription, &options)); rcl_reset_error(); } { auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_set_cft_expression_parameters, RMW_RET_ERROR); + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); EXPECT_EQ( RMW_RET_ERROR, - rcl_subscription_set_cft_expression_parameters( + rcl_subscription_set_content_filter( &subscription, &options)); rcl_reset_error(); } } -/* Test for all failure modes in rcl_subscription_get_cft_expression_parameters function. +/* Test for all failure modes in rcl_subscription_get_content_filter function. */ TEST_F( CLASSNAME( TestSubscriptionFixtureInit, - RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_cft_expression_parameters) { + RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_content_filter) { EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_subscription_get_cft_expression_parameters(nullptr, nullptr)); + rcl_subscription_get_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_SUBSCRIPTION_INVALID, - rcl_subscription_get_cft_expression_parameters(&subscription_zero_init, nullptr)); + rcl_subscription_get_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, - rcl_subscription_get_cft_expression_parameters(&subscription, nullptr)); + rcl_subscription_get_content_filter(&subscription, nullptr)); rcl_reset_error(); - rcl_subscription_content_filtered_topic_options_t options = - rcl_subscription_get_default_content_filtered_topic_options(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); { auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_get_cft_expression_parameters, RMW_RET_UNSUPPORTED); + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED); EXPECT_EQ( RMW_RET_UNSUPPORTED, - rcl_subscription_get_cft_expression_parameters( + rcl_subscription_get_content_filter( &subscription, &options)); rcl_reset_error(); } { auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_get_cft_expression_parameters, RMW_RET_ERROR); + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); EXPECT_EQ( RMW_RET_ERROR, - rcl_subscription_get_cft_expression_parameters( + rcl_subscription_get_content_filter( &subscription, &options)); rcl_reset_error(); } diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp new file mode 100644 index 000000000..d42b9b71c --- /dev/null +++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp @@ -0,0 +1,270 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/error_handling.h" +#include "rcl/subscription.h" + +TEST(TestSubscriptionContentFilterOptions, subscription_options_failure) { + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 1, nullptr, &subscription_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_fini( + nullptr) + ); + rcl_reset_error(); +} + +TEST(TestSubscriptionContentFilterOptions, subscription_options_success) +{ + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 0, nullptr, &subscription_options) + ); + + rmw_subscription_content_filter_options_t * content_filter_options = + subscription_options.rmw_subscription_options.content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "1", "2", "3", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_options( + filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_options) + ); + + rmw_subscription_content_filter_options_t * content_filter_options = + subscription_options.rmw_subscription_options.content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression); + ASSERT_NE(nullptr, content_filter_options->expression_parameters); + ASSERT_EQ( + expression_parameters_count2, + content_filter_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters->data[i], + expression_parameters2[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_fini(&subscription_options) + ); +} + +TEST(TestSubscriptionContentFilterOptions, content_filter_options_failure) { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_subscription_get_default_content_filter_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + filter_expression1, 1, nullptr, &content_filter_options) + ); + rcl_reset_error(); + + // set + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + filter_expression1, 1, nullptr, &content_filter_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_fini( + nullptr) + ); + rcl_reset_error(); +} + +TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) +{ + rmw_subscription_content_filter_options_t * content_filter_options; + const char * filter_expression1 = "filter=1"; + const char * filter_expression1_update = "filter=2"; + + rcl_subscription_content_filter_options_t subscription_content_filter_options = + rcl_subscription_get_default_content_filter_options(); + { + // init with filter_expression1 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + filter_expression1, 0, nullptr, &subscription_content_filter_options) + ); + + content_filter_options = + subscription_content_filter_options.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters); + + // set with filter_expression1_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_set( + filter_expression1_update, 0, nullptr, &subscription_content_filter_options) + ); + + content_filter_options = + subscription_content_filter_options.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression1_update, content_filter_options->filter_expression); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "1", "2", "3", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); + + const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; + const char * expression_parameters2_update[] = { + "11", "22", "33", + }; + size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char *); + + rcl_subscription_content_filter_options_t subscription_content_filter_options2 = + rcl_subscription_get_default_content_filter_options(); + { + // init with filter_expression2 and expression_parameters2 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_content_filter_options2) + ); + + content_filter_options = + subscription_content_filter_options2.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression); + ASSERT_NE(nullptr, content_filter_options->expression_parameters); + ASSERT_EQ( + expression_parameters_count2, + content_filter_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters->data[i], + expression_parameters2[i]); + } + + // set with filter_expression2_update and expression_parameters2_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_set( + filter_expression2_update, expression_parameters_count2_update, + expression_parameters2_update, &subscription_content_filter_options2) + ); + + content_filter_options = + subscription_content_filter_options2.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2_update, content_filter_options->filter_expression); + ASSERT_NE(nullptr, content_filter_options->expression_parameters); + ASSERT_EQ( + expression_parameters_count2_update, + content_filter_options->expression_parameters->size); + for (size_t i = 0; i < expression_parameters_count2_update; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters->data[i], + expression_parameters2_update[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription_content_filter_options) + ); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription_content_filter_options2) + ); +} diff --git a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp b/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp deleted file mode 100644 index 207d38008..000000000 --- a/rcl/test/rcl/test_subscription_content_filtered_topic_options.cpp +++ /dev/null @@ -1,270 +0,0 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "rcl/error_handling.h" -#include "rcl/subscription.h" - -TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_failure) { - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - - const char * filter_expression1 = "filter=1"; - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_options_set_content_filtered_topic_options( - nullptr, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_options_set_content_filtered_topic_options( - filter_expression1, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_options_set_content_filtered_topic_options( - filter_expression1, 1, nullptr, &subscription_options) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_options_fini( - nullptr) - ); - rcl_reset_error(); -} - -TEST(TestSubscriptionContentFilteredTopicOptions, subscription_options_success) -{ - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - - const char * filter_expression1 = "filter=1"; - - { - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_options_set_content_filtered_topic_options( - filter_expression1, 0, nullptr, &subscription_options) - ); - - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = - subscription_options.rmw_subscription_options.content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression1, content_filtered_topic_options->filter_expression); - EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); - } - - const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; - const char * expression_parameters2[] = { - "'p1'", "'p2'", "'q1'", - }; - size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); - - { - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_options_set_content_filtered_topic_options( - filter_expression2, expression_parameters_count2, - expression_parameters2, &subscription_options) - ); - - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = - subscription_options.rmw_subscription_options.content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); - ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ( - expression_parameters_count2, - content_filtered_topic_options->expression_parameters->size); - for (size_t i = 0; i < expression_parameters_count2; ++i) { - EXPECT_STREQ( - content_filtered_topic_options->expression_parameters->data[i], - expression_parameters2[i]); - } - } - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_options_fini(&subscription_options) - ); -} - -TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options_failure) { - rcl_subscription_content_filtered_topic_options_t content_filtered_topic_options = - rcl_subscription_get_default_content_filtered_topic_options(); - - const char * filter_expression1 = "filter=1"; - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_init( - nullptr, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_init( - filter_expression1, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_init( - filter_expression1, 1, nullptr, &content_filtered_topic_options) - ); - rcl_reset_error(); - - // set - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_set( - nullptr, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_set( - filter_expression1, 0, nullptr, nullptr) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_set( - filter_expression1, 1, nullptr, &content_filtered_topic_options) - ); - rcl_reset_error(); - - EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, - rcl_subscription_content_filtered_topic_options_fini( - nullptr) - ); - rcl_reset_error(); -} - -TEST(TestSubscriptionContentFilteredTopicOptions, content_filtered_topic_options_success) -{ - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options; - const char * filter_expression1 = "filter=1"; - const char * filter_expression1_update = "filter=2"; - - rcl_subscription_content_filtered_topic_options_t subscription_content_filtered_topic_options = - rcl_subscription_get_default_content_filtered_topic_options(); - { - // init with filter_expression1 - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( - filter_expression1, 0, nullptr, &subscription_content_filtered_topic_options) - ); - - content_filtered_topic_options = - subscription_content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression1, content_filtered_topic_options->filter_expression); - EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); - - // set with filter_expression1_update - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_set( - filter_expression1_update, 0, nullptr, &subscription_content_filtered_topic_options) - ); - - content_filtered_topic_options = - subscription_content_filtered_topic_options.rmw_subscription_content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression1_update, content_filtered_topic_options->filter_expression); - EXPECT_EQ(nullptr, content_filtered_topic_options->expression_parameters); - } - - const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; - const char * expression_parameters2[] = { - "'p1'", "'p2'", "'q1'", - }; - size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); - - const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; - const char * expression_parameters2_update[] = { - "'p11'", "'p22'", "'q11'", - }; - size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char *); - - rcl_subscription_content_filtered_topic_options_t subscription_content_filtered_topic_options2 = - rcl_subscription_get_default_content_filtered_topic_options(); - { - // init with filter_expression2 and expression_parameters2 - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_init( - filter_expression2, expression_parameters_count2, - expression_parameters2, &subscription_content_filtered_topic_options2) - ); - - content_filtered_topic_options = - subscription_content_filtered_topic_options2.rmw_subscription_content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression2, content_filtered_topic_options->filter_expression); - ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ( - expression_parameters_count2, - content_filtered_topic_options->expression_parameters->size); - for (size_t i = 0; i < expression_parameters_count2; ++i) { - EXPECT_STREQ( - content_filtered_topic_options->expression_parameters->data[i], - expression_parameters2[i]); - } - - // set with filter_expression2_update and expression_parameters2_update - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_set( - filter_expression2_update, expression_parameters_count2_update, - expression_parameters2_update, &subscription_content_filtered_topic_options2) - ); - - content_filtered_topic_options = - subscription_content_filtered_topic_options2.rmw_subscription_content_filtered_topic_options; - ASSERT_NE(nullptr, content_filtered_topic_options); - EXPECT_STREQ(filter_expression2_update, content_filtered_topic_options->filter_expression); - ASSERT_NE(nullptr, content_filtered_topic_options->expression_parameters); - ASSERT_EQ( - expression_parameters_count2_update, - content_filtered_topic_options->expression_parameters->size); - for (size_t i = 0; i < expression_parameters_count2_update; ++i) { - EXPECT_STREQ( - content_filtered_topic_options->expression_parameters->data[i], - expression_parameters2_update[i]); - } - } - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( - &subscription_content_filtered_topic_options) - ); - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( - &subscription_content_filtered_topic_options2) - ); -} From 8760e770345ea85824a60a01262a33c8f043fbcc Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 13 Jan 2022 18:04:58 +0800 Subject: [PATCH 18/20] refactor test Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 64 +++++++----------------------- 1 file changed, 15 insertions(+), 49 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 8c4f2a703..b4f6e28ad 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -855,11 +855,6 @@ TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_content_filtered) { - bool is_vendor_support_cft = - (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 - // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 - ); - const char * filter_expression1 = "string_value MATCH 'FilteredData'"; rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); @@ -890,11 +885,7 @@ TEST_F( rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - if (is_vendor_support_cft) { - ASSERT_TRUE(rcl_subscription_is_cft_enabled(&subscription)); - } else { - ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); - } + bool is_cft_support = rcl_subscription_is_cft_enabled(&subscription); ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); // publish with a non-filtered data @@ -908,7 +899,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); @@ -969,7 +960,7 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); @@ -992,7 +983,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); @@ -1043,7 +1034,7 @@ TEST_F( ret = rcl_subscription_get_content_filter( &subscription, &content_filter_options); - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rmw_subscription_content_filter_options_t * options = @@ -1081,7 +1072,7 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); @@ -1117,26 +1108,9 @@ TEST_F( }); ret = rcl_take(&subscription, &msg, nullptr, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - // it might take 'FilteredData' again, if so, contine to take data - std::string data = std::string(msg.string_value.data, msg.string_value.size); - if (is_vendor_support_cft) { - const int try_total_num = 3; - int i = 0; - while (data != test_string && i++ < try_total_num) { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__Strings__fini(&msg); - }); - ret = rcl_take(&subscription, &msg, nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - data = std::string(msg.string_value.data, msg.string_value.size); - } - } - ASSERT_EQ(std::string(test_string), data); + ASSERT_EQ( + std::string(test_string), + std::string(msg.string_value.data, msg.string_value.size)); } } @@ -1145,11 +1119,6 @@ TEST_F( TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { - bool is_vendor_support_cft = - (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 - // || std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0 - ); - rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = @@ -1182,11 +1151,7 @@ TEST_F( ret = rcl_subscription_get_content_filter( &subscription, &content_filter_options); - if (is_vendor_support_cft) { - ASSERT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret) << rcl_get_error_string().str; - } + ASSERT_NE(RCL_RET_OK, ret); } ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); @@ -1222,6 +1187,7 @@ TEST_F( const char * filter_expression2 = "string_value MATCH %0"; const char * expression_parameters2[] = {"'FilteredData'"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); + bool is_cft_support{true}; { rcl_subscription_content_filter_options_t options = rcl_subscription_get_default_content_filter_options(); @@ -1235,10 +1201,10 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (is_vendor_support_cft) { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if(RCL_RET_UNSUPPORTED == RCL_RET_OK) { + is_cft_support = false; } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } EXPECT_EQ( @@ -1258,7 +1224,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_vendor_support_cft) { + if (is_cft_support) { ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); From 42e7b60862859003a47ff6f3f317432842beaccb Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 13 Jan 2022 18:28:23 +0800 Subject: [PATCH 19/20] fix for uncrustify and typo Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index b4f6e28ad..4cc278c8c 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1201,7 +1201,7 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if(RCL_RET_UNSUPPORTED == RCL_RET_OK) { + if (RCL_RET_UNSUPPORTED == ret) { is_cft_support = false; } else { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; From 2f955bb039ec8cf9a4bd286a297513df06cc6223 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 17 Jan 2022 15:53:53 +0800 Subject: [PATCH 20/20] add content filter for action client feedback subscription Signed-off-by: Chen Lihui --- rcl/include/rcl/macros.h | 31 ++ rcl/src/rcl/logging_rosout.c | 32 +-- rcl_action/include/rcl_action/action_client.h | 59 ++++ rcl_action/include/rcl_action/types.h | 19 ++ rcl_action/src/rcl_action/action_client.c | 270 +++++++++++++++++- .../src/rcl_action/action_client_impl.h | 3 + .../rcl_action/test_action_communication.cpp | 101 +++++++ 7 files changed, 483 insertions(+), 32 deletions(-) diff --git a/rcl/include/rcl/macros.h b/rcl/include/rcl/macros.h index 4df9ff427..973dec116 100644 --- a/rcl/include/rcl/macros.h +++ b/rcl/include/rcl/macros.h @@ -27,6 +27,37 @@ extern "C" #define RCL_UNUSED(x) RCUTILS_UNUSED(x) +#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \ + { \ + rcutils_ret_t rcutils_ret = rcutils_expr; \ + if (RCUTILS_RET_OK != rcutils_ret) { \ + if (rcutils_error_is_set()) { \ + RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \ + } else { \ + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \ + } \ + } \ + switch (rcutils_ret) { \ + case RCUTILS_RET_OK: \ + rcl_ret_var = RCL_RET_OK; \ + break; \ + case RCUTILS_RET_ERROR: \ + rcl_ret_var = RCL_RET_ERROR; \ + break; \ + case RCUTILS_RET_BAD_ALLOC: \ + rcl_ret_var = RCL_RET_BAD_ALLOC; \ + break; \ + case RCUTILS_RET_INVALID_ARGUMENT: \ + rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \ + break; \ + case RCUTILS_RET_NOT_INITIALIZED: \ + rcl_ret_var = RCL_RET_NOT_INIT; \ + break; \ + default: \ + rcl_ret_var = RCUTILS_RET_ERROR; \ + } \ + } + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/logging_rosout.c b/rcl/src/rcl/logging_rosout.c index 4133a52f6..c1803f72a 100644 --- a/rcl/src/rcl/logging_rosout.c +++ b/rcl/src/rcl/logging_rosout.c @@ -15,6 +15,7 @@ #include "rcl/allocator.h" #include "rcl/error_handling.h" #include "rcl/logging_rosout.h" +#include "rcl/macros.h" #include "rcl/node.h" #include "rcl/publisher.h" #include "rcl/time.h" @@ -43,37 +44,6 @@ extern "C" return RCL_RET_OK; \ } -#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \ - { \ - rcutils_ret_t rcutils_ret = rcutils_expr; \ - if (RCUTILS_RET_OK != rcutils_ret) { \ - if (rcutils_error_is_set()) { \ - RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \ - } else { \ - RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \ - } \ - } \ - switch (rcutils_ret) { \ - case RCUTILS_RET_OK: \ - rcl_ret_var = RCL_RET_OK; \ - break; \ - case RCUTILS_RET_ERROR: \ - rcl_ret_var = RCL_RET_ERROR; \ - break; \ - case RCUTILS_RET_BAD_ALLOC: \ - rcl_ret_var = RCL_RET_BAD_ALLOC; \ - break; \ - case RCUTILS_RET_INVALID_ARGUMENT: \ - rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \ - break; \ - case RCUTILS_RET_NOT_INITIALIZED: \ - rcl_ret_var = RCL_RET_NOT_INIT; \ - break; \ - default: \ - rcl_ret_var = RCUTILS_RET_ERROR; \ - } \ - } - typedef struct rosout_map_entry_t { rcl_node_t * node; diff --git a/rcl_action/include/rcl_action/action_client.h b/rcl_action/include/rcl_action/action_client.h index 7fdf6df78..ce896d5d8 100644 --- a/rcl_action/include/rcl_action/action_client.h +++ b/rcl_action/include/rcl_action/action_client.h @@ -24,6 +24,7 @@ extern "C" #include "rcl_action/visibility_control.h" #include "rcl/macros.h" #include "rcl/node.h" +#include "rcl/subscription.h" /// Internal action client implementation struct. @@ -741,6 +742,64 @@ bool rcl_action_client_is_valid( const rcl_action_client_t * action_client); +/// Add a goal uuid. +/** + * This function is to add a goal uuid to the map of rcl_action_client_t + * and then try to set content filtered topic if it is supported. + * + * The caller must provide a lock to call this interface + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] action_client handle to the client that will take the goal response + * \param[in] uuid pointer to a uuid which length is 16 + * \return `RCL_RET_OK` if success on setting a goal uuid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or + * \return `RCL_RET_UNSUPPORTED` if setting content filtered topic is not supported + * in the middleware, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +rcl_ret_t rcl_action_add_goal_uuid( + const rcl_action_client_t * action_client, + const uint8_t * uuid); + +/// Remove a goal uuid. +/** + * This function is to remove a goal uuid from the map of rcl_action_client_t + * and then try to reset content filtered topic if it is supported. + * + * The caller must provide a lock to call this interface + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] action_client handle to the client that will take the goal response + * \param[in] uuid pointer to a uuid which length is 16 + * \return `RCL_RET_OK` if success on removing a goal uuid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or + * \return `RCL_RET_UNSUPPORTED` if setting content filtered topic is not supported + * in the middleware, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +rcl_ret_t rcl_action_remove_goal_uuid( + const rcl_action_client_t * action_client, + const uint8_t * uuid); + #ifdef __cplusplus } #endif diff --git a/rcl_action/include/rcl_action/types.h b/rcl_action/include/rcl_action/types.h index c34bd42db..5a31aa6bf 100644 --- a/rcl_action/include/rcl_action/types.h +++ b/rcl_action/include/rcl_action/types.h @@ -58,6 +58,25 @@ extern "C" #define uuidcmp(uuid0, uuid1) (0 == memcmp(uuid0, uuid1, UUID_SIZE)) #define zerouuid (uint8_t[UUID_SIZE]) {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #define uuidcmpzero(uuid) uuidcmp(uuid, (zerouuid)) +#define UUID_FMT \ + "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X" +#define UUID_FMT_ARGS(uuid) \ + uuid[0], \ + uuid[1], \ + uuid[2], \ + uuid[3], \ + uuid[4], \ + uuid[5], \ + uuid[6], \ + uuid[7], \ + uuid[8], \ + uuid[9], \ + uuid[10], \ + uuid[11], \ + uuid[12], \ + uuid[13], \ + uuid[14], \ + uuid[15] // Typedef generated messages for convenience typedef action_msgs__msg__GoalInfo rcl_action_goal_info_t; diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index a03a61ec3..790da3f92 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -32,6 +32,7 @@ extern "C" #include "rcl/types.h" #include "rcl/wait.h" +#include "rcutils/format_string.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" @@ -63,7 +64,8 @@ _rcl_action_get_zero_initialized_client_impl(void) 0, 0, 0, - 0 + 0, + rcutils_get_zero_initialized_hash_map() }; return null_action_client; } @@ -92,6 +94,29 @@ _rcl_action_client_fini_impl( ret = RCL_RET_ERROR; } allocator.deallocate(action_client->impl->action_name, allocator.state); + if (NULL != action_client->impl->goal_uuids.impl) { + uint8_t uuid[UUID_SIZE]; + char * value = NULL; + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcutils_ret_t hashmap_ret = rcutils_hash_map_get_next_key_and_data( + &action_client->impl->goal_uuids, NULL, uuid, &value); + while (RCUTILS_RET_OK == hashmap_ret) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "remove a uuid: %s", value); + default_allocator.deallocate(value, default_allocator.state); + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_unset(&action_client->impl->goal_uuids, uuid)); + if (ret == RCL_RET_OK) { + hashmap_ret = rcutils_hash_map_get_next_key_and_data( + &action_client->impl->goal_uuids, NULL, uuid, &value); + } + } + if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) { + RCL_RET_FROM_RCUTIL_RET(ret, hashmap_ret); + } + if (RCL_RET_OK == ret) { + RCL_RET_FROM_RCUTIL_RET(ret, rcutils_hash_map_fini(&action_client->impl->goal_uuids)); + } + } allocator.deallocate(action_client->impl, allocator.state); action_client->impl = NULL; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client finalized"); @@ -171,6 +196,26 @@ _rcl_action_client_fini_impl( goto fail; \ } +size_t hash_map_uuid_hash_func(const void * uuid) +{ + const uint8_t * ckey_str = (const uint8_t *) uuid; + size_t hash = 5381; + + for (size_t i = 0; i < UUID_SIZE; ++i) { + const char c = *(ckey_str++); + hash = ((hash << 5) + hash) + (size_t)c; /* hash * 33 + c */ + } + + return hash; +} + +int hash_map_uuid_cmp_func(const void * val1, const void * val2) +{ + const uint8_t * cval1 = (const uint8_t *)val1; + const uint8_t * cval2 = (const uint8_t *)val2; + return memcmp(cval1, cval2, UUID_SIZE); +} + rcl_ret_t rcl_action_client_init( rcl_action_client_t * action_client, @@ -222,6 +267,15 @@ rcl_action_client_init( SUBSCRIPTION_INIT(feedback); SUBSCRIPTION_INIT(status); + // Initialize goal_uuids map + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_init( + &action_client->impl->goal_uuids, 2, sizeof(uint8_t[16]), sizeof(const char **), + hash_map_uuid_hash_func, hash_map_uuid_cmp_func, &allocator)); + if (RCL_RET_OK != ret) { + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Action client initialized"); return ret; fail: @@ -649,6 +703,220 @@ rcl_action_client_wait_set_get_entities_ready( return RCL_RET_OK; } +static char * +to_uuid_string(const uint8_t * uuid, rcl_allocator_t allocator) +{ + char * uuid_str = rcutils_format_string(allocator, UUID_FMT, UUID_FMT_ARGS(uuid)); + return uuid_str; +} + +static +rcl_ret_t set_content_filtered_topic( + const rcl_action_client_t * action_client) +{ + rcl_ret_t ret; + rcutils_ret_t rcutils_ret; + uint8_t uuid[UUID_SIZE]; + char * uuid_str = NULL; + size_t size; + char * feedback_filter = NULL; + char * feedback_filter_update = NULL; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_subscription_content_filter_options_t options = + rcl_subscription_get_default_content_filter_options(); + + // content filter with empty string to reset + feedback_filter = rcutils_strdup("", allocator); + if (NULL == feedback_filter) { + RCL_SET_ERROR_MSG("failed to allocate memory for feedback filter string"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_get_size(&action_client->impl->goal_uuids, &size)); + if (RCL_RET_OK != ret) { + goto clean; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "size: %zu", size); + if (0 == size) { + goto set_cft; + } + + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_get_next_key_and_data( + &action_client->impl->goal_uuids, NULL, uuid, &uuid_str)); + if (RCL_RET_OK != ret) { + goto clean; + } + + feedback_filter_update = + rcutils_format_string(allocator, "goal_id.uuid = &hex(%s)", uuid_str); + if (NULL == feedback_filter_update) { + RCL_SET_ERROR_MSG("failed to format string for feedback filter"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + allocator.deallocate(feedback_filter, allocator.state); + feedback_filter = feedback_filter_update; + + while (RCUTILS_RET_OK == (rcutils_ret = rcutils_hash_map_get_next_key_and_data( + &action_client->impl->goal_uuids, uuid, uuid, &uuid_str))) + { + feedback_filter_update = rcutils_format_string( + allocator, "%s or goal_id.uuid = &hex(%s)", feedback_filter, uuid_str); + if (NULL == feedback_filter_update) { + RCL_SET_ERROR_MSG("failed to format string for feedback filter"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + allocator.deallocate(feedback_filter, allocator.state); + feedback_filter = feedback_filter_update; + } + if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != rcutils_ret) { + RCL_RET_FROM_RCUTIL_RET(ret, rcutils_ret); + } + if (RCL_RET_OK != ret) { + goto clean; + } + +set_cft: + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "feedback_filter: %s", feedback_filter); + + ret = rcl_subscription_content_filter_options_init(feedback_filter, 0, NULL, &options); + if (RCL_RET_OK != ret) { + goto clean; + } + + ret = rcl_subscription_set_content_filter( + &action_client->impl->feedback_subscription, &options); + if (RCL_RET_OK != ret) { + goto clean; + } + +clean: + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("failed to deallocate memory for options"); + } + } + + allocator.deallocate(feedback_filter, allocator.state); + return ret; +} + +rcl_ret_t rcl_action_add_goal_uuid( + const rcl_action_client_t * action_client, + const uint8_t * uuid) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ + } + RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + char * uuid_str = NULL; + rcl_allocator_t allocator = rcl_get_default_allocator(); + uuid_str = to_uuid_string(uuid, allocator); + if (NULL == uuid_str) { + RCL_SET_ERROR_MSG("failed to allocate memory for uuid value"); + ret = RCL_RET_BAD_ALLOC; + goto end; + } + + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_set(&action_client->impl->goal_uuids, uuid, &uuid_str)); + if (RCL_RET_OK != ret) { + goto clean; + } + + // to set content filtered topic + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "set content filtered topic after adding a uuid: %s", uuid_str); + ret = set_content_filtered_topic(action_client); + if (RCL_RET_OK != ret) { + char * err = rcutils_strdup(rcl_get_error_string().str, allocator); + if (NULL == err) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "failed to allocate memory for error"); + ret = RCL_RET_BAD_ALLOC; + goto clean; + } + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to set_content_filtered_topic: %s", err); + allocator.deallocate(err, allocator.state); + } + goto end; + +clean: + allocator.deallocate(uuid_str, allocator.state); + +end: + return ret; +} + +rcl_ret_t rcl_action_remove_goal_uuid( + const rcl_action_client_t * action_client, + const uint8_t * uuid) +{ + if (!rcl_action_client_is_valid(action_client)) { + return RCL_RET_ACTION_CLIENT_INVALID; /* error already set */ + } + RCL_CHECK_ARGUMENT_FOR_NULL(uuid, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + char * uuid_str = NULL; + char * uuid_value = NULL; + rcl_allocator_t allocator = rcl_get_default_allocator(); + uuid_str = to_uuid_string(uuid, allocator); + if (NULL == uuid_str) { + RCL_SET_ERROR_MSG("failed to allocate memory for uuid value"); + ret = RCL_RET_BAD_ALLOC; + goto end; + } + if (!rcutils_hash_map_key_exists(&action_client->impl->goal_uuids, uuid)) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "item key [%s] not found in the map of goal uuids", + uuid_str); + ret = RCL_RET_ERROR; + goto end; + } + + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_get(&action_client->impl->goal_uuids, uuid, &uuid_value)); + if (RCL_RET_OK != ret) { + goto end; + } + allocator.deallocate(uuid_value, allocator.state); + + RCL_RET_FROM_RCUTIL_RET( + ret, rcutils_hash_map_unset(&action_client->impl->goal_uuids, uuid)); + if (RCL_RET_OK != ret) { + goto end; + } + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "set content filtered topic after removing a uuid: %s", uuid_str); + ret = set_content_filtered_topic(action_client); + if (RCL_RET_OK != ret) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * err = rcutils_strdup(rcl_get_error_string().str, allocator); + if (NULL == err) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "failed to allocate memory for error"); + ret = RCL_RET_BAD_ALLOC; + goto end; + } + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("failed to set_content_filtered_topic: %s", err); + allocator.deallocate(err, allocator.state); + } + +end: + allocator.deallocate(uuid_str, allocator.state); + return ret; +} + #ifdef __cplusplus } #endif diff --git a/rcl_action/src/rcl_action/action_client_impl.h b/rcl_action/src/rcl_action/action_client_impl.h index 777416bfd..c16fa59e9 100644 --- a/rcl_action/src/rcl_action/action_client_impl.h +++ b/rcl_action/src/rcl_action/action_client_impl.h @@ -18,6 +18,8 @@ #include "rcl_action/types.h" #include "rcl/rcl.h" +#include "rcutils/types.h" + typedef struct rcl_action_client_impl_s { rcl_client_t goal_client; @@ -33,6 +35,7 @@ typedef struct rcl_action_client_impl_s size_t wait_set_result_client_index; size_t wait_set_feedback_subscription_index; size_t wait_set_status_subscription_index; + rcutils_hash_map_t goal_uuids; } rcl_action_client_impl_t; diff --git a/rcl_action/test/rcl_action/test_action_communication.cpp b/rcl_action/test/rcl_action/test_action_communication.cpp index c13685ebc..1e485b02b 100644 --- a/rcl_action/test/rcl_action/test_action_communication.cpp +++ b/rcl_action/test/rcl_action/test_action_communication.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "osrf_testing_tools_cpp/scope_exit.hpp" @@ -727,6 +728,106 @@ TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal test_msgs__action__Fibonacci_SendGoal_Request__fini(&incoming_goal_request); } +TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_valid_feedback_comm_cft) +{ + #define FEEDBACK_SIZE 2 + test_msgs__action__Fibonacci_FeedbackMessage outgoing_feedback[FEEDBACK_SIZE]; + test_msgs__action__Fibonacci_FeedbackMessage incoming_feedback[FEEDBACK_SIZE]; + for (size_t i = 0; i < FEEDBACK_SIZE; ++i) { + test_msgs__action__Fibonacci_FeedbackMessage__init(&outgoing_feedback[i]); + test_msgs__action__Fibonacci_FeedbackMessage__init(&incoming_feedback[i]); + } + + uint8_t uuid[UUID_SIZE]; + init_test_uuid0(uuid); + rcl_ret_t ret = rcl_action_add_goal_uuid(&this->action_client, uuid); + bool if_cft_supported = false; + if (ret != RMW_RET_OK) { + ASSERT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str; + } else { + if_cft_supported = true; + } + + // Initialize feedback + // set uuid of feedback with uuid0 if index is 0, otherwise the uuid of feedback is uuid1 + for (size_t i = 0; i < FEEDBACK_SIZE; ++i) { + ASSERT_TRUE( + rosidl_runtime_c__int32__Sequence__init( + &outgoing_feedback[i].feedback.sequence, 3)); + outgoing_feedback[i].feedback.sequence.data[0] = 0; + outgoing_feedback[i].feedback.sequence.data[1] = 1; + outgoing_feedback[i].feedback.sequence.data[2] = 2; + i == 0 ? init_test_uuid0(outgoing_feedback[i].goal_id.uuid) : + init_test_uuid1(outgoing_feedback[i].goal_id.uuid); + } + + // Publish feedback with valid arguments + for (size_t i = 0; i < FEEDBACK_SIZE; ++i) { + ret = rcl_action_publish_feedback(&this->action_server, &outgoing_feedback[i]); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_wait_set_clear(&this->wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_action_wait_set_add_action_client( + &this->wait_set, &this->action_client, NULL, NULL); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_wait(&this->wait_set, RCL_S_TO_NS(10)); + // if content filter is unsupported, the action client will receive different action feedback + // message, otherwise it will only receive the matched uuid0 feedback message. + if (!if_cft_supported || 0 == i) { + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_action_client_wait_set_get_entities_ready( + &this->wait_set, + &this->action_client, + &this->is_feedback_ready, + &this->is_status_ready, + &this->is_goal_response_ready, + &this->is_cancel_response_ready, + &this->is_result_response_ready); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + EXPECT_TRUE(this->is_feedback_ready); + EXPECT_FALSE(this->is_status_ready); + EXPECT_FALSE(this->is_result_response_ready); + EXPECT_FALSE(this->is_cancel_response_ready); + EXPECT_FALSE(this->is_goal_response_ready); + + // Take feedback with valid arguments + ret = rcl_action_take_feedback(&this->action_client, &incoming_feedback[i]); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // Check that feedback was received correctly + EXPECT_TRUE( + uuidcmp( + outgoing_feedback[i].goal_id.uuid, + incoming_feedback[i].goal_id.uuid)); + ASSERT_EQ( + outgoing_feedback[i].feedback.sequence.size, incoming_feedback[i].feedback.sequence.size); + EXPECT_TRUE( + !memcmp( + outgoing_feedback[i].feedback.sequence.data, + incoming_feedback[i].feedback.sequence.data, + outgoing_feedback[i].feedback.sequence.size)); + } else { + EXPECT_EQ(ret, RCL_RET_TIMEOUT) << rcl_get_error_string().str; + } + } + + ret = rcl_action_remove_goal_uuid(&this->action_client, uuid); + if (if_cft_supported) { + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } else { + EXPECT_EQ(ret, RCL_RET_UNSUPPORTED) << rcl_get_error_string().str; + } + + for (size_t i = 0; i < FEEDBACK_SIZE; ++i) { + test_msgs__action__Fibonacci_FeedbackMessage__fini(&outgoing_feedback[i]); + test_msgs__action__Fibonacci_FeedbackMessage__fini(&incoming_feedback[i]); + } +} + TEST_F(CLASSNAME(TestActionCommunication, RMW_IMPLEMENTATION), test_invalid_goal_response_opts) { test_msgs__action__Fibonacci_SendGoal_Response outgoing_goal_response;