From 20165572b6c22e3d44c5c16ad7192fa9695aed6b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 2 Dec 2020 15:26:40 +0800 Subject: [PATCH 01/34] 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 121949999..cbeacd072 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -28,6 +28,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" @@ -209,6 +210,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 32c3ca767..146868db8 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 6a6aa81c1..01353f9a9 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1125,6 +1125,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(); @@ -1136,6 +1138,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 593a4bfb918fceb985c4064cd0856620942ff10f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 3 Dec 2020 15:58:21 +0800 Subject: [PATCH 02/34] 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 cbeacd072..20df9f173 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -281,9 +281,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. @@ -314,9 +313,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 fc651ec352bec89627e450a2f589fc9e2c2eb967 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 2 Feb 2021 14:28:20 +0800 Subject: [PATCH 03/34] 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 20df9f173..47ff35c13 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -291,6 +291,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 @@ -327,6 +328,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 146868db8..753597df8 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 1af2e837e6d8157848fe82e12a074ac45dd4ba43 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 22 Feb 2021 11:05:04 +0800 Subject: [PATCH 04/34] 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 47ff35c13..d26f5cedc 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -250,7 +250,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 @@ -283,14 +283,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. */ @@ -324,9 +322,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 753597df8..f1f91f14e 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 01353f9a9..fc58dd5f5 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1166,7 +1166,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( @@ -1178,7 +1178,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 953ad14c85ff2f984034a7e7ce08c3f28ebad7f7 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 Feb 2021 15:27:47 +0800 Subject: [PATCH 05/34] 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 f1f91f14e..ba332fddd 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 9626a6cc901a195440b0649ff11df84146ad3a17 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 Feb 2021 18:30:58 +0800 Subject: [PATCH 06/34] 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 d26f5cedc..d83d01c91 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -210,34 +210,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 ba332fddd..639245fb1 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 da7be139d00a06217eca9c4ef2ee3bb1d9adc2cf Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 25 Feb 2021 17:07:51 +0800 Subject: [PATCH 07/34] 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 fc58dd5f5..760c8e907 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1142,7 +1142,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( @@ -1188,7 +1188,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 14ff61ea23ded908c55f9abefb20dd12be63d45b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 11 Mar 2021 16:53:12 +0800 Subject: [PATCH 08/34] 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 d83d01c91..64dff6050 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -230,7 +230,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. @@ -255,9 +255,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 0aaf6e1c1f25cd84c2e6d16bfceb917fb8dd75f9 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 12 Mar 2021 13:09:39 +0800 Subject: [PATCH 09/34] 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 760c8e907..7d3fdc2e3 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" @@ -885,6 +886,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 190948c14de8fefc7ca0a06654a8483ff2662cc6 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 11 Oct 2021 14:40:39 +0800 Subject: [PATCH 10/34] 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 64dff6050..b32ab8fa3 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -28,7 +28,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" @@ -54,6 +53,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 @@ -210,6 +219,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. /** *
@@ -228,23 +300,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 @@ -254,19 +328,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. */ @@ -275,8 +341,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. @@ -291,16 +356,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. @@ -310,8 +372,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 639245fb1..6ec03e649 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 91e9e44bf..7d9ebdd6a 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -443,3 +443,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 7d3fdc2e3..d088aa50d 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -896,6 +896,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 = @@ -911,8 +912,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( @@ -921,9 +927,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"; @@ -981,25 +987,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 @@ -1057,38 +1071,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 @@ -1139,7 +1179,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); @@ -1166,7 +1206,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 @@ -1197,21 +1237,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; @@ -1508,7 +1562,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)); @@ -1521,7 +1575,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(); } @@ -1533,40 +1587,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(); } } @@ -1579,31 +1649,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( @@ -1611,8 +1671,7 @@ TEST_F( EXPECT_EQ( RMW_RET_UNSUPPORTED, rcl_subscription_get_cft_expression_parameters( - &subscription, &filter_expression, - ¶meters)); + &subscription, &options)); rcl_reset_error(); } @@ -1620,10 +1679,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 6381b080bca3081307782f821b006ff36604c1b8 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 13 Oct 2021 17:52:16 +0800 Subject: [PATCH 11/34] 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 b32ab8fa3..2169fcd7d 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -219,7 +219,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 @@ -282,7 +296,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 @@ -292,7 +306,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 6ec03e649..ab45388b8 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 d088aa50d..5bf50d1ba 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -894,7 +894,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; @@ -931,7 +933,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; @@ -943,9 +945,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); @@ -970,7 +972,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; @@ -1027,9 +1029,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); @@ -1054,7 +1056,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; @@ -1118,7 +1120,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); @@ -1141,7 +1143,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; @@ -1159,7 +1161,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( @@ -1181,7 +1183,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(); @@ -1207,7 +1211,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"; @@ -1220,7 +1224,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; @@ -1277,9 +1281,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); @@ -1305,7 +1309,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 154ed6fd338e133c55c38b293a19e16653b20b69 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 09:36:23 +0800 Subject: [PATCH 12/34] 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 680d0c22f03d8811978246e5642c984a057985ed Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:24:10 +0800 Subject: [PATCH 13/34] 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 2169fcd7d..48a67ccba 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -55,7 +55,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 @@ -232,14 +232,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 @@ -373,7 +382,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 ab45388b8..672e61da9 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 5bf50d1ba..842abf75b 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -893,8 +893,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 ); @@ -934,6 +934,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; @@ -1103,7 +1105,6 @@ TEST_F( } } - // reset filter { rcl_subscription_content_filtered_topic_options_t options = @@ -1133,7 +1134,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); @@ -1182,8 +1183,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 ); @@ -1211,9 +1212,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; @@ -1243,7 +1255,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 069b196d5df5808d20cc11b54dada3eed5780495 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:32:44 +0800 Subject: [PATCH 14/34] 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 48a67ccba..9884b26a1 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -232,7 +232,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 @@ -247,7 +247,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 328e8ffdfd0435f6a63f5a11573ce03367b31689 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:42:52 +0800 Subject: [PATCH 15/34] 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 9884b26a1..dd015e796 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -330,8 +330,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 842abf75b..677bab062 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -894,7 +894,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 ); @@ -993,7 +993,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(); @@ -1184,7 +1184,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 ); @@ -1220,7 +1220,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 65f53dd9a0d851f96974cae02af77ac9b59cac8f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 13:50:51 +0800 Subject: [PATCH 16/34] 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 f3364d9d1a9f01620fc64485ff286d110dec0145 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 26 Oct 2021 14:24:17 +0800 Subject: [PATCH 17/34] 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 dd015e796..a9726bd3d 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -53,15 +53,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`. /** @@ -239,7 +238,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. @@ -252,25 +251,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. /** @@ -290,22 +289,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 @@ -323,8 +322,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. /** @@ -350,20 +349,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. @@ -379,9 +378,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 @@ -392,9 +391,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 672e61da9..108c9c024 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 7d9ebdd6a..ffd7f7b42 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -444,8 +444,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 677bab062..c115d59a1 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -917,7 +917,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) ); @@ -995,17 +995,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; @@ -1015,7 +1015,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1076,16 +1076,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); @@ -1097,8 +1097,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); @@ -1107,17 +1107,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; @@ -1129,7 +1129,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1215,11 +1215,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 { @@ -1261,17 +1261,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; @@ -1281,7 +1281,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filtered_topic_options_fini( + rcl_subscription_content_filter_options_fini( &options) ); } @@ -1599,33 +1599,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, @@ -1636,71 +1636,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 555061592896ca430c22a3e10ff2e0697502cde2 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 13 Jan 2022 18:04:58 +0800 Subject: [PATCH 18/34] 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 c115d59a1..e392fb487 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -893,11 +893,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(); @@ -928,11 +923,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 @@ -946,7 +937,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)); @@ -1007,7 +998,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); @@ -1030,7 +1021,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)); @@ -1081,7 +1072,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 = @@ -1119,7 +1110,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)); @@ -1155,26 +1146,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)); } } @@ -1183,11 +1157,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 = @@ -1220,11 +1189,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)); @@ -1260,6 +1225,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(); @@ -1273,10 +1239,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( @@ -1296,7 +1262,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 bdc50e6375e6be4a66fb456182190e8e1f613453 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 13 Jan 2022 18:28:23 +0800 Subject: [PATCH 19/34] 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 e392fb487..b3c620449 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1239,7 +1239,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 1667f3a6695313903ed1034d7727d4f2732210bd Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 17 Mar 2022 11:13:39 +0800 Subject: [PATCH 20/34] relate to `rcutils_string_array_t expression_parameters` changed in rmw Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 11 ++------- rcl/test/rcl/test_subscription.cpp | 5 ++-- ...st_subscription_content_filter_options.cpp | 24 +++++++++---------- 3 files changed, 16 insertions(+), 24 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 108c9c024..b88882a09 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -422,17 +422,10 @@ rcl_subscription_set_content_filter( // copy options into subscription_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_filter_options->expression_parameters) { - expression_parameters_size = content_filter_options->expression_parameters->size; - expression_parameters = - (const char **)content_filter_options->expression_parameters->data; - } return rcl_subscription_options_set_content_filter_options( content_filter_options->filter_expression, - expression_parameters_size, - expression_parameters, + content_filter_options->expression_parameters.size, + (const char **)content_filter_options->expression_parameters.data, &subscription->impl->options ); } diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index b3c620449..b499104cd 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1079,11 +1079,10 @@ TEST_F( 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); - ASSERT_EQ(expression_parameters2_count, options->expression_parameters->size); + 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], + options->expression_parameters.data[i], expression_parameters2[i]); } EXPECT_EQ( diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp index d42b9b71c..1e6c9456c 100644 --- a/rcl/test/rcl/test_subscription_content_filter_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp @@ -67,7 +67,8 @@ TEST(TestSubscriptionContentFilterOptions, subscription_options_success) 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); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); } const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; @@ -88,13 +89,12 @@ TEST(TestSubscriptionContentFilterOptions, subscription_options_success) 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); + 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], + content_filter_options->expression_parameters.data[i], expression_parameters2[i]); } } @@ -181,7 +181,8 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) 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); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); // set with filter_expression1_update EXPECT_EQ( @@ -194,7 +195,8 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) 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); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); } const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; @@ -224,13 +226,12 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) 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); + 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], + content_filter_options->expression_parameters.data[i], expression_parameters2[i]); } @@ -246,13 +247,12 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) 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); + 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], + content_filter_options->expression_parameters.data[i], expression_parameters2_update[i]); } } From e03e121388d50c04a6ffc9cfe07a2a390a11f469 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 24 Mar 2022 09:42:51 +0800 Subject: [PATCH 21/34] add necessary structure for fallback interfaces Signed-off-by: Chen Lihui --- rcl/include/rcl/context.h | 25 +++++++++++++++++++++++++ rcl/src/rcl/context.c | 11 +++++++++++ rcl/src/rcl/context_impl.h | 2 ++ rcl/src/rcl/init.c | 2 ++ rcl/src/rcl/subscription.c | 19 +++++++++++++++++++ rcl/src/rcl/subscription_impl.h | 4 ++++ 6 files changed, 63 insertions(+) diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 68500da81..c8327448b 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -43,6 +43,9 @@ extern "C" /// A unique ID per context instance. typedef uint64_t rcl_context_instance_id_t; +/// Common content filter factory (Defined in a new package) +typedef struct common_content_filter_factory_s common_content_filter_factory_t; + typedef struct rcl_context_impl_s rcl_context_impl_t; /// Encapsulates the non-global state of an init/shutdown cycle. @@ -314,6 +317,28 @@ RCL_WARN_UNUSED rmw_context_t * rcl_context_get_rmw_context(rcl_context_t * context); +/// Return pointer to the common content filter factory +/** + * If context is `NULL`, then `NULL` is returned. + * If context is zero-initialized, then `NULL` is returned. + * If context is uninitialized, then it is undefined behavior. + * + * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] context object from which the common content filter factory should be retrieved. + * \return pointer to common content filter factory if valid, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +common_content_filter_factory_t * +rcl_context_get_common_content_filter_factory(rcl_context_t * context); + + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 5414cc5e9..0aa4c55c8 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -105,6 +105,14 @@ rcl_context_get_rmw_context(rcl_context_t * context) return &(context->impl->rmw_context); } +common_content_filter_factory_t * +rcl_context_get_common_content_filter_factory(rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(context->impl, "context is zero-initialized", return NULL); + return context->impl->common_content_filter_factory; +} + rcl_ret_t __cleanup_context(rcl_context_t * context) { @@ -172,6 +180,9 @@ __cleanup_context(rcl_context_t * context) } allocator.deallocate(context->impl->argv, allocator.state); } + + // TODO(iuhilnehc-ynos): finalize context->impl->common_content_filter_factory. + allocator.deallocate(context->impl, allocator.state); } // if (NULL != context->impl) diff --git a/rcl/src/rcl/context_impl.h b/rcl/src/rcl/context_impl.h index 10c9e82cc..a327ac9f1 100644 --- a/rcl/src/rcl/context_impl.h +++ b/rcl/src/rcl/context_impl.h @@ -38,6 +38,8 @@ struct rcl_context_impl_s char ** argv; /// rmw context. rmw_context_t rmw_context; + /// Common content filter factory + common_content_filter_factory_t * common_content_filter_factory; }; RCL_LOCAL diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 92cd25dc4..960152826 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -123,6 +123,8 @@ rcl_init( } } + // TODO(iuhilnehc-ynos): initialize context->impl->common_content_filter_factory. + // Parse the ROS specific arguments. ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments); if (RCL_RET_OK != ret) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index b88882a09..7d4c78802 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -69,6 +69,7 @@ rcl_subscription_init( RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } + // TODO(iuhilnehc-ynos): check the expression_parameters_argc < 100 in content filter options. // Expand and remap the given topic name. char * remapped_topic_name = NULL; @@ -120,6 +121,8 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; + // TODO(iuhilnehc-ynos): initialize the `subscription->impl->common_content_filter` with + // common_content_filter_factory by rcl_context. RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -184,6 +187,8 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } + // TODO(iuhilnehc-ynos): finalize the `subscription->impl->common_content_filter` with + // common_content_filter_factory by rcl_context. 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); @@ -240,6 +245,7 @@ rcl_subscription_options_set_content_filter_options( rcl_subscription_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + // TODO(iuhilnehc-ynos): check the expression_parameters_argc < 100. 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); @@ -415,6 +421,9 @@ rcl_subscription_set_content_filter( options->rmw_subscription_content_filter_options); if (ret != RMW_RET_OK) { + // TODO(iuhilnehc-ynos): to call common_content_filter_set_content_filter(...) of + // subscription->impl->common_content_filter. + RCL_SET_ERROR_MSG(rmw_get_error_string().str); return rcl_convert_rmw_ret_to_rcl_ret(ret); } @@ -461,6 +470,9 @@ rcl_subscription_get_content_filter( &options->allocator, content_filter_options); if (rmw_ret != RMW_RET_OK) { + // TODO(iuhilnehc-ynos): to call common_content_filter_get_content_filter(...) of + // subscription->impl->common_content_filter. + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); goto failed; } @@ -506,6 +518,9 @@ rcl_take( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of + // subscription->impl->common_content_filter. + return RCL_RET_OK; } @@ -552,6 +567,8 @@ rcl_take_sequence( if (0u == taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of + // subscription->impl->common_content_filter. return RCL_RET_OK; } @@ -621,6 +638,8 @@ rcl_take_loaned_message( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of + // subscription->impl->common_content_filter. return RCL_RET_OK; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 0fe962ab4..2957f2d2f 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -19,11 +19,15 @@ #include "rcl/subscription.h" +/// Common content filter. (Defined in other package) +typedef struct common_content_filter_s common_content_filter_t; + struct rcl_subscription_impl_s { rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; + common_content_filter_t * common_content_filter; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ From 100d4a012d4c7409725fb2162e8b10f06c56b046 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 24 Mar 2022 09:58:20 +0800 Subject: [PATCH 22/34] remove the implementation temporary, add them with fallback in the feature use stack/inline storage Signed-off-by: Chen Lihui --- rcl/include/rcl/context.h | 1 - rcl/include/rcl/subscription.h | 25 +- rcl/src/rcl/context.c | 3 - rcl/src/rcl/init.c | 2 - rcl/src/rcl/subscription.c | 141 +---- rcl/test/rcl/test_subscription.cpp | 489 +----------------- ...st_subscription_content_filter_options.cpp | 16 +- 7 files changed, 44 insertions(+), 633 deletions(-) diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index c8327448b..15e561309 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -338,7 +338,6 @@ RCL_WARN_UNUSED common_content_filter_factory_t * rcl_context_get_common_content_filter_factory(rcl_context_t * context); - #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index a9726bd3d..61437d63f 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -59,7 +59,7 @@ typedef struct rcl_subscription_content_filter_options_s /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ rcl_allocator_t allocator; /// rmw specific subscription content filter options - rmw_subscription_content_filter_options_t * rmw_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`. @@ -257,19 +257,11 @@ rcl_subscription_options_set_content_filter_options( const char * expression_parameter_argv[], rcl_subscription_options_t * options); -/// Return the default subscription content filter options. -/** - * The defaults are: - * - * - allocator = rcl_get_default_allocator() - * - rmw_subscription_content_filter_options = NULL; - * - * \return A structure containing the default options for a subscription. - */ +/// Return the zero initialized subscription content filter options. RCL_PUBLIC RCL_WARN_UNUSED rcl_subscription_content_filter_options_t -rcl_subscription_get_default_content_filter_options(void); +rcl_get_zero_initialized_subscription_content_filter_options(void); /// Allocate. /** @@ -325,17 +317,6 @@ rcl_ret_t 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. -/** - * Depending on the middleware and whether cft is enabled in the subscription. - * - * \return `true` if the content filtered topic of `subscription` is enabled, otherwise `false` - */ -RCL_PUBLIC -RCL_WARN_UNUSED -bool -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 diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index 0aa4c55c8..dcf970875 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -180,9 +180,6 @@ __cleanup_context(rcl_context_t * context) } allocator.deallocate(context->impl->argv, allocator.state); } - - // TODO(iuhilnehc-ynos): finalize context->impl->common_content_filter_factory. - allocator.deallocate(context->impl, allocator.state); } // if (NULL != context->impl) diff --git a/rcl/src/rcl/init.c b/rcl/src/rcl/init.c index 960152826..92cd25dc4 100644 --- a/rcl/src/rcl/init.c +++ b/rcl/src/rcl/init.c @@ -123,8 +123,6 @@ rcl_init( } } - // TODO(iuhilnehc-ynos): initialize context->impl->common_content_filter_factory. - // Parse the ROS specific arguments. ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments); if (RCL_RET_OK != ret) { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 7d4c78802..da14dd2dc 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -69,7 +69,6 @@ rcl_subscription_init( RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); return RCL_RET_ALREADY_INIT; } - // TODO(iuhilnehc-ynos): check the expression_parameters_argc < 100 in content filter options. // Expand and remap the given topic name. char * remapped_topic_name = NULL; @@ -121,8 +120,6 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; - // TODO(iuhilnehc-ynos): initialize the `subscription->impl->common_content_filter` with - // common_content_filter_factory by rcl_context. RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; @@ -187,8 +184,6 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) RCL_SET_ERROR_MSG(rmw_get_error_string().str); result = RCL_RET_ERROR; } - // TODO(iuhilnehc-ynos): finalize the `subscription->impl->common_content_filter` with - // common_content_filter_factory by rcl_context. 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); @@ -245,7 +240,6 @@ rcl_subscription_options_set_content_filter_options( rcl_subscription_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); - // TODO(iuhilnehc-ynos): check the expression_parameters_argc < 100. 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); @@ -298,12 +292,13 @@ rcl_subscription_options_set_content_filter_options( } rcl_subscription_content_filter_options_t -rcl_subscription_get_default_content_filter_options() +rcl_get_zero_initialized_subscription_content_filter_options() { - static rcl_subscription_content_filter_options_t default_options; - default_options.allocator = rcl_get_default_allocator(); - default_options.rmw_subscription_content_filter_options = NULL; - return default_options; + return (const rcl_subscription_content_filter_options_t) { + .allocator = rcl_get_default_allocator(), + .rmw_subscription_content_filter_options = + rmw_get_zero_initialized_content_filter_options() + }; // NOLINT(readability/braces): false positive } rcl_ret_t @@ -317,34 +312,15 @@ rcl_subscription_content_filter_options_init( 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_filter_options_t * content_filter_options = - allocator->allocate( - 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_filter_options = rmw_get_zero_initialized_content_filter_options(); - rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init( filter_expression, expression_parameters_argc, expression_parameter_argv, allocator, - content_filter_options + &options->rmw_subscription_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_filter_options = content_filter_options; - return RCL_RET_OK; - -failed: - allocator->deallocate(content_filter_options, allocator->state); - return ret; + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } rcl_ret_t @@ -364,7 +340,7 @@ rcl_subscription_content_filter_options_set( expression_parameters_argc, expression_parameter_argv, allocator, - options->rmw_subscription_content_filter_options + &options->rmw_subscription_content_filter_options ); return rcl_convert_rmw_ret_to_rcl_ret(ret); } @@ -378,25 +354,11 @@ rcl_subscription_content_filter_options_fini( RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rmw_ret_t ret = rmw_subscription_content_filter_options_fini( - options->rmw_subscription_content_filter_options, + &options->rmw_subscription_content_filter_options, allocator ); - if (ret != RMW_RET_OK) { - return rcl_convert_rmw_ret_to_rcl_ret(ret); - } - - allocator->deallocate(options->rmw_subscription_content_filter_options, allocator->state); - return RCL_RET_OK; -} - -bool -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_enabled; + return rcl_convert_rmw_ret_to_rcl_ret(ret); } rcl_ret_t @@ -405,38 +367,10 @@ rcl_subscription_set_content_filter( const rcl_subscription_content_filter_options_t * options ) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + (void)subscription; + (void)options; - if (!rcl_subscription_is_valid(subscription)) { - return RCL_RET_SUBSCRIPTION_INVALID; - } - - 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_content_filter( - subscription->impl->rmw_handle, - options->rmw_subscription_content_filter_options); - - if (ret != RMW_RET_OK) { - // TODO(iuhilnehc-ynos): to call common_content_filter_set_content_filter(...) of - // subscription->impl->common_content_filter. - - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return rcl_convert_rmw_ret_to_rcl_ret(ret); - } - - // copy options into subscription_options - rmw_subscription_content_filter_options_t * content_filter_options = - options->rmw_subscription_content_filter_options; - return rcl_subscription_options_set_content_filter_options( - content_filter_options->filter_expression, - content_filter_options->expression_parameters.size, - (const char **)content_filter_options->expression_parameters.data, - &subscription->impl->options - ); + return RCL_RET_UNSUPPORTED; } rcl_ret_t @@ -445,45 +379,10 @@ rcl_subscription_get_content_filter( rcl_subscription_content_filter_options_t * options ) { - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); - RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + (void)subscription; + (void)options; - if (!rcl_subscription_is_valid(subscription)) { - return RCL_RET_SUBSCRIPTION_INVALID; - } - 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_filter_options_t * content_filter_options = - allocator->allocate( - 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_filter_options = rmw_get_zero_initialized_content_filter_options(); - - rmw_ret_t rmw_ret = rmw_subscription_get_content_filter( - subscription->impl->rmw_handle, - &options->allocator, content_filter_options); - - if (rmw_ret != RMW_RET_OK) { - // TODO(iuhilnehc-ynos): to call common_content_filter_get_content_filter(...) of - // subscription->impl->common_content_filter. - - ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto failed; - } - - options->rmw_subscription_content_filter_options = content_filter_options; - - return RCL_RET_OK; - -failed: - allocator->deallocate(content_filter_options, allocator->state); - return ret; + return RCL_RET_UNSUPPORTED; } rcl_ret_t @@ -518,8 +417,6 @@ rcl_take( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of - // subscription->impl->common_content_filter. return RCL_RET_OK; } @@ -567,8 +464,6 @@ rcl_take_sequence( if (0u == taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of - // subscription->impl->common_content_filter. return RCL_RET_OK; } @@ -638,8 +533,6 @@ rcl_take_loaned_message( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // TODO(iuhilnehc-ynos): to call common_content_filter_evaluate(...) of - // subscription->impl->common_content_filter. return RCL_RET_OK; } diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index b499104cd..c1a084242 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -886,427 +886,6 @@ 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) { - 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 = - 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(); - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_options_set_content_filter_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( - { - rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - 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 - 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_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)); - - 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, 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; - ASSERT_EQ( - std::string(test_filtered_string), - std::string(msg.string_value.data, msg.string_value.size)); - } - - // 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 *); - { - rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_init( - filter_expression2, expression_parameters2_count, expression_parameters2, - &options) - ); - - ret = rcl_subscription_set_content_filter( - &subscription, &options); - if (is_cft_support) { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - } - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_fini( - &options) - ); - } - - // 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_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)); - - 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, 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; - ASSERT_EQ( - std::string(test_filtered_other_string), - std::string(msg.string_value.data, msg.string_value.size)); - } - - // get filter - { - rcl_subscription_content_filter_options_t content_filter_options = - rcl_subscription_get_default_content_filter_options(); - - ret = rcl_subscription_get_content_filter( - &subscription, &content_filter_options); - if (is_cft_support) { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - 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_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_filter_options_fini( - &content_filter_options) - ); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - } - } - - // reset filter - { - rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_init( - "", 0, nullptr, - &options) - ); - - ret = rcl_subscription_set_content_filter( - &subscription, &options); - 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)); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - } - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_fini( - &options) - ); - } - - // publish with a non-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, 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; - ASSERT_EQ( - std::string(test_string), - std::string(msg.string_value.data, msg.string_value.size)); - } -} - -/* A subscription without a content filtered topic setting at beginning. - */ -TEST_F( - CLASSNAME( - TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { - 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_enabled(&subscription)); - - // failed to get filter - { - rcl_subscription_content_filter_options_t content_filter_options = - rcl_subscription_get_default_content_filter_options(); - - ret = rcl_subscription_get_content_filter( - &subscription, &content_filter_options); - ASSERT_NE(RCL_RET_OK, ret); - } - - 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; - 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, 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; - ASSERT_EQ( - std::string(test_string), - std::string(msg.string_value.data, msg.string_value.size)); - } - - // 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 *); - bool is_cft_support{true}; - { - rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_init( - filter_expression2, expression_parameters2_count, expression_parameters2, - &options) - ); - - ret = rcl_subscription_set_content_filter( - &subscription, &options); - if (RCL_RET_UNSUPPORTED == ret) { - is_cft_support = false; - } else { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - - EXPECT_EQ( - RCL_RET_OK, - rcl_subscription_content_filter_options_fini( - &options) - ); - } - - // 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_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)); - - 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, 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; - 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 = @@ -1547,8 +1126,6 @@ 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_enabled(nullptr)); - rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); rcl_reset_error(); @@ -1560,8 +1137,6 @@ 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_enabled(&subscription_zero_init)); - rcl_reset_error(); } /* Test for all failure modes in rcl_subscription_set_content_filter function. @@ -1571,27 +1146,27 @@ TEST_F( TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_content_filter) { EXPECT_EQ( - RCL_RET_SUBSCRIPTION_INVALID, + RMW_RET_UNSUPPORTED, rcl_subscription_set_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( - RCL_RET_SUBSCRIPTION_INVALID, + RMW_RET_UNSUPPORTED, rcl_subscription_set_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, + RMW_RET_UNSUPPORTED, rcl_subscription_set_content_filter(&subscription, nullptr)); rcl_reset_error(); // an options used later rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); + rcl_get_zero_initialized_subscription_content_filter_options(); EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( - "data MATCH '0'", + "data = '0'", 0, nullptr, &options @@ -1605,25 +1180,10 @@ TEST_F( ); }); - { - auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); - EXPECT_EQ( - RMW_RET_UNSUPPORTED, - rcl_subscription_set_content_filter( - &subscription, &options)); - rcl_reset_error(); - } - - { - auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); - EXPECT_EQ( - RMW_RET_ERROR, - rcl_subscription_set_content_filter( - &subscription, &options)); - rcl_reset_error(); - } + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_set_content_filter(&subscription, &options)); + rcl_reset_error(); } /* Test for all failure modes in rcl_subscription_get_content_filter function. @@ -1633,42 +1193,27 @@ TEST_F( TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_content_filter) { EXPECT_EQ( - RCL_RET_SUBSCRIPTION_INVALID, + RMW_RET_UNSUPPORTED, rcl_subscription_get_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( - RCL_RET_SUBSCRIPTION_INVALID, + RMW_RET_UNSUPPORTED, rcl_subscription_get_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( - RCL_RET_INVALID_ARGUMENT, + RMW_RET_UNSUPPORTED, rcl_subscription_get_content_filter(&subscription, nullptr)); rcl_reset_error(); 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_content_filter, RMW_RET_UNSUPPORTED); - EXPECT_EQ( - RMW_RET_UNSUPPORTED, - rcl_subscription_get_content_filter( - &subscription, &options)); - rcl_reset_error(); - } + rcl_get_zero_initialized_subscription_content_filter_options(); - { - auto mock = mocking_utils::patch_and_return( - "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); - EXPECT_EQ( - RMW_RET_ERROR, - rcl_subscription_get_content_filter( - &subscription, &options)); - rcl_reset_error(); - } + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_get_content_filter(&subscription, &options)); + rcl_reset_error(); } TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail) diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp index 1e6c9456c..3108a5798 100644 --- a/rcl/test/rcl/test_subscription_content_filter_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp @@ -107,7 +107,7 @@ TEST(TestSubscriptionContentFilterOptions, subscription_options_success) TEST(TestSubscriptionContentFilterOptions, content_filter_options_failure) { rcl_subscription_content_filter_options_t content_filter_options = - rcl_subscription_get_default_content_filter_options(); + rcl_get_zero_initialized_subscription_content_filter_options(); const char * filter_expression1 = "filter=1"; EXPECT_EQ( @@ -168,7 +168,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) const char * filter_expression1_update = "filter=2"; rcl_subscription_content_filter_options_t subscription_content_filter_options = - rcl_subscription_get_default_content_filter_options(); + rcl_get_zero_initialized_subscription_content_filter_options(); { // init with filter_expression1 EXPECT_EQ( @@ -178,8 +178,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) ); content_filter_options = - subscription_content_filter_options.rmw_subscription_content_filter_options; - ASSERT_NE(nullptr, content_filter_options); + &subscription_content_filter_options.rmw_subscription_content_filter_options; EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression); EXPECT_EQ(0u, content_filter_options->expression_parameters.size); EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); @@ -192,8 +191,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) ); content_filter_options = - subscription_content_filter_options.rmw_subscription_content_filter_options; - ASSERT_NE(nullptr, content_filter_options); + &subscription_content_filter_options.rmw_subscription_content_filter_options; EXPECT_STREQ(filter_expression1_update, content_filter_options->filter_expression); EXPECT_EQ(0u, content_filter_options->expression_parameters.size); EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); @@ -212,7 +210,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) 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(); + rcl_get_zero_initialized_subscription_content_filter_options(); { // init with filter_expression2 and expression_parameters2 EXPECT_EQ( @@ -223,7 +221,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) ); content_filter_options = - subscription_content_filter_options2.rmw_subscription_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_EQ( @@ -244,7 +242,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) ); content_filter_options = - subscription_content_filter_options2.rmw_subscription_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_EQ( From b33d745d539b104803ce202c390c9da19bf7d168 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 10:09:49 +0800 Subject: [PATCH 23/34] address comments Signed-off-by: Chen Lihui --- rcl/include/rcl/context.h | 24 ------------------------ rcl/include/rcl/subscription.h | 9 ++++++--- rcl/src/rcl/context.c | 8 -------- rcl/src/rcl/context_impl.h | 3 +++ rcl/src/rcl/subscription.c | 6 ++++-- 5 files changed, 13 insertions(+), 37 deletions(-) diff --git a/rcl/include/rcl/context.h b/rcl/include/rcl/context.h index 15e561309..68500da81 100644 --- a/rcl/include/rcl/context.h +++ b/rcl/include/rcl/context.h @@ -43,9 +43,6 @@ extern "C" /// A unique ID per context instance. typedef uint64_t rcl_context_instance_id_t; -/// Common content filter factory (Defined in a new package) -typedef struct common_content_filter_factory_s common_content_filter_factory_t; - typedef struct rcl_context_impl_s rcl_context_impl_t; /// Encapsulates the non-global state of an init/shutdown cycle. @@ -317,27 +314,6 @@ RCL_WARN_UNUSED rmw_context_t * rcl_context_get_rmw_context(rcl_context_t * context); -/// Return pointer to the common content filter factory -/** - * If context is `NULL`, then `NULL` is returned. - * If context is zero-initialized, then `NULL` is returned. - * If context is uninitialized, then it is undefined behavior. - * - * Attribute | Adherence - * ------------------ | ------------- - * Allocates Memory | No - * Thread-Safe | No - * Uses Atomics | No - * Lock-Free | No - * - * \param[in] context object from which the common content filter factory should be retrieved. - * \return pointer to common content filter factory if valid, otherwise `NULL` - */ -RCL_PUBLIC -RCL_WARN_UNUSED -common_content_filter_factory_t * -rcl_context_get_common_content_filter_factory(rcl_context_t * context); - #ifdef __cplusplus } #endif diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 61437d63f..260319360 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -240,9 +240,12 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option); /// 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. - * \param[in] expression_parameter_argv The expression parameters argv. + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. * \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 diff --git a/rcl/src/rcl/context.c b/rcl/src/rcl/context.c index dcf970875..5414cc5e9 100644 --- a/rcl/src/rcl/context.c +++ b/rcl/src/rcl/context.c @@ -105,14 +105,6 @@ rcl_context_get_rmw_context(rcl_context_t * context) return &(context->impl->rmw_context); } -common_content_filter_factory_t * -rcl_context_get_common_content_filter_factory(rcl_context_t * context) -{ - RCL_CHECK_ARGUMENT_FOR_NULL(context, NULL); - RCL_CHECK_FOR_NULL_WITH_MSG(context->impl, "context is zero-initialized", return NULL); - return context->impl->common_content_filter_factory; -} - rcl_ret_t __cleanup_context(rcl_context_t * context) { diff --git a/rcl/src/rcl/context_impl.h b/rcl/src/rcl/context_impl.h index a327ac9f1..029d21f59 100644 --- a/rcl/src/rcl/context_impl.h +++ b/rcl/src/rcl/context_impl.h @@ -25,6 +25,9 @@ extern "C" { #endif +/// Common content filter factory (Defined in a new package) +typedef struct common_content_filter_factory_s common_content_filter_factory_t; + /// \internal struct rcl_context_impl_s { diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index da14dd2dc..65703865e 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; // options subscription->impl->options = *options; - RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACEPOINT( @@ -240,6 +239,10 @@ rcl_subscription_options_set_content_filter_options( rcl_subscription_options_t * options) { RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return 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); @@ -417,7 +420,6 @@ rcl_take( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - return RCL_RET_OK; } From 0ca51058da545522d13c4fb304be3cae53969a29 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 10:28:46 +0800 Subject: [PATCH 24/34] update comments Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 59 +++++++++++++++++++++++++++------- rcl/src/rcl/subscription.c | 9 +++++- 2 files changed, 56 insertions(+), 12 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 260319360..dc968297e 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -223,10 +223,10 @@ rcl_subscription_get_default_options(void); *
* Attribute | Adherence * ------------------ | ------------- - * Allocates Memory | No + * Allocates Memory | Yes * Thread-Safe | No * Uses Atomics | No - * Lock-Free | Yes + * Lock-Free | No * * \param[in] option The structure which its resources have to be deallocated. * \return `RCL_RET_OK` if the memory was successfully freed, or @@ -240,6 +240,14 @@ rcl_subscription_options_fini(rcl_subscription_options_t * option); /// Set the content filter options for the given subscription options. /** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause. * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder @@ -266,7 +274,7 @@ RCL_WARN_UNUSED rcl_subscription_content_filter_options_t rcl_get_zero_initialized_subscription_content_filter_options(void); -/// Allocate. +/// Initialize the content filter options for the given subscription options. /** *
* Attribute | Adherence @@ -274,12 +282,19 @@ rcl_get_zero_initialized_subscription_content_filter_options(void); * Allocates Memory | Yes * Thread-Safe | No * Uses Atomics | No - * Lock-Free | Yes + * Lock-Free | No * - * \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. + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, + * use empty ("") can reset (or clear) the content filter setting of a subscription. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * \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 + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. */ RCL_PUBLIC RCL_WARN_UNUSED @@ -290,6 +305,28 @@ rcl_subscription_content_filter_options_init( const char * expression_parameter_argv[], rcl_subscription_content_filter_options_t * options); +/// Set the content filter options for the given subscription options. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, + * use empty ("") can reset (or clear) the content filter setting of a subscription. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * \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 + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. + */ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t @@ -304,10 +341,10 @@ rcl_subscription_content_filter_options_set( *
* Attribute | Adherence * ------------------ | ------------- - * Allocates Memory | No + * Allocates Memory | Yes * Thread-Safe | No * Uses Atomics | No - * Lock-Free | Yes + * Lock-Free | No * * \param[in] options The structure which its resources have to be deallocated. * \return `RCL_RET_OK` if the memory was successfully freed, or @@ -337,7 +374,7 @@ rcl_subscription_content_filter_options_fini( * \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_filter_options` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `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. */ diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 65703865e..38eda8627 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -314,6 +314,10 @@ rcl_subscription_content_filter_options_init( 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); + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return RCL_RET_INVALID_ARGUMENT; + } rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init( filter_expression, @@ -333,7 +337,10 @@ rcl_subscription_content_filter_options_set( const char * expression_parameter_argv[], rcl_subscription_content_filter_options_t * options) { - RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return 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); From 3f6421f3c388d88de00e005e447cbe3765d87fe0 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 13:10:32 +0800 Subject: [PATCH 25/34] add DDS content filter implementation without fallback Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 11 + rcl/src/rcl/subscription.c | 58 +++- rcl/test/rcl/test_subscription.cpp | 481 ++++++++++++++++++++++++++++- 3 files changed, 530 insertions(+), 20 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index dc968297e..6344a017c 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -357,6 +357,17 @@ rcl_ret_t 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. +/** + * Depending on the middleware and whether cft is enabled in the subscription. + * + * \return `true` if the content filtered topic of `subscription` is enabled, otherwise `false` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool +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 diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 38eda8627..13fce6ca1 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -371,16 +371,50 @@ rcl_subscription_content_filter_options_fini( return rcl_convert_rmw_ret_to_rcl_ret(ret); } +bool +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_enabled; +} + rcl_ret_t rcl_subscription_set_content_filter( const rcl_subscription_t * subscription, const rcl_subscription_content_filter_options_t * options ) { - (void)subscription; - (void)options; + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); - return RCL_RET_UNSUPPORTED; + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + + 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_content_filter( + subscription->impl->rmw_handle, + &options->rmw_subscription_content_filter_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 + rmw_subscription_content_filter_options_t * content_filter_options = + &options->rmw_subscription_content_filter_options; + return rcl_subscription_options_set_content_filter_options( + content_filter_options->filter_expression, + content_filter_options->expression_parameters.size, + (const char **)content_filter_options->expression_parameters.data, + &subscription->impl->options + ); } rcl_ret_t @@ -389,10 +423,22 @@ rcl_subscription_get_content_filter( rcl_subscription_content_filter_options_t * options ) { - (void)subscription; - (void)options; + 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(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 rmw_ret = rmw_subscription_get_content_filter( + subscription->impl->rmw_handle, + &options->allocator, + &options->rmw_subscription_content_filter_options); - return RCL_RET_UNSUPPORTED; + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } rcl_ret_t diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index c1a084242..4910e319e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -886,6 +886,425 @@ 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) { + 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 = + 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(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_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( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + 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 + 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_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)); + + 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, 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; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // 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 *); + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + if (is_cft_support) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &options) + ); + } + + // 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_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)); + + 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, 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; + ASSERT_EQ( + std::string(test_filtered_other_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // get filter + { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); + if (is_cft_support) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rmw_subscription_content_filter_options_t * options = + &content_filter_options.rmw_subscription_content_filter_options; + ASSERT_STREQ(filter_expression2, options->filter_expression); + 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_filter_options_fini( + &content_filter_options) + ); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + } + + // reset filter + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + "", 0, nullptr, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + 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)); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &options) + ); + } + + // publish with a non-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, 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; + ASSERT_EQ( + std::string(test_string), + std::string(msg.string_value.data, msg.string_value.size)); + } +} + +/* A subscription without a content filtered topic setting at beginning. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { + 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_enabled(&subscription)); + + // failed to get filter + { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); + ASSERT_NE(RCL_RET_OK, ret); + } + + 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; + 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, 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; + ASSERT_EQ( + std::string(test_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // 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 *); + bool is_cft_support{true}; + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + if (RCL_RET_UNSUPPORTED == ret) { + is_cft_support = false; + } else { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &options) + ); + } + + // 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_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)); + + 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, 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; + 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 = @@ -1126,6 +1545,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_enabled(nullptr)); + rcl_reset_error(); EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); rcl_reset_error(); @@ -1137,6 +1558,8 @@ 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_enabled(&subscription_zero_init)); + rcl_reset_error(); } /* Test for all failure modes in rcl_subscription_set_content_filter function. @@ -1146,17 +1569,17 @@ TEST_F( TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_content_filter) { EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_set_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_set_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_INVALID_ARGUMENT, rcl_subscription_set_content_filter(&subscription, nullptr)); rcl_reset_error(); @@ -1180,10 +1603,25 @@ TEST_F( ); }); - EXPECT_EQ( - RMW_RET_UNSUPPORTED, - rcl_subscription_set_content_filter(&subscription, &options)); - rcl_reset_error(); + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_set_content_filter( + &subscription, &options)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); + EXPECT_EQ( + RMW_RET_ERROR, + rcl_subscription_set_content_filter( + &subscription, &options)); + rcl_reset_error(); + } } /* Test for all failure modes in rcl_subscription_get_content_filter function. @@ -1193,27 +1631,42 @@ TEST_F( TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_content_filter) { EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_get_content_filter(nullptr, nullptr)); rcl_reset_error(); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_get_content_filter(&subscription_zero_init, nullptr)); rcl_reset_error(); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RCL_RET_INVALID_ARGUMENT, rcl_subscription_get_content_filter(&subscription, nullptr)); rcl_reset_error(); rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); - EXPECT_EQ( - RMW_RET_UNSUPPORTED, - rcl_subscription_get_content_filter(&subscription, &options)); - rcl_reset_error(); + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_get_content_filter( + &subscription, &options)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); + EXPECT_EQ( + RMW_RET_ERROR, + rcl_subscription_get_content_filter( + &subscription, &options)); + rcl_reset_error(); + } } TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail) From eaa87270537d2caa9c294156ada726b8c1f241e5 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 13:45:38 +0800 Subject: [PATCH 26/34] waiting to allow for filter propagation Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 4910e319e..4d57d863e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -999,6 +999,8 @@ TEST_F( &subscription, &options); if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } @@ -1109,6 +1111,8 @@ TEST_F( &subscription, &options); if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } else { @@ -1240,6 +1244,8 @@ TEST_F( is_cft_support = false; } else { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); } EXPECT_EQ( From cd8e85931a92f3bdb25fb879c6df43e03cfbba75 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 13:58:50 +0800 Subject: [PATCH 27/34] use = instead of match symbol as it is not standard Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 4d57d863e..dd67a1c70 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -892,7 +892,7 @@ TEST_F( CLASSNAME( TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_content_filtered) { - const char * filter_expression1 = "string_value MATCH 'FilteredData'"; + const char * filter_expression1 = "string_value = 'FilteredData'"; rcl_ret_t ret; rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); const rosidl_message_type_support_t * ts = @@ -981,7 +981,7 @@ TEST_F( } // set filter - const char * filter_expression2 = "string_value MATCH %0"; + const char * filter_expression2 = "string_value = %0"; const char * expression_parameters2[] = {"'FilteredOtherData'"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); { @@ -1223,7 +1223,7 @@ TEST_F( } // set filter - const char * filter_expression2 = "string_value MATCH %0"; + const char * filter_expression2 = "string_value = %0"; const char * expression_parameters2[] = {"'FilteredData'"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); bool is_cft_support{true}; From 0fcdc5b9a58a4e2f15dc3784abb8ff81e2f2f099 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 29 Mar 2022 09:42:37 +0800 Subject: [PATCH 28/34] remove unnecessary code and update error message Signed-off-by: Chen Lihui --- rcl/src/rcl/context_impl.h | 5 ----- rcl/src/rcl/subscription.c | 7 ++----- rcl/src/rcl/subscription_impl.h | 4 ---- rcl/test/rcl/test_subscription_content_filter_options.cpp | 2 +- 4 files changed, 3 insertions(+), 15 deletions(-) diff --git a/rcl/src/rcl/context_impl.h b/rcl/src/rcl/context_impl.h index 029d21f59..10c9e82cc 100644 --- a/rcl/src/rcl/context_impl.h +++ b/rcl/src/rcl/context_impl.h @@ -25,9 +25,6 @@ extern "C" { #endif -/// Common content filter factory (Defined in a new package) -typedef struct common_content_filter_factory_s common_content_filter_factory_t; - /// \internal struct rcl_context_impl_s { @@ -41,8 +38,6 @@ struct rcl_context_impl_s char ** argv; /// rmw context. rmw_context_t rmw_context; - /// Common content filter factory - common_content_filter_factory_t * common_content_filter_factory; }; RCL_LOCAL diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 13fce6ca1..bff31456d 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -252,7 +252,7 @@ rcl_subscription_options_set_content_filter_options( allocator->allocate( sizeof(rmw_subscription_content_filter_options_t), allocator->state); if (!content_filter_options) { - RCL_SET_ERROR_MSG("allocating memory failed"); + RCL_SET_ERROR_MSG("failed to allocate memory"); return RCL_RET_BAD_ALLOC; } @@ -393,9 +393,6 @@ rcl_subscription_set_content_filter( return RCL_RET_SUBSCRIPTION_INVALID; } - 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_content_filter( subscription->impl->rmw_handle, @@ -407,7 +404,7 @@ rcl_subscription_set_content_filter( } // copy options into subscription_options - rmw_subscription_content_filter_options_t * content_filter_options = + const rmw_subscription_content_filter_options_t * content_filter_options = &options->rmw_subscription_content_filter_options; return rcl_subscription_options_set_content_filter_options( content_filter_options->filter_expression, diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 2957f2d2f..0fe962ab4 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -19,15 +19,11 @@ #include "rcl/subscription.h" -/// Common content filter. (Defined in other package) -typedef struct common_content_filter_s common_content_filter_t; - struct rcl_subscription_impl_s { rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; - common_content_filter_t * common_content_filter; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp index 3108a5798..020e4360a 100644 --- a/rcl/test/rcl/test_subscription_content_filter_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Open Source Robotics Foundation, Inc. +// Copyright 2022 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. From 206facde4e4621d2af045e3ff176758f81ad6fda Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 30 Mar 2022 10:05:22 +0800 Subject: [PATCH 29/34] update test case name and use BasicTypes Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 67 ++++++++++++++---------------- 1 file changed, 31 insertions(+), 36 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index dd67a1c70..cb3ea51f4 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1157,11 +1157,12 @@ TEST_F( */ TEST_F( CLASSNAME( - TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_not_content_filtered_at_begin) { + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_not_initialized_with_content_filtering) { 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); + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); 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); @@ -1196,35 +1197,33 @@ TEST_F( ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); // publish with a non-filtered data - constexpr char test_string[] = "NotFilteredData"; + int32_t test_value = 3; { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_value; ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__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)); + ASSERT_TRUE(test_value == msg.int32_value); } // set filter - const char * filter_expression2 = "string_value = %0"; - const char * expression_parameters2[] = {"'FilteredData'"}; + const char * filter_expression2 = "int32_value = %0"; + const char * expression_parameters2[] = {"4"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); bool is_cft_support{true}; { @@ -1257,11 +1256,11 @@ TEST_F( // 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)); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_value; ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } @@ -1270,44 +1269,40 @@ TEST_F( } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__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)); + ASSERT_TRUE(test_value == msg.int32_value); } // publish filtered data - constexpr char test_filtered_string[] = "FilteredData"; + int32_t test_filtered_value = 4; { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_filtered_value; ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { - test_msgs__msg__Strings__fini(&msg); + test_msgs__msg__BasicTypes__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)); + ASSERT_TRUE(test_filtered_value == msg.int32_value); } } From 05811f0e5d1cbc72cca2abb212b3f06fb7e57f71 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 10:54:42 +0800 Subject: [PATCH 30/34] use the rcl_subscription_option_t allocator instead Signed-off-by: Chen Lihui --- rcl/include/rcl/subscription.h | 13 +- rcl/src/rcl/subscription.c | 26 +++- rcl/test/CMakeLists.txt | 1 + rcl/test/rcl/test_subscription.cpp | 13 +- ...st_subscription_content_filter_options.cpp | 120 +++++++++++++++--- 5 files changed, 141 insertions(+), 32 deletions(-) diff --git a/rcl/include/rcl/subscription.h b/rcl/include/rcl/subscription.h index 6344a017c..28bbf1713 100644 --- a/rcl/include/rcl/subscription.h +++ b/rcl/include/rcl/subscription.h @@ -55,10 +55,6 @@ typedef struct rcl_subscription_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 filter options rmw_subscription_content_filter_options_t rmw_subscription_content_filter_options; } rcl_subscription_content_filter_options_t; @@ -284,6 +280,7 @@ rcl_get_zero_initialized_subscription_content_filter_options(void); * Uses Atomics | No * Lock-Free | No * + * \param[in] subscription the handle to the subscription. * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, * use empty ("") can reset (or clear) the content filter setting of a subscription. * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. @@ -293,6 +290,7 @@ rcl_get_zero_initialized_subscription_content_filter_options(void); * It can be NULL if there is no "%n" tokens placeholder in filter_expression. * \param[out] options The subscription options to be set. * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. */ @@ -300,6 +298,7 @@ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_subscription_content_filter_options_init( + const rcl_subscription_t * subscription, const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], @@ -315,6 +314,7 @@ rcl_subscription_content_filter_options_init( * Uses Atomics | No * Lock-Free | No * + * \param[in] subscription the handle to the subscription. * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, * use empty ("") can reset (or clear) the content filter setting of a subscription. * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. @@ -324,6 +324,7 @@ rcl_subscription_content_filter_options_init( * It can be NULL if there is no "%n" tokens placeholder in filter_expression. * \param[out] options The subscription options to be set. * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. */ @@ -331,6 +332,7 @@ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_subscription_content_filter_options_set( + const rcl_subscription_t * subscription, const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], @@ -346,8 +348,10 @@ rcl_subscription_content_filter_options_set( * Uses Atomics | No * Lock-Free | No * + * \param[in] subscription the handle to the subscription. * \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_SUBSCRIPTION_INVALID` if subscription is invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or * if its allocator is invalid and the structure contains initialized memory. */ @@ -355,6 +359,7 @@ RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t rcl_subscription_content_filter_options_fini( + const rcl_subscription_t * subscription, rcl_subscription_content_filter_options_t * options); /// Check if the content filtered topic feature is enabled in the subscription. diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index bff31456d..bc110e425 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -278,7 +278,8 @@ rcl_subscription_options_set_content_filter_options( ); if (rmw_ret != RMW_RET_OK) { - return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto failed; } allocator->deallocate( @@ -298,7 +299,6 @@ rcl_subscription_content_filter_options_t rcl_get_zero_initialized_subscription_content_filter_options() { return (const rcl_subscription_content_filter_options_t) { - .allocator = rcl_get_default_allocator(), .rmw_subscription_content_filter_options = rmw_get_zero_initialized_content_filter_options() }; // NOLINT(readability/braces): false positive @@ -306,13 +306,17 @@ rcl_get_zero_initialized_subscription_content_filter_options() rcl_ret_t rcl_subscription_content_filter_options_init( + const rcl_subscription_t * subscription, const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], rcl_subscription_content_filter_options_t * options) { + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - const rcl_allocator_t * allocator = &options->allocator; + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); if (expression_parameters_argc > 100) { RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); @@ -332,17 +336,21 @@ rcl_subscription_content_filter_options_init( rcl_ret_t rcl_subscription_content_filter_options_set( + const rcl_subscription_t * subscription, const char * filter_expression, size_t expression_parameters_argc, const char * expression_parameter_argv[], rcl_subscription_content_filter_options_t * options) { + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } if (expression_parameters_argc > 100) { RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); return RCL_RET_INVALID_ARGUMENT; } RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - const rcl_allocator_t * allocator = &options->allocator; + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rmw_ret_t ret = rmw_subscription_content_filter_options_set( @@ -357,10 +365,14 @@ rcl_subscription_content_filter_options_set( rcl_ret_t rcl_subscription_content_filter_options_fini( + const rcl_subscription_t * subscription, rcl_subscription_content_filter_options_t * options) { + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - const rcl_allocator_t * allocator = &options->allocator; + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rmw_ret_t ret = rmw_subscription_content_filter_options_fini( @@ -427,12 +439,12 @@ rcl_subscription_get_content_filter( return RCL_RET_SUBSCRIPTION_INVALID; } RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); - const rcl_allocator_t * allocator = &options->allocator; + rcl_allocator_t * allocator = &subscription->impl->options.allocator; RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret = rmw_subscription_get_content_filter( subscription->impl->rmw_handle, - &options->allocator, + allocator, &options->rmw_subscription_content_filter_options); return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index ffd7f7b42..61b17b37c 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -448,4 +448,5 @@ 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} + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "test_msgs" ) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index cb3ea51f4..a9786d5f2 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -991,6 +991,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( + &subscription, filter_expression2, expression_parameters2_count, expression_parameters2, &options) ); @@ -1008,7 +1009,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( - &options) + &subscription, &options) ); } @@ -1088,6 +1089,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( + &subscription, &content_filter_options) ); } else { @@ -1103,6 +1105,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( + &subscription, "", 0, nullptr, &options) ); @@ -1122,7 +1125,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( - &options) + &subscription, &options) ); } @@ -1233,6 +1236,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( + &subscription, filter_expression2, expression_parameters2_count, expression_parameters2, &options) ); @@ -1250,7 +1254,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( - &options) + &subscription, &options) ); } @@ -1590,6 +1594,7 @@ TEST_F( EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( + &subscription, "data = '0'", 0, nullptr, @@ -1600,7 +1605,7 @@ TEST_F( { EXPECT_EQ( RCL_RET_OK, - rcl_subscription_content_filter_options_fini(&options) + rcl_subscription_content_filter_options_fini(&subscription, &options) ); }); diff --git a/rcl/test/rcl/test_subscription_content_filter_options.cpp b/rcl/test/rcl/test_subscription_content_filter_options.cpp index 020e4360a..163bf3db7 100644 --- a/rcl/test/rcl/test_subscription_content_filter_options.cpp +++ b/rcl/test/rcl/test_subscription_content_filter_options.cpp @@ -15,9 +15,15 @@ #include #include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcl/rcl.h" #include "rcl/subscription.h" -TEST(TestSubscriptionContentFilterOptions, subscription_options_failure) { +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "test_msgs/msg/basic_types.h" + + +TEST(TestSubscriptionOptionsContentFilter, subscription_options_failure) { rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); const char * filter_expression1 = "filter=1"; @@ -50,7 +56,7 @@ TEST(TestSubscriptionContentFilterOptions, subscription_options_failure) { rcl_reset_error(); } -TEST(TestSubscriptionContentFilterOptions, subscription_options_success) +TEST(TestSubscriptionOptionsContentFilter, subscription_options_success) { rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); @@ -105,63 +111,141 @@ TEST(TestSubscriptionContentFilterOptions, subscription_options_success) ); } -TEST(TestSubscriptionContentFilterOptions, content_filter_options_failure) { + +class TestSubscriptionContentFilterOptions : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_subscription_t * subscription_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_subscription_content_filter_options_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "chatter"; + + this->subscription_ptr = new rcl_subscription_t; + *this->subscription_ptr = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + this->subscription_ptr, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_F(TestSubscriptionContentFilterOptions, content_filter_options_failure) { rcl_subscription_content_filter_options_t content_filter_options = rcl_get_zero_initialized_subscription_content_filter_options(); const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_init( + nullptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_subscription_content_filter_options_init( - nullptr, 0, nullptr, nullptr) + this->subscription_ptr, nullptr, 0, nullptr, nullptr) ); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_subscription_content_filter_options_init( - filter_expression1, 0, nullptr, nullptr) + this->subscription_ptr, 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) + this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options) ); rcl_reset_error(); // set + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_set( + nullptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_subscription_content_filter_options_set( - nullptr, 0, nullptr, nullptr) + this->subscription_ptr, nullptr, 0, nullptr, nullptr) ); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_subscription_content_filter_options_set( - filter_expression1, 0, nullptr, nullptr) + this->subscription_ptr, 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) + this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_fini( + nullptr, nullptr) ); rcl_reset_error(); EXPECT_EQ( RCL_RET_INVALID_ARGUMENT, rcl_subscription_content_filter_options_fini( - nullptr) + this->subscription_ptr, nullptr) ); rcl_reset_error(); } -TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) +TEST_F(TestSubscriptionContentFilterOptions, content_filter_options_success) { rmw_subscription_content_filter_options_t * content_filter_options; const char * filter_expression1 = "filter=1"; @@ -174,7 +258,8 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( - filter_expression1, 0, nullptr, &subscription_content_filter_options) + this->subscription_ptr, filter_expression1, 0, nullptr, + &subscription_content_filter_options) ); content_filter_options = @@ -187,7 +272,8 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_set( - filter_expression1_update, 0, nullptr, &subscription_content_filter_options) + this->subscription_ptr, filter_expression1_update, 0, nullptr, + &subscription_content_filter_options) ); content_filter_options = @@ -216,7 +302,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_init( - filter_expression2, expression_parameters_count2, + this->subscription_ptr, filter_expression2, expression_parameters_count2, expression_parameters2, &subscription_content_filter_options2) ); @@ -237,7 +323,7 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_set( - filter_expression2_update, expression_parameters_count2_update, + this->subscription_ptr, filter_expression2_update, expression_parameters_count2_update, expression_parameters2_update, &subscription_content_filter_options2) ); @@ -258,11 +344,11 @@ TEST(TestSubscriptionContentFilterOptions, content_filter_options_success) EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( - &subscription_content_filter_options) + this->subscription_ptr, &subscription_content_filter_options) ); EXPECT_EQ( RCL_RET_OK, rcl_subscription_content_filter_options_fini( - &subscription_content_filter_options2) + this->subscription_ptr, &subscription_content_filter_options2) ); } From d421cf267061020cd3e3bd8a519f75538f62a415 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 11:46:03 +0800 Subject: [PATCH 31/34] set the option data directly seems a bit more complicated Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 91 ++++++++++++++++++++++++++++---------- 1 file changed, 67 insertions(+), 24 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index bc110e425..fd5984ded 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -248,22 +248,40 @@ rcl_subscription_options_set_content_filter_options( RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); rcl_ret_t ret; - rmw_subscription_content_filter_options_t * content_filter_options = - allocator->allocate( - sizeof(rmw_subscription_content_filter_options_t), allocator->state); - if (!content_filter_options) { - RCL_SET_ERROR_MSG("failed to allocate memory"); - return RCL_RET_BAD_ALLOC; + rmw_ret_t rmw_ret; + rmw_subscription_content_filter_options_t * original_content_filter_options = + options->rmw_subscription_options.content_filter_options; + rmw_subscription_content_filter_options_t content_filter_options_backup = + rmw_get_zero_initialized_content_filter_options(); + + if (original_content_filter_options) { + // make a backup, restore the data if failure happened + rmw_ret = rmw_subscription_content_filter_options_copy( + original_content_filter_options, + allocator, + &content_filter_options_backup + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + } else { + options->rmw_subscription_options.content_filter_options = + allocator->allocate( + sizeof(rmw_subscription_content_filter_options_t), allocator->state); + if (!options->rmw_subscription_options.content_filter_options) { + RCL_SET_ERROR_MSG("failed to allocate memory"); + return RCL_RET_BAD_ALLOC; + } + *options->rmw_subscription_options.content_filter_options = + rmw_get_zero_initialized_content_filter_options(); } - *content_filter_options = rmw_get_zero_initialized_content_filter_options(); - - rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_set( + rmw_ret = rmw_subscription_content_filter_options_set( filter_expression, expression_parameters_argc, expression_parameter_argv, allocator, - content_filter_options + options->rmw_subscription_options.content_filter_options ); if (rmw_ret != RMW_RET_OK) { @@ -271,27 +289,52 @@ rcl_subscription_options_set_content_filter_options( goto failed; } - if (options->rmw_subscription_options.content_filter_options) { + rmw_ret = rmw_subscription_content_filter_options_fini( + &content_filter_options_backup, + allocator + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + return RMW_RET_OK; + +failed: + + if (original_content_filter_options == NULL) { + if (options->rmw_subscription_options.content_filter_options) { + rmw_ret = rmw_subscription_content_filter_options_fini( + options->rmw_subscription_options.content_filter_options, + allocator + ); + + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + allocator->deallocate( + options->rmw_subscription_options.content_filter_options, allocator->state); + options->rmw_subscription_options.content_filter_options = NULL; + } + } else { + rmw_ret = rmw_subscription_content_filter_options_copy( + &content_filter_options_backup, + allocator, + options->rmw_subscription_options.content_filter_options + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rmw_ret = rmw_subscription_content_filter_options_fini( - options->rmw_subscription_options.content_filter_options, + &content_filter_options_backup, allocator ); - if (rmw_ret != RMW_RET_OK) { - ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); - goto failed; + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } - - allocator->deallocate( - options->rmw_subscription_options.content_filter_options, allocator->state); - options->rmw_subscription_options.content_filter_options = NULL; } - options->rmw_subscription_options.content_filter_options = content_filter_options; - return RCL_RET_OK; - -failed: - allocator->deallocate(content_filter_options, allocator->state); return ret; } From 630113369c87433e0bfe5d0f18b8ec73c267f7c7 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 21:50:39 +0800 Subject: [PATCH 32/34] explicitly check the cft supported by rmw_connextdds and rmw_fastrtps_cpp Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index a9786d5f2..546585beb 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1228,7 +1228,9 @@ TEST_F( const char * filter_expression2 = "int32_value = %0"; const char * expression_parameters2[] = {"4"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); - bool is_cft_support{true}; + bool is_cft_support = + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 || + std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps_cpp") == 0); { rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); @@ -1243,8 +1245,8 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (RCL_RET_UNSUPPORTED == ret) { - is_cft_support = false; + if (!is_cft_support) { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } else { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation From 4934583e9f12783d3b553115a514cf1fe56a4b69 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 1 Apr 2022 10:33:15 +0800 Subject: [PATCH 33/34] increase the maximun time for events and content filter propagation Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 34 +++++++++++++++--------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 546585beb..46bd15213 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -923,7 +923,7 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); bool is_cft_support = rcl_subscription_is_cft_enabled(&subscription); - ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); // publish with a non-filtered data constexpr char test_string[] = "NotFilteredData"; @@ -937,9 +937,9 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -964,7 +964,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, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); { test_msgs__msg__Strings msg; @@ -1001,7 +1001,7 @@ TEST_F( if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(10)); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); } @@ -1024,9 +1024,9 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -1051,7 +1051,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, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); { test_msgs__msg__Strings msg; @@ -1115,8 +1115,8 @@ TEST_F( if (is_cft_support) { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); - ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); + std::this_thread::sleep_for(std::chrono::seconds(10)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); } else { ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); @@ -1139,7 +1139,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, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); { test_msgs__msg__Strings msg; @@ -1197,7 +1197,7 @@ TEST_F( ASSERT_NE(RCL_RET_OK, ret); } - ASSERT_TRUE(wait_for_established_subscription(&publisher, 30, 100)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); // publish with a non-filtered data int32_t test_value = 3; @@ -1210,7 +1210,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, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); { test_msgs__msg__BasicTypes msg; @@ -1250,7 +1250,7 @@ TEST_F( } else { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(10)); } EXPECT_EQ( @@ -1271,9 +1271,9 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); test_msgs__msg__BasicTypes msg; test_msgs__msg__BasicTypes__init(&msg); @@ -1297,7 +1297,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, 30, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); { test_msgs__msg__BasicTypes msg; From 954982ee240cdc305302bd3163bcab3ebcf8933a Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 1 Apr 2022 18:55:04 +0800 Subject: [PATCH 34/34] set test timeout to 120 Signed-off-by: Chen Lihui --- rcl/test/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 61b17b37c..a9969f579 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -245,6 +245,7 @@ function(test_target_function) APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + TIMEOUT 120 ) # TODO(asorbini) Enable message timestamp tests for rmw_connextdds on Windows # once clock incompatibilities are resolved.