From 424a9811d8863716f2be9878dae5bcf778671cad Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 27 Nov 2020 16:52:53 +0800 Subject: [PATCH 01/24] to support a feature of content filtered topic Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 35 +++++++ .../include/rclcpp/subscription_options.hpp | 70 +++++++++++++ rclcpp/src/rclcpp/subscription_base.cpp | 98 +++++++++++++++++++ 3 files changed, 203 insertions(+) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 26650cd121..1aae997cec 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -491,6 +491,41 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_[event_type]->clear_on_ready_callback(); } + /// Check if subscription instance can support content filter topic feature. + /** + * Depending on the middleware and the message type, this will return true if the middleware + * can support content filter topic feature. + * + * \return boolean flag indicating if middleware can support content filter topic feature. + */ + RCLCPP_PUBLIC + bool + is_cft_supported() const; + + /// Set the filter expression and expression parameters for the subscription. + /** + * \param[in] filter_expression An filter expression to set. + * \param[in] expression_parameters Array of expression parameters to set. + */ + RCLCPP_PUBLIC + virtual + void + set_cft_expression_parameters( + const std::string & filter_expression, + const std::vector & expression_parameters); + + /// Get the filter expression and expression parameters for the subscription. + /** + * \param[out] filter_expression An filter expression to get. + * \param[out] expression_parameters Array of expression parameters to get. + */ + RCLCPP_PUBLIC + virtual + void + get_cft_expression_parameters( + std::string & filter_expression, + std::vector & expression_parameters) const; + protected: template void diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index d6d2d4c60d..7a78b55135 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -21,6 +21,9 @@ #include #include +#include "rcutils/strdup.h" +#include "rcutils/types/string_array.h" + #include "rclcpp/callback_group.hpp" #include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp" #include "rclcpp/intra_process_buffer_type.hpp" @@ -81,6 +84,17 @@ struct SubscriptionOptionsBase TopicStatisticsOptions topic_stats_options; QosOverridingOptions qos_overriding_options; + + // Options to configure content filtered topic in the subscription. + struct ContentFilterOptions + { + // Filter expression like SQL statement + std::string filter_expression; + // Expression parameters for filter expression + std::vector expression_parameters; + }; + + ContentFilterOptions content_filter_options; }; /// Structure containing optional configuration for Subscriptions. @@ -123,6 +137,62 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } + auto fail_clean = [&result]() + { + rcl_ret_t ret = rcl_subscription_options_fini(&result); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }; + + // utils, string -> char * + // utils, vector -> rcutils_string_array_t * + + // Copy content_filter_options into rcl_subscription_options if necessary. + if (!content_filter_options.filter_expression.empty()) { + char * expression = + rcutils_strdup(content_filter_options.filter_expression.c_str(), allocator); + if (!expression) { + throw std::runtime_error("failed to allocate memory for filter expression"); + } + result.rmw_subscription_options.filter_expression = expression; + } + + if (!content_filter_options.expression_parameters.empty()) { + rcutils_string_array_t * parameters = + static_cast(allocator.allocate( + sizeof(rcutils_string_array_t), + allocator.state)); + if (!parameters) { + fail_clean(); + throw std::runtime_error("failed to allocate memory for expression parameters"); + } + rcutils_ret_t ret = rcutils_string_array_init( + parameters, content_filter_options.expression_parameters.size(), &allocator); + if (RCUTILS_RET_OK != ret) { + fail_clean(); + rclcpp::exceptions::throw_from_rcl_error( + RCL_RET_ERROR, + "failed to initialize string array for expression parameters"); + } + + for (size_t i = 0; i < content_filter_options.expression_parameters.size(); ++i) { + char * parameter = + rcutils_strdup(content_filter_options.expression_parameters[i].c_str(), allocator); + if (!parameter) { + fail_clean(); + throw std::runtime_error("failed to allocate memory for expression parameters"); + } + parameters->data[i] = parameter; + } + + result.rmw_subscription_options.expression_parameters = parameters; + } + return result; } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 12841fe6b6..48af419f67 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -20,12 +20,16 @@ #include #include +#include "rcutils/strdup.h" +#include "rcutils/types/string_array.h" + #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/qos_event.hpp" +#include "rclcpp/scope_exit.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -46,6 +50,20 @@ SubscriptionBase::SubscriptionBase( type_support_(type_support_handle), is_serialized_(is_serialized) { + // finalize subscription_options + RCLCPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_options_fini( + const_cast(&subscription_options)); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "Failed to fini subscription option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { @@ -354,3 +372,83 @@ SubscriptionBase::set_on_new_message_callback( throw_from_rcl_error(ret, "failed to set the on new message callback for subscription"); } } + +bool +SubscriptionBase::is_cft_supported() const +{ + return rcl_subscription_is_cft_supported(subscription_handle_.get()); +} + +void +SubscriptionBase::set_cft_expression_parameters( + const std::string & filter_expression, + const std::vector & expression_parameters) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_array_t parameters; + parameters = rcutils_get_zero_initialized_string_array(); + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + ¶meters, expression_parameters.size(), &allocator); + if (RCUTILS_RET_OK != rcutils_ret) { + rclcpp::exceptions::throw_from_rcl_error( + RCL_RET_ERROR, + "failed to initialize string array for expression parameters"); + } + RCLCPP_SCOPE_EXIT( + { + rcutils_ret_t rcutils_ret = rcutils_string_array_fini(¶meters); + if (RCUTILS_RET_OK != rcutils_ret) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "failed to finalize parameters: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + for (size_t i = 0; i < expression_parameters.size(); ++i) { + parameters.data[i] = rcutils_strdup(expression_parameters[i].c_str(), allocator); + } + rcl_ret_t ret = rcl_subscription_set_cft_expression_parameters( + subscription_handle_.get(), + filter_expression.c_str(), + ¶meters); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); + } +} + +void +SubscriptionBase::get_cft_expression_parameters( + std::string & filter_expression, + std::vector & expression_parameters) const +{ + char * expression = NULL; + rcutils_string_array_t parameters; + parameters = rcutils_get_zero_initialized_string_array(); + rcl_ret_t ret = rcl_subscription_get_cft_expression_parameters( + subscription_handle_.get(), + &expression, + ¶meters); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); + } + + filter_expression = expression; + for (size_t i = 0; i < parameters.size; ++i) { + expression_parameters.push_back(parameters.data[i]); + } + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + allocator.deallocate(expression, allocator.state); + rcutils_ret_t rcutils_ret = rcutils_string_array_fini(¶meters); + if (RCUTILS_RET_OK != rcutils_ret) { + RCLCPP_ERROR( + rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), + "failed to finalize parameters: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } +} From 0444bd2afbffeb37847171b25b7ffc10e44dc93a Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 3 Dec 2020 16:59:12 +0800 Subject: [PATCH 02/24] update for checking memory allocated Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/subscription_base.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 48af419f67..6fee0e9cfa 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -408,6 +408,9 @@ SubscriptionBase::set_cft_expression_parameters( for (size_t i = 0; i < expression_parameters.size(); ++i) { parameters.data[i] = rcutils_strdup(expression_parameters[i].c_str(), allocator); + if (!parameters.data[i]) { + throw std::runtime_error("failed to allocate memory for expression parameters"); + } } rcl_ret_t ret = rcl_subscription_set_cft_expression_parameters( subscription_handle_.get(), From 1a57ae56b7e5429321ea96e19c68faed40d76a9d Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 22 Feb 2021 15:47:30 +0800 Subject: [PATCH 03/24] set expression parameter only if filter is valid Signed-off-by: Chen Lihui --- .../include/rclcpp/subscription_options.hpp | 52 +++++++++---------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 7a78b55135..45f5bcce6c 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -160,37 +160,37 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase throw std::runtime_error("failed to allocate memory for filter expression"); } result.rmw_subscription_options.filter_expression = expression; - } - if (!content_filter_options.expression_parameters.empty()) { - rcutils_string_array_t * parameters = - static_cast(allocator.allocate( - sizeof(rcutils_string_array_t), - allocator.state)); - if (!parameters) { - fail_clean(); - throw std::runtime_error("failed to allocate memory for expression parameters"); - } - rcutils_ret_t ret = rcutils_string_array_init( - parameters, content_filter_options.expression_parameters.size(), &allocator); - if (RCUTILS_RET_OK != ret) { - fail_clean(); - rclcpp::exceptions::throw_from_rcl_error( - RCL_RET_ERROR, - "failed to initialize string array for expression parameters"); - } - - for (size_t i = 0; i < content_filter_options.expression_parameters.size(); ++i) { - char * parameter = - rcutils_strdup(content_filter_options.expression_parameters[i].c_str(), allocator); - if (!parameter) { + if (!content_filter_options.expression_parameters.empty()) { + rcutils_string_array_t * parameters = + static_cast(allocator.allocate( + sizeof(rcutils_string_array_t), + allocator.state)); + if (!parameters) { fail_clean(); throw std::runtime_error("failed to allocate memory for expression parameters"); } - parameters->data[i] = parameter; - } + rcutils_ret_t ret = rcutils_string_array_init( + parameters, content_filter_options.expression_parameters.size(), &allocator); + if (RCUTILS_RET_OK != ret) { + fail_clean(); + rclcpp::exceptions::throw_from_rcl_error( + RCL_RET_ERROR, + "failed to initialize string array for expression parameters"); + } - result.rmw_subscription_options.expression_parameters = parameters; + for (size_t i = 0; i < content_filter_options.expression_parameters.size(); ++i) { + char * parameter = + rcutils_strdup(content_filter_options.expression_parameters[i].c_str(), allocator); + if (!parameter) { + fail_clean(); + throw std::runtime_error("failed to allocate memory for expression parameters"); + } + parameters->data[i] = parameter; + } + + result.rmw_subscription_options.expression_parameters = parameters; + } } return result; From 3c949956e329514659c8b18163404f5f31250983 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 23 Feb 2021 16:52:08 +0800 Subject: [PATCH 04/24] add test Signed-off-by: Chen Lihui --- .../include/rclcpp/subscription_options.hpp | 6 +- rclcpp/package.xml | 1 + rclcpp/src/rclcpp/subscription_base.cpp | 14 ---- rclcpp/test/rclcpp/CMakeLists.txt | 1 + rclcpp/test/rclcpp/test_subscription.cpp | 75 +++++++++++++++++++ 5 files changed, 80 insertions(+), 17 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 45f5bcce6c..94b1969ffc 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -163,13 +163,15 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase if (!content_filter_options.expression_parameters.empty()) { rcutils_string_array_t * parameters = - static_cast(allocator.allocate( + static_cast(allocator.zero_allocate( + 1, sizeof(rcutils_string_array_t), allocator.state)); if (!parameters) { fail_clean(); throw std::runtime_error("failed to allocate memory for expression parameters"); } + result.rmw_subscription_options.expression_parameters = parameters; rcutils_ret_t ret = rcutils_string_array_init( parameters, content_filter_options.expression_parameters.size(), &allocator); if (RCUTILS_RET_OK != ret) { @@ -188,8 +190,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase } parameters->data[i] = parameter; } - - result.rmw_subscription_options.expression_parameters = parameters; } } diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 43f6ae2527..0891afb33a 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -47,6 +47,7 @@ rmw rmw_implementation_cmake rosidl_default_generators + std_msgs test_msgs diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 6fee0e9cfa..db31638c7a 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -50,20 +50,6 @@ SubscriptionBase::SubscriptionBase( type_support_(type_support_handle), is_serialized_(is_serialized) { - // finalize subscription_options - RCLCPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_subscription_options_fini( - const_cast(&subscription_options)); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), - "Failed to fini subscription option: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - }); - auto custom_deletor = [node_handle = this->node_handle_](rcl_subscription_t * rcl_subs) { if (rcl_subscription_fini(rcl_subs, node_handle.get()) != RCL_RET_OK) { diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 331c39fb27..e9f822c75f 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -501,6 +501,7 @@ if(TARGET test_subscription) "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" + "std_msgs" "test_msgs" ) target_link_libraries(test_subscription ${PROJECT_NAME} mimick) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 6facada4dd..566db4f275 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -28,6 +28,7 @@ #include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/msg/empty.hpp" +#include "std_msgs/msg/string.hpp" using namespace std::chrono_literals; @@ -144,6 +145,44 @@ class SubscriptionClass } }; +class TestCftSubscription : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_cft_subscription", "/ns"); + rclcpp::SubscriptionOptionsBase options_base; + options_base.content_filter_options.filter_expression = + "data MATCH 'Hello World: 5' or data MATCH %0"; + options_base.content_filter_options.expression_parameters = {"'Hello World: 10'"}; + rclcpp::SubscriptionOptionsWithAllocator> subscription_options( + options_base); + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( + "topic", 10, callback, subscription_options); + } + + void TearDown() + { + sub.reset(); + node.reset(); + } + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::Subscription::SharedPtr sub; +}; + /* Testing subscription construction and destruction. */ @@ -440,6 +479,42 @@ TEST_F(TestSubscription, handle_loaned_message) { EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); } +TEST_F(TestCftSubscription, is_cft_supported) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_supported, false); + EXPECT_FALSE(sub->is_cft_supported()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_supported, true); + EXPECT_TRUE(sub->is_cft_supported()); + } +} + +TEST_F(TestCftSubscription, get_cft_expression_parameters_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_cft_expression_parameters, RCL_RET_ERROR); + + std::string filter_expression; + std::vector expression_parameters; + EXPECT_THROW( + sub->get_cft_expression_parameters(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestCftSubscription, set_cft_expression_parameters_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_cft_expression_parameters, RCL_RET_ERROR); + + std::string filter_expression = "data MATCH %0"; + std::string expression_parameter = "'Hello World: 8'"; + EXPECT_THROW( + sub->set_cft_expression_parameters(filter_expression, {expression_parameter}), + rclcpp::exceptions::RCLError); +} + /* Testing on_new_message callbacks. */ From f2460fa378a8cb6f69cc814e286f442fac6857bf Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 12 Mar 2021 13:18:05 +0800 Subject: [PATCH 05/24] use a default empty value for expresion parameters Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 1aae997cec..fcfc2ae093 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -512,7 +512,7 @@ class SubscriptionBase : public std::enable_shared_from_this void set_cft_expression_parameters( const std::string & filter_expression, - const std::vector & expression_parameters); + const std::vector & expression_parameters = {}); /// Get the filter expression and expression parameters for the subscription. /** From 323b9194872f512f2223a7680958c5f19f76d223 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 12 Mar 2021 13:39:13 +0800 Subject: [PATCH 06/24] remove std_msgs dependency Signed-off-by: Chen Lihui --- rclcpp/package.xml | 1 - rclcpp/test/rclcpp/CMakeLists.txt | 1 - rclcpp/test/rclcpp/test_subscription.cpp | 16 ++++++++-------- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/rclcpp/package.xml b/rclcpp/package.xml index 0891afb33a..43f6ae2527 100644 --- a/rclcpp/package.xml +++ b/rclcpp/package.xml @@ -47,7 +47,6 @@ rmw rmw_implementation_cmake rosidl_default_generators - std_msgs test_msgs diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index e9f822c75f..331c39fb27 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -501,7 +501,6 @@ if(TARGET test_subscription) "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" - "std_msgs" "test_msgs" ) target_link_libraries(test_subscription ${PROJECT_NAME} mimick) diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index 566db4f275..c742b8b011 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -27,8 +27,8 @@ #include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" +#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/msg/empty.hpp" -#include "std_msgs/msg/string.hpp" using namespace std::chrono_literals; @@ -163,12 +163,12 @@ class TestCftSubscription : public ::testing::Test node = std::make_shared("test_cft_subscription", "/ns"); rclcpp::SubscriptionOptionsBase options_base; options_base.content_filter_options.filter_expression = - "data MATCH 'Hello World: 5' or data MATCH %0"; - options_base.content_filter_options.expression_parameters = {"'Hello World: 10'"}; + "int32_value = %0"; + options_base.content_filter_options.expression_parameters = {"10"}; rclcpp::SubscriptionOptionsWithAllocator> subscription_options( options_base); - auto callback = [](std::shared_ptr) {}; - sub = node->create_subscription( + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( "topic", 10, callback, subscription_options); } @@ -180,7 +180,7 @@ class TestCftSubscription : public ::testing::Test protected: rclcpp::Node::SharedPtr node; - rclcpp::Subscription::SharedPtr sub; + rclcpp::Subscription::SharedPtr sub; }; /* @@ -508,8 +508,8 @@ TEST_F(TestCftSubscription, set_cft_expression_parameters_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_set_cft_expression_parameters, RCL_RET_ERROR); - std::string filter_expression = "data MATCH %0"; - std::string expression_parameter = "'Hello World: 8'"; + std::string filter_expression = "int32_value = %0"; + std::string expression_parameter = "100"; EXPECT_THROW( sub->set_cft_expression_parameters(filter_expression, {expression_parameter}), rclcpp::exceptions::RCLError); From 95a442887088e66d4795af35294d1eda61391252 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 13 Oct 2021 13:50:34 +0800 Subject: [PATCH 07/24] use new rcl interface Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 13 ++- .../include/rclcpp/subscription_options.hpp | 67 +++-------- rclcpp/include/rclcpp/utilities.hpp | 9 ++ rclcpp/src/rclcpp/subscription_base.cpp | 92 ++++++++-------- rclcpp/src/rclcpp/utilities.cpp | 13 +++ rclcpp/test/rclcpp/CMakeLists.txt | 12 ++ rclcpp/test/rclcpp/test_subscription.cpp | 75 ------------- ...st_subscription_content_filtered_topic.cpp | 104 ++++++++++++++++++ 8 files changed, 207 insertions(+), 178 deletions(-) create mode 100644 rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index fcfc2ae093..ab190d8c43 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -491,21 +491,20 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_[event_type]->clear_on_ready_callback(); } - /// Check if subscription instance can support content filter topic feature. + /// Check if content filtered topic feature of the subscription instance is enabled. /** - * Depending on the middleware and the message type, this will return true if the middleware - * can support content filter topic feature. - * - * \return boolean flag indicating if middleware can support content filter topic feature. + * \return boolean flag indicating if middleware can support content filtered topic feature. */ RCLCPP_PUBLIC bool - is_cft_supported() const; + is_cft_enabled() const; /// Set the filter expression and expression parameters for the subscription. /** * \param[in] filter_expression An filter expression to set. * \param[in] expression_parameters Array of expression parameters to set. + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs */ RCLCPP_PUBLIC virtual @@ -518,6 +517,8 @@ class SubscriptionBase : public std::enable_shared_from_this /** * \param[out] filter_expression An filter expression to get. * \param[out] expression_parameters Array of expression parameters to get. + * \throws RCLBadAlloc if memory cannot be allocated + * \throws RCLError if an unexpect error occurs */ RCLCPP_PUBLIC virtual diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 94b1969ffc..77ad131e49 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -88,9 +88,9 @@ struct SubscriptionOptionsBase // Options to configure content filtered topic in the subscription. struct ContentFilterOptions { - // Filter expression like SQL statement + // Filter expression like the 'where' part of SQL statement std::string filter_expression; - // Expression parameters for filter expression + // Expression parameters if there is placeholder '%n' in the filter expression std::vector expression_parameters; }; @@ -137,59 +137,18 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } - auto fail_clean = [&result]() - { - rcl_ret_t ret = rcl_subscription_options_fini(&result); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Failed to fini subscription option: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - }; - - // utils, string -> char * - // utils, vector -> rcutils_string_array_t * - - // Copy content_filter_options into rcl_subscription_options if necessary. + // Copy content_filter_options into rcl_subscription_options. if (!content_filter_options.filter_expression.empty()) { - char * expression = - rcutils_strdup(content_filter_options.filter_expression.c_str(), allocator); - if (!expression) { - throw std::runtime_error("failed to allocate memory for filter expression"); - } - result.rmw_subscription_options.filter_expression = expression; - - if (!content_filter_options.expression_parameters.empty()) { - rcutils_string_array_t * parameters = - static_cast(allocator.zero_allocate( - 1, - sizeof(rcutils_string_array_t), - allocator.state)); - if (!parameters) { - fail_clean(); - throw std::runtime_error("failed to allocate memory for expression parameters"); - } - result.rmw_subscription_options.expression_parameters = parameters; - rcutils_ret_t ret = rcutils_string_array_init( - parameters, content_filter_options.expression_parameters.size(), &allocator); - if (RCUTILS_RET_OK != ret) { - fail_clean(); - rclcpp::exceptions::throw_from_rcl_error( - RCL_RET_ERROR, - "failed to initialize string array for expression parameters"); - } - - for (size_t i = 0; i < content_filter_options.expression_parameters.size(); ++i) { - char * parameter = - rcutils_strdup(content_filter_options.expression_parameters[i].c_str(), allocator); - if (!parameter) { - fail_clean(); - throw std::runtime_error("failed to allocate memory for expression parameters"); - } - parameters->data[i] = parameter; - } + std::vector cstrings = + get_c_vector_string(content_filter_options.expression_parameters); + rcl_ret_t ret = rcl_subscription_options_set_content_filtered_topic_options( + get_c_string(content_filter_options.filter_expression), + cstrings.size(), + cstrings.data(), + &result); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set content_filtered_topic_options"); } } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index fa48a010c4..20ded67ad9 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -321,6 +321,15 @@ RCLCPP_PUBLIC const char * get_c_string(const std::string & string_in); +/// Return the std::vector of C string from the given std::vector. +/** + * \param[in] string_in is a std::string + * \return the std::vector of C string from the std::vector + */ +RCLCPP_PUBLIC +std::vector +get_c_vector_string(const std::vector & strings); + } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index db31638c7a..b00eda5915 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -20,6 +20,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rcutils/strdup.h" #include "rcutils/types/string_array.h" @@ -360,9 +362,9 @@ SubscriptionBase::set_on_new_message_callback( } bool -SubscriptionBase::is_cft_supported() const +SubscriptionBase::is_cft_enabled() const { - return rcl_subscription_is_cft_supported(subscription_handle_.get()); + return rcl_subscription_is_cft_enabled(subscription_handle_.get()); } void @@ -370,38 +372,34 @@ SubscriptionBase::set_cft_expression_parameters( const std::string & filter_expression, const std::vector & expression_parameters) { - rcl_allocator_t allocator = rcl_get_default_allocator(); - rcutils_string_array_t parameters; - parameters = rcutils_get_zero_initialized_string_array(); - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - ¶meters, expression_parameters.size(), &allocator); - if (RCUTILS_RET_OK != rcutils_ret) { + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + + std::vector cstrings = + get_c_vector_string(expression_parameters); + rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_init( + get_c_string(filter_expression), + cstrings.size(), + cstrings.data(), + &options); + if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( - RCL_RET_ERROR, - "failed to initialize string array for expression parameters"); + ret, "failed to init subscription content_filtered_topic option"); } - RCLCPP_SCOPE_EXIT( - { - rcutils_ret_t rcutils_ret = rcutils_string_array_fini(¶meters); - if (RCUTILS_RET_OK != rcutils_ret) { + RCPPUTILS_SCOPE_EXIT( + rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); + if (RCL_RET_OK != ret) { RCLCPP_ERROR( - rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), - "failed to finalize parameters: %s", + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", rcl_get_error_string().str); rcl_reset_error(); } - }); + ); - for (size_t i = 0; i < expression_parameters.size(); ++i) { - parameters.data[i] = rcutils_strdup(expression_parameters[i].c_str(), allocator); - if (!parameters.data[i]) { - throw std::runtime_error("failed to allocate memory for expression parameters"); - } - } - rcl_ret_t ret = rcl_subscription_set_cft_expression_parameters( + ret = rcl_subscription_set_cft_expression_parameters( subscription_handle_.get(), - filter_expression.c_str(), - ¶meters); + &options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); @@ -413,31 +411,39 @@ SubscriptionBase::get_cft_expression_parameters( std::string & filter_expression, std::vector & expression_parameters) const { - char * expression = NULL; - rcutils_string_array_t parameters; - parameters = rcutils_get_zero_initialized_string_array(); + rcl_subscription_content_filtered_topic_options_t options = + rcl_subscription_get_default_content_filtered_topic_options(); + + // use rcl_content_filtered_topic_options_t rcl_ret_t ret = rcl_subscription_get_cft_expression_parameters( subscription_handle_.get(), - &expression, - ¶meters); + &options); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); } - filter_expression = expression; - for (size_t i = 0; i < parameters.size; ++i) { - expression_parameters.push_back(parameters.data[i]); + RCLCPP_SCOPE_EXIT( + rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + ); + + rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = + options.rmw_subscription_content_filtered_topic_options; + if (content_filtered_topic_options->filter_expression) { + filter_expression = content_filtered_topic_options->filter_expression; } - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - allocator.deallocate(expression, allocator.state); - rcutils_ret_t rcutils_ret = rcutils_string_array_fini(¶meters); - if (RCUTILS_RET_OK != rcutils_ret) { - RCLCPP_ERROR( - rclcpp::get_node_logger(node_handle_.get()).get_child("rclcpp"), - "failed to finalize parameters: %s", - rcl_get_error_string().str); - rcl_reset_error(); + if (content_filtered_topic_options->expression_parameters) { + for (size_t i = 0; i < content_filtered_topic_options->expression_parameters->size; ++i) { + expression_parameters.push_back( + content_filtered_topic_options->expression_parameters->data[i]); + } } } diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index a6ead85c03..1dac78807d 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -214,4 +214,17 @@ get_c_string(const std::string & string_in) return string_in.c_str(); } +std::vector +get_c_vector_string(const std::vector & strings) +{ + std::vector cstrings; + cstrings.reserve(strings.size()); + + for(size_t i = 0; i < strings.size(); ++i) { + cstrings.push_back(strings[i].c_str()); + } + + return cstrings; +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 331c39fb27..c97aece337 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -733,3 +733,15 @@ ament_add_gtest(test_graph_listener test_graph_listener.cpp) if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() + +ament_add_gtest(test_subscription_content_filtered_topic test_subscription_content_filtered_topic.cpp) +if(TARGET test_subscription_content_filtered_topic) + ament_target_dependencies(test_subscription_content_filtered_topic + "rcl_interfaces" + "rmw" + "rosidl_runtime_cpp" + "rosidl_typesupport_cpp" + "test_msgs" + ) + target_link_libraries(test_subscription_content_filtered_topic ${PROJECT_NAME} mimick) +endif() diff --git a/rclcpp/test/rclcpp/test_subscription.cpp b/rclcpp/test/rclcpp/test_subscription.cpp index c742b8b011..6facada4dd 100644 --- a/rclcpp/test/rclcpp/test_subscription.cpp +++ b/rclcpp/test/rclcpp/test_subscription.cpp @@ -27,7 +27,6 @@ #include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" -#include "test_msgs/msg/basic_types.hpp" #include "test_msgs/msg/empty.hpp" using namespace std::chrono_literals; @@ -145,44 +144,6 @@ class SubscriptionClass } }; -class TestCftSubscription : public ::testing::Test -{ -public: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - void SetUp() - { - node = std::make_shared("test_cft_subscription", "/ns"); - rclcpp::SubscriptionOptionsBase options_base; - options_base.content_filter_options.filter_expression = - "int32_value = %0"; - options_base.content_filter_options.expression_parameters = {"10"}; - rclcpp::SubscriptionOptionsWithAllocator> subscription_options( - options_base); - auto callback = [](std::shared_ptr) {}; - sub = node->create_subscription( - "topic", 10, callback, subscription_options); - } - - void TearDown() - { - sub.reset(); - node.reset(); - } - -protected: - rclcpp::Node::SharedPtr node; - rclcpp::Subscription::SharedPtr sub; -}; - /* Testing subscription construction and destruction. */ @@ -479,42 +440,6 @@ TEST_F(TestSubscription, handle_loaned_message) { EXPECT_NO_THROW(sub->handle_loaned_message(&msg, message_info)); } -TEST_F(TestCftSubscription, is_cft_supported) { - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_is_cft_supported, false); - EXPECT_FALSE(sub->is_cft_supported()); - } - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_is_cft_supported, true); - EXPECT_TRUE(sub->is_cft_supported()); - } -} - -TEST_F(TestCftSubscription, get_cft_expression_parameters_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_cft_expression_parameters, RCL_RET_ERROR); - - std::string filter_expression; - std::vector expression_parameters; - EXPECT_THROW( - sub->get_cft_expression_parameters(filter_expression, expression_parameters), - rclcpp::exceptions::RCLError); -} - -TEST_F(TestCftSubscription, set_cft_expression_parameters_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_set_cft_expression_parameters, RCL_RET_ERROR); - - std::string filter_expression = "int32_value = %0"; - std::string expression_parameter = "100"; - EXPECT_THROW( - sub->set_cft_expression_parameters(filter_expression, {expression_parameter}), - rclcpp::exceptions::RCLError); -} - /* Testing on_new_message callbacks. */ diff --git a/rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp b/rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp new file mode 100644 index 0000000000..3ed2df96a6 --- /dev/null +++ b/rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp @@ -0,0 +1,104 @@ +// 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 +#include +#include +#include +#include +#include + +#include "rclcpp/exceptions.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" + +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/msg/empty.hpp" + +class TestCftSubscription : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + void SetUp() + { + node = std::make_shared("test_cft_subscription", "/ns"); + + auto options = rclcpp::SubscriptionOptions(); + options.content_filter_options.filter_expression = "int32_value = %0"; + options.content_filter_options.expression_parameters = {"10"}; + + auto callback = [](std::shared_ptr) {}; + sub = node->create_subscription( + "topic", 10, callback, options); + } + + void TearDown() + { + sub.reset(); + node.reset(); + } + +protected: + rclcpp::Node::SharedPtr node; + rclcpp::Subscription::SharedPtr sub; +}; + +TEST_F(TestCftSubscription, is_cft_enabled) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, false); + EXPECT_FALSE(sub->is_cft_enabled()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, true); + EXPECT_TRUE(sub->is_cft_enabled()); + } +} + +TEST_F(TestCftSubscription, get_cft_expression_parameters_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_cft_expression_parameters, RCL_RET_ERROR); + + std::string filter_expression; + std::vector expression_parameters; + EXPECT_THROW( + sub->get_cft_expression_parameters(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); +} + +TEST_F(TestCftSubscription, set_cft_expression_parameters_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_cft_expression_parameters, RCL_RET_ERROR); + + std::string filter_expression = "int32_value = %0"; + std::string expression_parameter = "100"; + EXPECT_THROW( + sub->set_cft_expression_parameters(filter_expression, {expression_parameter}), + rclcpp::exceptions::RCLError); +} From 0124396af45eebe97e9b200f92937f8091e6ad7a Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 15:11:27 +0800 Subject: [PATCH 08/24] remove unused include header files and fix unscrutify Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_options.hpp | 3 --- rclcpp/src/rclcpp/subscription_base.cpp | 14 ++++++-------- rclcpp/src/rclcpp/utilities.cpp | 6 +++--- 3 files changed, 9 insertions(+), 14 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 77ad131e49..b1b2ca8a6e 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -21,9 +21,6 @@ #include #include -#include "rcutils/strdup.h" -#include "rcutils/types/string_array.h" - #include "rclcpp/callback_group.hpp" #include "rclcpp/detail/rmw_implementation_specific_subscription_payload.hpp" #include "rclcpp/intra_process_buffer_type.hpp" diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index b00eda5915..7e850e0e24 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -22,16 +22,12 @@ #include "rcpputils/scope_exit.hpp" -#include "rcutils/strdup.h" -#include "rcutils/types/string_array.h" - #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/qos_event.hpp" -#include "rclcpp/scope_exit.hpp" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -387,6 +383,7 @@ SubscriptionBase::set_cft_expression_parameters( ret, "failed to init subscription content_filtered_topic option"); } RCPPUTILS_SCOPE_EXIT( + { rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( @@ -395,7 +392,7 @@ SubscriptionBase::set_cft_expression_parameters( rcl_get_error_string().str); rcl_reset_error(); } - ); + }); ret = rcl_subscription_set_cft_expression_parameters( subscription_handle_.get(), @@ -414,7 +411,7 @@ SubscriptionBase::get_cft_expression_parameters( rcl_subscription_content_filtered_topic_options_t options = rcl_subscription_get_default_content_filtered_topic_options(); - // use rcl_content_filtered_topic_options_t + // use rcl_content_filtered_topic_options_t instead of char * and rcl_ret_t ret = rcl_subscription_get_cft_expression_parameters( subscription_handle_.get(), &options); @@ -423,7 +420,8 @@ SubscriptionBase::get_cft_expression_parameters( rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); } - RCLCPP_SCOPE_EXIT( + RCPPUTILS_SCOPE_EXIT( + { rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( @@ -432,7 +430,7 @@ SubscriptionBase::get_cft_expression_parameters( rcl_get_error_string().str); rcl_reset_error(); } - ); + }); rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = options.rmw_subscription_content_filtered_topic_options; diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 1dac78807d..535292b76c 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -217,11 +217,11 @@ get_c_string(const std::string & string_in) std::vector get_c_vector_string(const std::vector & strings) { - std::vector cstrings; + std::vector cstrings; cstrings.reserve(strings.size()); - for(size_t i = 0; i < strings.size(); ++i) { - cstrings.push_back(strings[i].c_str()); + for (size_t i = 0; i < strings.size(); ++i) { + cstrings.push_back(strings[i].c_str()); } return cstrings; From 4dc661b087808d7ae276fd5b445a1d6fbb5c3e9e Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 25 Oct 2021 15:21:58 +0800 Subject: [PATCH 09/24] update comment Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index ab190d8c43..8bcb87fc57 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -493,7 +493,7 @@ class SubscriptionBase : public std::enable_shared_from_this /// Check if content filtered topic feature of the subscription instance is enabled. /** - * \return boolean flag indicating if middleware can support content filtered topic feature. + * \return boolean flag indicating if the content filtered topic of this subscription is enabled. */ RCLCPP_PUBLIC bool From 771e5a0c8893d66334b284d20fbad7250ebbd1e8 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 26 Oct 2021 13:37:06 +0800 Subject: [PATCH 10/24] update comment Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 2 ++ rclcpp/include/rclcpp/subscription_options.hpp | 9 ++++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 8bcb87fc57..cfda4db4dc 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -502,7 +502,9 @@ class SubscriptionBase : public std::enable_shared_from_this /// Set the filter expression and expression parameters for the subscription. /** * \param[in] filter_expression An filter expression to set. + * \sa SubscriptionOptionsBase::ContentFilterOptions::filter_expression * \param[in] expression_parameters Array of expression parameters to set. + * \sa SubscriptionOptionsBase::ContentFilterOptions::expression_parameters * \throws RCLBadAlloc if memory cannot be allocated * \throws RCLError if an unexpect error occurs */ diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index b1b2ca8a6e..8c23aacc99 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -82,12 +82,15 @@ struct SubscriptionOptionsBase QosOverridingOptions qos_overriding_options; - // Options to configure content filtered topic in the subscription. + /// Options to configure content filtered topic in the subscription. struct ContentFilterOptions { - // Filter expression like the 'where' part of SQL statement + /// Filter expression is similar to the WHERE part of an SQL clause std::string filter_expression; - // Expression parameters if there is placeholder '%n' in the filter expression + /** + * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) + * in the filter_expression + */ std::vector expression_parameters; }; From 0bf1f520ec4c2d1b73f6baee63a4ce3499a54d9e Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 26 Oct 2021 14:24:59 +0800 Subject: [PATCH 11/24] rename Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 4 +- .../include/rclcpp/subscription_options.hpp | 4 +- rclcpp/src/rclcpp/subscription_base.cpp | 38 +++++++++---------- rclcpp/test/rclcpp/CMakeLists.txt | 8 ++-- ...p => test_subscription_content_filter.cpp} | 12 +++--- 5 files changed, 33 insertions(+), 33 deletions(-) rename rclcpp/test/rclcpp/{test_subscription_content_filtered_topic.cpp => test_subscription_content_filter.cpp} (84%) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index cfda4db4dc..9170fb1dc4 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -511,7 +511,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - set_cft_expression_parameters( + set_content_filter( const std::string & filter_expression, const std::vector & expression_parameters = {}); @@ -525,7 +525,7 @@ class SubscriptionBase : public std::enable_shared_from_this RCLCPP_PUBLIC virtual void - get_cft_expression_parameters( + get_content_filter( std::string & filter_expression, std::vector & expression_parameters) const; diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 8c23aacc99..e44704c7b0 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -141,14 +141,14 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase if (!content_filter_options.filter_expression.empty()) { std::vector cstrings = get_c_vector_string(content_filter_options.expression_parameters); - rcl_ret_t ret = rcl_subscription_options_set_content_filtered_topic_options( + rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( get_c_string(content_filter_options.filter_expression), cstrings.size(), cstrings.data(), &result); if (RCL_RET_OK != ret) { rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to set content_filtered_topic_options"); + ret, "failed to set content_filter_options"); } } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 7e850e0e24..2976021ab5 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -364,16 +364,16 @@ SubscriptionBase::is_cft_enabled() const } void -SubscriptionBase::set_cft_expression_parameters( +SubscriptionBase::set_content_filter( const std::string & filter_expression, const std::vector & expression_parameters) { - 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(); std::vector cstrings = get_c_vector_string(expression_parameters); - rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_init( + rcl_ret_t ret = rcl_subscription_content_filter_options_init( get_c_string(filter_expression), cstrings.size(), cstrings.data(), @@ -384,7 +384,7 @@ SubscriptionBase::set_cft_expression_parameters( } RCPPUTILS_SCOPE_EXIT( { - rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), @@ -394,7 +394,7 @@ SubscriptionBase::set_cft_expression_parameters( } }); - ret = rcl_subscription_set_cft_expression_parameters( + ret = rcl_subscription_set_content_filter( subscription_handle_.get(), &options); @@ -404,15 +404,15 @@ SubscriptionBase::set_cft_expression_parameters( } void -SubscriptionBase::get_cft_expression_parameters( +SubscriptionBase::get_content_filter( std::string & filter_expression, std::vector & expression_parameters) const { - 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(); - // use rcl_content_filtered_topic_options_t instead of char * and - rcl_ret_t ret = rcl_subscription_get_cft_expression_parameters( + // use rcl_content_filter_options_t instead of char * and + rcl_ret_t ret = rcl_subscription_get_content_filter( subscription_handle_.get(), &options); @@ -422,7 +422,7 @@ SubscriptionBase::get_cft_expression_parameters( RCPPUTILS_SCOPE_EXIT( { - rcl_ret_t ret = rcl_subscription_content_filtered_topic_options_fini(&options); + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), @@ -432,16 +432,16 @@ SubscriptionBase::get_cft_expression_parameters( } }); - rmw_subscription_content_filtered_topic_options_t * content_filtered_topic_options = - options.rmw_subscription_content_filtered_topic_options; - if (content_filtered_topic_options->filter_expression) { - filter_expression = content_filtered_topic_options->filter_expression; + rmw_subscription_content_filter_options_t * content_filter_options = + options.rmw_subscription_content_filter_options; + if (content_filter_options->filter_expression) { + filter_expression = content_filter_options->filter_expression; } - if (content_filtered_topic_options->expression_parameters) { - for (size_t i = 0; i < content_filtered_topic_options->expression_parameters->size; ++i) { + if (content_filter_options->expression_parameters) { + for (size_t i = 0; i < content_filter_options->expression_parameters->size; ++i) { expression_parameters.push_back( - content_filtered_topic_options->expression_parameters->data[i]); + content_filter_options->expression_parameters->data[i]); } } } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index c97aece337..19aecf8845 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -734,14 +734,14 @@ if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_subscription_content_filtered_topic test_subscription_content_filtered_topic.cpp) -if(TARGET test_subscription_content_filtered_topic) - ament_target_dependencies(test_subscription_content_filtered_topic +ament_add_gtest(test_subscription_content_filter test_subscription_content_filter.cpp) +if(TARGET test_subscription_content_filter) + ament_target_dependencies(test_subscription_content_filter "rcl_interfaces" "rmw" "rosidl_runtime_cpp" "rosidl_typesupport_cpp" "test_msgs" ) - target_link_libraries(test_subscription_content_filtered_topic ${PROJECT_NAME} mimick) + target_link_libraries(test_subscription_content_filter ${PROJECT_NAME} mimick) endif() diff --git a/rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp similarity index 84% rename from rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp rename to rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 3ed2df96a6..01da0a8f2d 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filtered_topic.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -81,24 +81,24 @@ TEST_F(TestCftSubscription, is_cft_enabled) { } } -TEST_F(TestCftSubscription, get_cft_expression_parameters_error) { +TEST_F(TestCftSubscription, get_content_filter_error) { auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_cft_expression_parameters, RCL_RET_ERROR); + "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); std::string filter_expression; std::vector expression_parameters; EXPECT_THROW( - sub->get_cft_expression_parameters(filter_expression, expression_parameters), + sub->get_content_filter(filter_expression, expression_parameters), rclcpp::exceptions::RCLError); } -TEST_F(TestCftSubscription, set_cft_expression_parameters_error) { +TEST_F(TestCftSubscription, set_content_filter_error) { auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_set_cft_expression_parameters, RCL_RET_ERROR); + "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); std::string filter_expression = "int32_value = %0"; std::string expression_parameter = "100"; EXPECT_THROW( - sub->set_cft_expression_parameters(filter_expression, {expression_parameter}), + sub->set_content_filter(filter_expression, {expression_parameter}), rclcpp::exceptions::RCLError); } From b044aa2c7d51b1e3f65e048c9bee0bad725de15e Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 14 Jan 2022 15:38:26 +0800 Subject: [PATCH 12/24] refactor test Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/wait_for_message.hpp | 3 + rclcpp/test/rclcpp/CMakeLists.txt | 25 +- .../test_subscription_content_filter.cpp | 221 +++++++++++++++++- 3 files changed, 231 insertions(+), 18 deletions(-) diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index 25c45ad782..c4a2ef282c 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -59,6 +59,9 @@ bool wait_for_message( RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); ); wait_set.add_guard_condition(gc); auto ret = wait_set.wait(time_to_wait); + // if it is fixed in other PR, remove them and rebase. + wait_set.remove_subscription(subscription); + wait_set.remove_guard_condition(gc); if (ret.kind() != rclcpp::WaitResultKind::Ready) { return false; } diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 19aecf8845..cfbc4c1f18 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -734,14 +734,19 @@ if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() -ament_add_gtest(test_subscription_content_filter test_subscription_content_filter.cpp) -if(TARGET test_subscription_content_filter) - ament_target_dependencies(test_subscription_content_filter - "rcl_interfaces" - "rmw" - "rosidl_runtime_cpp" - "rosidl_typesupport_cpp" - "test_msgs" +function(test_subscription_content_filter_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_subscription_content_filter${target_suffix} + test_subscription_content_filter.cpp + ENV ${rmw_implementation_env_var} ) - target_link_libraries(test_subscription_content_filter ${PROJECT_NAME} mimick) -endif() + if(TARGET test_subscription_content_filter${target_suffix}) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_subscription_content_filter${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() +endfunction() +call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 01da0a8f2d..ab6998517c 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -23,6 +23,7 @@ #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_for_message.hpp" #include "../mocking_utils/patch.hpp" #include "../utils/rclcpp_gtest_macros.hpp" @@ -30,7 +31,14 @@ #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/msg/empty.hpp" -class TestCftSubscription : public ::testing::Test +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::testing::Test { public: static void SetUpTestCase() @@ -45,15 +53,17 @@ class TestCftSubscription : public ::testing::Test void SetUp() { - node = std::make_shared("test_cft_subscription", "/ns"); + node = std::make_shared("test_content_filter_node", "/ns"); + context = node->get_node_options().context(); + qos.reliable().transient_local(); auto options = rclcpp::SubscriptionOptions(); - options.content_filter_options.filter_expression = "int32_value = %0"; - options.content_filter_options.expression_parameters = {"10"}; + options.content_filter_options.filter_expression = filter_expression_init; + options.content_filter_options.expression_parameters = expression_parameters_1; auto callback = [](std::shared_ptr) {}; sub = node->create_subscription( - "topic", 10, callback, options); + "content_filter_topic", qos, callback, options); } void TearDown() @@ -62,12 +72,49 @@ class TestCftSubscription : public ::testing::Test node.reset(); } + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node); + } + return true; + } + protected: rclcpp::Node::SharedPtr node; + rclcpp::Context::SharedPtr context; + rclcpp::QoS qos{rclcpp::KeepLast{10}}; rclcpp::Subscription::SharedPtr sub; + + std::string filter_expression_init = "int32_value = %0"; + std::vector expression_parameters_1 = {"3"}; + std::vector expression_parameters_2 = {"4"}; }; -TEST_F(TestCftSubscription, is_cft_enabled) { +bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) +{ + return m1.bool_value == m2.bool_value && + m1.byte_value == m2.byte_value && + m1.char_value == m2.char_value && + m1.float32_value == m2.float32_value && + m1.float64_value == m2.float64_value && + m1.int8_value == m2.int8_value && + m1.uint8_value == m2.uint8_value && + m1.int16_value == m2.int16_value && + m1.uint16_value == m2.uint16_value && + m1.int32_value == m2.int32_value && + m1.uint32_value == m2.uint32_value && + m1.int64_value == m2.int64_value && + m1.uint64_value == m2.uint64_value; +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_is_cft_enabled, false); @@ -81,7 +128,7 @@ TEST_F(TestCftSubscription, is_cft_enabled) { } } -TEST_F(TestCftSubscription, get_content_filter_error) { +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); @@ -92,7 +139,7 @@ TEST_F(TestCftSubscription, get_content_filter_error) { rclcpp::exceptions::RCLError); } -TEST_F(TestCftSubscription, set_content_filter_error) { +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); @@ -102,3 +149,161 @@ TEST_F(TestCftSubscription, set_content_filter_error) { sub->set_content_filter(filter_expression, {expression_parameter}), rclcpp::exceptions::RCLError); } + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { + std::string filter_expression; + std::vector expression_parameters; + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->get_content_filter(filter_expression, expression_parameters)); + + EXPECT_EQ(filter_expression, filter_expression_init); + EXPECT_EQ(expression_parameters, expression_parameters_1); + } else { + EXPECT_THROW( + sub->get_content_filter(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + } else { + EXPECT_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_FALSE(receive); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter("")); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} From 43c59e8a3e35754405776afd744d63a42df642ab Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 17 Mar 2022 11:13:54 +0800 Subject: [PATCH 13/24] relate to `rcutils_string_array_t expression_parameters` changed in rmw Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/subscription_base.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2976021ab5..8d7085bbc5 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -438,10 +438,8 @@ SubscriptionBase::get_content_filter( filter_expression = content_filter_options->filter_expression; } - if (content_filter_options->expression_parameters) { - for (size_t i = 0; i < content_filter_options->expression_parameters->size; ++i) { - expression_parameters.push_back( - content_filter_options->expression_parameters->data[i]); - } + for (size_t i = 0; i < content_filter_options->expression_parameters.size; ++i) { + expression_parameters.push_back( + content_filter_options->expression_parameters.data[i]); } } From e72216e694e85058359ea29c686829d4888dbc95 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 24 Mar 2022 11:01:40 +0800 Subject: [PATCH 14/24] remove the implementation temporary, add them with fallback in the feature 1. add DEFINE_CONTENT_FILTER with default(ON) to build content filter interfaces 2. user need a compile option(CONTENT_FILTER_ENABLE) to turn on the feature to set the filter or call the interfaces Signed-off-by: Chen Lihui --- rclcpp/CMakeLists.txt | 5 + rclcpp/include/rclcpp/subscription_base.hpp | 10 +- .../include/rclcpp/subscription_options.hpp | 17 +- rclcpp/include/rclcpp/utilities.hpp | 9 - rclcpp/include/rclcpp/wait_for_message.hpp | 3 - rclcpp/src/rclcpp/subscription_base.cpp | 81 +------ rclcpp/src/rclcpp/utilities.cpp | 13 - .../test_subscription_content_filter.cpp | 229 +----------------- 8 files changed, 18 insertions(+), 349 deletions(-) diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 9572f422a8..de424342ed 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -2,6 +2,11 @@ cmake_minimum_required(VERSION 3.12) project(rclcpp) +option(DEFINE_CONTENT_FILTER "Content filter feature support" ON) +if(DEFINE_CONTENT_FILTER) + add_definitions(-DCONTENT_FILTER_ENABLE) +endif() + find_package(Threads REQUIRED) find_package(ament_cmake_ros REQUIRED) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 9170fb1dc4..088df405ef 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -491,14 +491,7 @@ class SubscriptionBase : public std::enable_shared_from_this event_handlers_[event_type]->clear_on_ready_callback(); } - /// Check if content filtered topic feature of the subscription instance is enabled. - /** - * \return boolean flag indicating if the content filtered topic of this subscription is enabled. - */ - RCLCPP_PUBLIC - bool - is_cft_enabled() const; - +#ifdef CONTENT_FILTER_ENABLE /// Set the filter expression and expression parameters for the subscription. /** * \param[in] filter_expression An filter expression to set. @@ -528,6 +521,7 @@ class SubscriptionBase : public std::enable_shared_from_this get_content_filter( std::string & filter_expression, std::vector & expression_parameters) const; +#endif // CONTENT_FILTER_ENABLE protected: template diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index e44704c7b0..b16fa9bbe4 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -82,6 +82,7 @@ struct SubscriptionOptionsBase QosOverridingOptions qos_overriding_options; +#ifdef CONTENT_FILTER_ENABLE /// Options to configure content filtered topic in the subscription. struct ContentFilterOptions { @@ -95,6 +96,7 @@ struct SubscriptionOptionsBase }; ContentFilterOptions content_filter_options; +#endif // CONTENT_FILTER_ENABLE }; /// Structure containing optional configuration for Subscriptions. @@ -137,21 +139,6 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } - // Copy content_filter_options into rcl_subscription_options. - if (!content_filter_options.filter_expression.empty()) { - std::vector cstrings = - get_c_vector_string(content_filter_options.expression_parameters); - rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( - get_c_string(content_filter_options.filter_expression), - cstrings.size(), - cstrings.data(), - &result); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to set content_filter_options"); - } - } - return result; } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 20ded67ad9..fa48a010c4 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -321,15 +321,6 @@ RCLCPP_PUBLIC const char * get_c_string(const std::string & string_in); -/// Return the std::vector of C string from the given std::vector. -/** - * \param[in] string_in is a std::string - * \return the std::vector of C string from the std::vector - */ -RCLCPP_PUBLIC -std::vector -get_c_vector_string(const std::vector & strings); - } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index c4a2ef282c..25c45ad782 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -59,9 +59,6 @@ bool wait_for_message( RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); ); wait_set.add_guard_condition(gc); auto ret = wait_set.wait(time_to_wait); - // if it is fixed in other PR, remove them and rebase. - wait_set.remove_subscription(subscription); - wait_set.remove_guard_condition(gc); if (ret.kind() != rclcpp::WaitResultKind::Ready) { return false; } diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 8d7085bbc5..2e61b02e7e 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -20,8 +20,6 @@ #include #include -#include "rcpputils/scope_exit.hpp" - #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -357,50 +355,16 @@ SubscriptionBase::set_on_new_message_callback( } } -bool -SubscriptionBase::is_cft_enabled() const -{ - return rcl_subscription_is_cft_enabled(subscription_handle_.get()); -} - +#ifdef CONTENT_FILTER_ENABLE void SubscriptionBase::set_content_filter( const std::string & filter_expression, const std::vector & expression_parameters) { - rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); - - std::vector cstrings = - get_c_vector_string(expression_parameters); - rcl_ret_t ret = rcl_subscription_content_filter_options_init( - get_c_string(filter_expression), - cstrings.size(), - cstrings.data(), - &options); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error( - ret, "failed to init subscription content_filtered_topic option"); - } - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Failed to fini subscription content_filtered_topic option: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - }); + static_cast(filter_expression); + static_cast(expression_parameters); - ret = rcl_subscription_set_content_filter( - subscription_handle_.get(), - &options); - - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); - } + rclcpp::exceptions::throw_from_rcl_error(RMW_RET_UNSUPPORTED, "content filter unsupported"); } void @@ -408,38 +372,9 @@ SubscriptionBase::get_content_filter( std::string & filter_expression, std::vector & expression_parameters) const { - rcl_subscription_content_filter_options_t options = - rcl_subscription_get_default_content_filter_options(); - - // use rcl_content_filter_options_t instead of char * and - rcl_ret_t ret = rcl_subscription_get_content_filter( - subscription_handle_.get(), - &options); + static_cast(filter_expression); + static_cast(expression_parameters); - if (RCL_RET_OK != ret) { - rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); - } - - RCPPUTILS_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); - if (RCL_RET_OK != ret) { - RCLCPP_ERROR( - rclcpp::get_logger("rclcpp"), - "Failed to fini subscription content_filtered_topic option: %s", - rcl_get_error_string().str); - rcl_reset_error(); - } - }); - - rmw_subscription_content_filter_options_t * content_filter_options = - options.rmw_subscription_content_filter_options; - if (content_filter_options->filter_expression) { - filter_expression = content_filter_options->filter_expression; - } - - for (size_t i = 0; i < content_filter_options->expression_parameters.size; ++i) { - expression_parameters.push_back( - content_filter_options->expression_parameters.data[i]); - } + rclcpp::exceptions::throw_from_rcl_error(RMW_RET_UNSUPPORTED, "content filter unsupported"); } +#endif // CONTENT_FILTER_ENABLE diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 535292b76c..a6ead85c03 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -214,17 +214,4 @@ get_c_string(const std::string & string_in) return string_in.c_str(); } -std::vector -get_c_vector_string(const std::vector & strings) -{ - std::vector cstrings; - cstrings.reserve(strings.size()); - - for (size_t i = 0; i < strings.size(); ++i) { - cstrings.push_back(strings[i].c_str()); - } - - return cstrings; -} - } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index ab6998517c..925b732fcc 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -14,22 +14,14 @@ #include -#include -#include #include #include -#include #include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" -#include "rclcpp/wait_for_message.hpp" - -#include "../mocking_utils/patch.hpp" -#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/msg/basic_types.hpp" -#include "test_msgs/msg/empty.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -54,16 +46,11 @@ class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::t void SetUp() { node = std::make_shared("test_content_filter_node", "/ns"); - context = node->get_node_options().context(); - qos.reliable().transient_local(); auto options = rclcpp::SubscriptionOptions(); - options.content_filter_options.filter_expression = filter_expression_init; - options.content_filter_options.expression_parameters = expression_parameters_1; - auto callback = [](std::shared_ptr) {}; sub = node->create_subscription( - "content_filter_topic", qos, callback, options); + "content_filter_topic", qos.reliable().transient_local(), callback, options); } void TearDown() @@ -72,66 +59,13 @@ class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::t node.reset(); } - template - bool wait_for(const Condition & condition, const Duration & timeout) - { - using clock = std::chrono::system_clock; - auto start = clock::now(); - while (!condition()) { - if ((clock::now() - start) > timeout) { - return false; - } - rclcpp::spin_some(node); - } - return true; - } - protected: rclcpp::Node::SharedPtr node; - rclcpp::Context::SharedPtr context; rclcpp::QoS qos{rclcpp::KeepLast{10}}; rclcpp::Subscription::SharedPtr sub; - - std::string filter_expression_init = "int32_value = %0"; - std::vector expression_parameters_1 = {"3"}; - std::vector expression_parameters_2 = {"4"}; }; -bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) -{ - return m1.bool_value == m2.bool_value && - m1.byte_value == m2.byte_value && - m1.char_value == m2.char_value && - m1.float32_value == m2.float32_value && - m1.float64_value == m2.float64_value && - m1.int8_value == m2.int8_value && - m1.uint8_value == m2.uint8_value && - m1.int16_value == m2.int16_value && - m1.uint16_value == m2.uint16_value && - m1.int32_value == m2.int32_value && - m1.uint32_value == m2.uint32_value && - m1.int64_value == m2.int64_value && - m1.uint64_value == m2.uint64_value; -} - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_is_cft_enabled, false); - EXPECT_FALSE(sub->is_cft_enabled()); - } - - { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_is_cft_enabled, true); - EXPECT_TRUE(sub->is_cft_enabled()); - } -} - TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); - std::string filter_expression; std::vector expression_parameters; EXPECT_THROW( @@ -140,170 +74,9 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content } TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { - auto mock = mocking_utils::patch_and_return( - "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); - std::string filter_expression = "int32_value = %0"; std::string expression_parameter = "100"; EXPECT_THROW( sub->set_content_filter(filter_expression, {expression_parameter}), rclcpp::exceptions::RCLError); } - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { - std::string filter_expression; - std::vector expression_parameters; - - if (sub->is_cft_enabled()) { - EXPECT_NO_THROW( - sub->get_content_filter(filter_expression, expression_parameters)); - - EXPECT_EQ(filter_expression, filter_expression_init); - EXPECT_EQ(expression_parameters, expression_parameters_1); - } else { - EXPECT_THROW( - sub->get_content_filter(filter_expression, expression_parameters), - rclcpp::exceptions::RCLError); - } -} - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { - if (sub->is_cft_enabled()) { - EXPECT_NO_THROW( - sub->set_content_filter(filter_expression_init, expression_parameters_2)); - } else { - EXPECT_THROW( - sub->set_content_filter(filter_expression_init, expression_parameters_2), - rclcpp::exceptions::RCLError); - } -} - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { - using namespace std::chrono_literals; - { - test_msgs::msg::BasicTypes msg; - rclcpp::MessageInfo msg_info; - EXPECT_FALSE(sub->take(msg, msg_info)); - } - { - rclcpp::PublisherOptions po; - auto pub = node->create_publisher("content_filter_topic", qos, po); - - auto connected = [pub, sub = this->sub]() -> bool { - return pub->get_subscription_count() && sub->get_publisher_count(); - }; - ASSERT_TRUE(wait_for(connected, 5s)); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 3; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - EXPECT_TRUE(receive); - EXPECT_EQ(original_message, output_message); - - if (sub->is_cft_enabled()) { - EXPECT_NO_THROW( - sub->set_content_filter(filter_expression_init, expression_parameters_2)); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 3; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - EXPECT_FALSE(receive); - } - } -} - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { - using namespace std::chrono_literals; - { - test_msgs::msg::BasicTypes msg; - rclcpp::MessageInfo msg_info; - EXPECT_FALSE(sub->take(msg, msg_info)); - } - { - rclcpp::PublisherOptions po; - auto pub = node->create_publisher("content_filter_topic", qos, po); - - auto connected = [pub, sub = this->sub]() -> bool { - return pub->get_subscription_count() && sub->get_publisher_count(); - }; - ASSERT_TRUE(wait_for(connected, 5s)); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 4; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - if (sub->is_cft_enabled()) { - EXPECT_FALSE(receive); - } else { - EXPECT_TRUE(receive); - EXPECT_EQ(original_message, output_message); - } - - if (sub->is_cft_enabled()) { - EXPECT_NO_THROW( - sub->set_content_filter(filter_expression_init, expression_parameters_2)); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 4; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - EXPECT_TRUE(receive); - EXPECT_EQ(original_message, output_message); - } - } -} - -TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { - using namespace std::chrono_literals; - { - test_msgs::msg::BasicTypes msg; - rclcpp::MessageInfo msg_info; - EXPECT_FALSE(sub->take(msg, msg_info)); - } - { - rclcpp::PublisherOptions po; - auto pub = node->create_publisher("content_filter_topic", qos, po); - - auto connected = [pub, sub = this->sub]() -> bool { - return pub->get_subscription_count() && sub->get_publisher_count(); - }; - ASSERT_TRUE(wait_for(connected, 5s)); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 4; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - if (sub->is_cft_enabled()) { - EXPECT_FALSE(receive); - } else { - EXPECT_TRUE(receive); - EXPECT_EQ(original_message, output_message); - } - - if (sub->is_cft_enabled()) { - EXPECT_NO_THROW( - sub->set_content_filter("")); - - test_msgs::msg::BasicTypes original_message; - original_message.int32_value = 4; - pub->publish(original_message); - - test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); - EXPECT_TRUE(receive); - EXPECT_EQ(original_message, output_message); - } - } -} From 88320c9ded7cb5057edb6e5d280be76d0fea15aa Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 10:40:49 +0800 Subject: [PATCH 15/24] update comments and check the option to build content filter test Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 1 + rclcpp/test/rclcpp/CMakeLists.txt | 32 +++++++++++---------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 088df405ef..0b40f11b4d 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -496,6 +496,7 @@ class SubscriptionBase : public std::enable_shared_from_this /** * \param[in] filter_expression An filter expression to set. * \sa SubscriptionOptionsBase::ContentFilterOptions::filter_expression + * Use empty string ("") can reset (or clear) the content filter setting of a subscription. * \param[in] expression_parameters Array of expression parameters to set. * \sa SubscriptionOptionsBase::ContentFilterOptions::expression_parameters * \throws RCLBadAlloc if memory cannot be allocated diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index cfbc4c1f18..7da5372667 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -734,19 +734,21 @@ if(TARGET test_graph_listener) target_link_libraries(test_graph_listener ${PROJECT_NAME} mimick) endif() -function(test_subscription_content_filter_for_rmw_implementation) - set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) - ament_add_gmock(test_subscription_content_filter${target_suffix} - test_subscription_content_filter.cpp - ENV ${rmw_implementation_env_var} - ) - if(TARGET test_subscription_content_filter${target_suffix}) - target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) - ament_target_dependencies(test_subscription_content_filter${target_suffix} - "rcpputils" - "rosidl_typesupport_cpp" - "test_msgs" +if(DEFINE_CONTENT_FILTER) + function(test_subscription_content_filter_for_rmw_implementation) + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + ament_add_gmock(test_subscription_content_filter${target_suffix} + test_subscription_content_filter.cpp + ENV ${rmw_implementation_env_var} ) - endif() -endfunction() -call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) + if(TARGET test_subscription_content_filter${target_suffix}) + target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) + ament_target_dependencies(test_subscription_content_filter${target_suffix} + "rcpputils" + "rosidl_typesupport_cpp" + "test_msgs" + ) + endif() + endfunction() + call_for_each_rmw_implementation(test_subscription_content_filter_for_rmw_implementation) +endif() From 98e3872c3a1979bbf2f4a9c3eaf0d3dff7816e4b Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 25 Mar 2022 13:50:52 +0800 Subject: [PATCH 16/24] add DDS content filter implementation Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 8 + .../include/rclcpp/subscription_options.hpp | 16 ++ rclcpp/include/rclcpp/utilities.hpp | 9 + rclcpp/src/rclcpp/subscription_base.cpp | 76 +++++- rclcpp/src/rclcpp/utilities.cpp | 13 + .../test_subscription_content_filter.cpp | 234 +++++++++++++++++- 6 files changed, 349 insertions(+), 7 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 0b40f11b4d..3e41a19575 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -492,6 +492,14 @@ class SubscriptionBase : public std::enable_shared_from_this } #ifdef CONTENT_FILTER_ENABLE + /// Check if content filtered topic feature of the subscription instance is enabled. + /** + * \return boolean flag indicating if the content filtered topic of this subscription is enabled. + */ + RCLCPP_PUBLIC + bool + is_cft_enabled() const; + /// Set the filter expression and expression parameters for the subscription. /** * \param[in] filter_expression An filter expression to set. diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index b16fa9bbe4..be23f20fa3 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -139,6 +139,22 @@ struct SubscriptionOptionsWithAllocator : public SubscriptionOptionsBase rmw_implementation_payload->modify_rmw_subscription_options(result.rmw_subscription_options); } +#ifdef CONTENT_FILTER_ENABLE + // Copy content_filter_options into rcl_subscription_options. + if (!content_filter_options.filter_expression.empty()) { + std::vector cstrings = + get_c_vector_string(content_filter_options.expression_parameters); + rcl_ret_t ret = rcl_subscription_options_set_content_filter_options( + get_c_string(content_filter_options.filter_expression), + cstrings.size(), + cstrings.data(), + &result); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to set content_filter_options"); + } + } +#endif // CONTENT_FILTER_ENABLE return result; } diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index fa48a010c4..20ded67ad9 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -321,6 +321,15 @@ RCLCPP_PUBLIC const char * get_c_string(const std::string & string_in); +/// Return the std::vector of C string from the given std::vector. +/** + * \param[in] string_in is a std::string + * \return the std::vector of C string from the std::vector + */ +RCLCPP_PUBLIC +std::vector +get_c_vector_string(const std::vector & strings); + } // namespace rclcpp #endif // RCLCPP__UTILITIES_HPP_ diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 2e61b02e7e..c8cd3c2aa3 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -20,6 +20,8 @@ #include #include +#include "rcpputils/scope_exit.hpp" + #include "rclcpp/exceptions.hpp" #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/experimental/intra_process_manager.hpp" @@ -356,15 +358,50 @@ SubscriptionBase::set_on_new_message_callback( } #ifdef CONTENT_FILTER_ENABLE +bool +SubscriptionBase::is_cft_enabled() const +{ + return rcl_subscription_is_cft_enabled(subscription_handle_.get()); +} + void SubscriptionBase::set_content_filter( const std::string & filter_expression, const std::vector & expression_parameters) { - static_cast(filter_expression); - static_cast(expression_parameters); + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + std::vector cstrings = + get_c_vector_string(expression_parameters); + rcl_ret_t ret = rcl_subscription_content_filter_options_init( + get_c_string(filter_expression), + cstrings.size(), + cstrings.data(), + &options); + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error( + ret, "failed to init subscription content_filtered_topic option"); + } + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); - rclcpp::exceptions::throw_from_rcl_error(RMW_RET_UNSUPPORTED, "content filter unsupported"); + ret = rcl_subscription_set_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to set cft expression parameters"); + } } void @@ -372,9 +409,36 @@ SubscriptionBase::get_content_filter( std::string & filter_expression, std::vector & expression_parameters) const { - static_cast(filter_expression); - static_cast(expression_parameters); + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + rcl_ret_t ret = rcl_subscription_get_content_filter( + subscription_handle_.get(), + &options); + + if (RCL_RET_OK != ret) { + rclcpp::exceptions::throw_from_rcl_error(ret, "failed to get cft expression parameters"); + } - rclcpp::exceptions::throw_from_rcl_error(RMW_RET_UNSUPPORTED, "content filter unsupported"); + RCPPUTILS_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + if (RCL_RET_OK != ret) { + RCLCPP_ERROR( + rclcpp::get_logger("rclcpp"), + "Failed to fini subscription content_filtered_topic option: %s", + rcl_get_error_string().str); + rcl_reset_error(); + } + }); + + rmw_subscription_content_filter_options_t & content_filter_options = + options.rmw_subscription_content_filter_options; + filter_expression = content_filter_options.filter_expression; + + for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) { + expression_parameters.push_back( + content_filter_options.expression_parameters.data[i]); + } } #endif // CONTENT_FILTER_ENABLE diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index a6ead85c03..535292b76c 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -214,4 +214,17 @@ get_c_string(const std::string & string_in) return string_in.c_str(); } +std::vector +get_c_vector_string(const std::vector & strings) +{ + std::vector cstrings; + cstrings.reserve(strings.size()); + + for (size_t i = 0; i < strings.size(); ++i) { + cstrings.push_back(strings[i].c_str()); + } + + return cstrings; +} + } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 925b732fcc..37956559da 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -14,12 +14,19 @@ #include +#include +#include #include #include +#include #include #include "rclcpp/exceptions.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/wait_for_message.hpp" + +#include "../mocking_utils/patch.hpp" +#include "../utils/rclcpp_gtest_macros.hpp" #include "test_msgs/msg/basic_types.hpp" @@ -46,11 +53,16 @@ class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::t void SetUp() { node = std::make_shared("test_content_filter_node", "/ns"); + context = node->get_node_options().context(); + qos.reliable().transient_local(); auto options = rclcpp::SubscriptionOptions(); + options.content_filter_options.filter_expression = filter_expression_init; + options.content_filter_options.expression_parameters = expression_parameters_1; + auto callback = [](std::shared_ptr) {}; sub = node->create_subscription( - "content_filter_topic", qos.reliable().transient_local(), callback, options); + "content_filter_topic", qos, callback, options); } void TearDown() @@ -59,13 +71,66 @@ class CLASSNAME (TestContentFilterSubscription, RMW_IMPLEMENTATION) : public ::t node.reset(); } + template + bool wait_for(const Condition & condition, const Duration & timeout) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + rclcpp::spin_some(node); + } + return true; + } + protected: rclcpp::Node::SharedPtr node; + rclcpp::Context::SharedPtr context; rclcpp::QoS qos{rclcpp::KeepLast{10}}; rclcpp::Subscription::SharedPtr sub; + + std::string filter_expression_init = "int32_value = %0"; + std::vector expression_parameters_1 = {"3"}; + std::vector expression_parameters_2 = {"4"}; }; +bool operator==(const test_msgs::msg::BasicTypes & m1, const test_msgs::msg::BasicTypes & m2) +{ + return m1.bool_value == m2.bool_value && + m1.byte_value == m2.byte_value && + m1.char_value == m2.char_value && + m1.float32_value == m2.float32_value && + m1.float64_value == m2.float64_value && + m1.int8_value == m2.int8_value && + m1.uint8_value == m2.uint8_value && + m1.int16_value == m2.int16_value && + m1.uint16_value == m2.uint16_value && + m1.int32_value == m2.int32_value && + m1.uint32_value == m2.uint32_value && + m1.int64_value == m2.int64_value && + m1.uint64_value == m2.uint64_value; +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), is_cft_enabled) { + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, false); + EXPECT_FALSE(sub->is_cft_enabled()); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_is_cft_enabled, true); + EXPECT_TRUE(sub->is_cft_enabled()); + } +} + TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); + std::string filter_expression; std::vector expression_parameters; EXPECT_THROW( @@ -74,9 +139,176 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content } TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter_error) { + auto mock = mocking_utils::patch_and_return( + "lib:rclcpp", rcl_subscription_set_content_filter, RCL_RET_ERROR); + std::string filter_expression = "int32_value = %0"; std::string expression_parameter = "100"; EXPECT_THROW( sub->set_content_filter(filter_expression, {expression_parameter}), rclcpp::exceptions::RCLError); } + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { + std::string filter_expression; + std::vector expression_parameters; + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->get_content_filter(filter_expression, expression_parameters)); + + EXPECT_EQ(filter_expression, filter_expression_init); + EXPECT_EQ(expression_parameters, expression_parameters_1); + } else { + EXPECT_THROW( + sub->get_content_filter(filter_expression, expression_parameters), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content_filter) { + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + } else { + EXPECT_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2), + rclcpp::exceptions::RCLError); + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_begin) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 3; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_FALSE(receive); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_get_later) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter(filter_expression_init, expression_parameters_2)); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} + +TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_filter_reset) { + using namespace std::chrono_literals; + { + test_msgs::msg::BasicTypes msg; + rclcpp::MessageInfo msg_info; + EXPECT_FALSE(sub->take(msg, msg_info)); + } + { + rclcpp::PublisherOptions po; + auto pub = node->create_publisher("content_filter_topic", qos, po); + + auto connected = [pub, sub = this->sub]() -> bool { + return pub->get_subscription_count() && sub->get_publisher_count(); + }; + ASSERT_TRUE(wait_for(connected, 5s)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + if (sub->is_cft_enabled()) { + EXPECT_FALSE(receive); + } else { + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + + if (sub->is_cft_enabled()) { + EXPECT_NO_THROW( + sub->set_content_filter("")); + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(1)); + + test_msgs::msg::BasicTypes original_message; + original_message.int32_value = 4; + pub->publish(original_message); + + test_msgs::msg::BasicTypes output_message; + bool receive = wait_for_message(output_message, sub, context, 5s); + EXPECT_TRUE(receive); + EXPECT_EQ(original_message, output_message); + } + } +} From 307b8d4b3d15e79cc5ec7e341007c1bb79115420 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Wed, 30 Mar 2022 13:03:28 +0800 Subject: [PATCH 17/24] address review Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_options.hpp | 4 ++-- rclcpp/include/rclcpp/utilities.hpp | 4 ++-- rclcpp/src/rclcpp/utilities.cpp | 8 ++++---- rclcpp/test/rclcpp/test_subscription_content_filter.cpp | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index be23f20fa3..cea75cf849 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -86,11 +86,11 @@ struct SubscriptionOptionsBase /// Options to configure content filtered topic in the subscription. struct ContentFilterOptions { - /// Filter expression is similar to the WHERE part of an SQL clause + /// Filter expression is similar to the WHERE part of an SQL clause. std::string filter_expression; /** * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) - * in the filter_expression + * in the filter_expression. The maximum expression_parameters size is 100. */ std::vector expression_parameters; }; diff --git a/rclcpp/include/rclcpp/utilities.hpp b/rclcpp/include/rclcpp/utilities.hpp index 20ded67ad9..5274cba33a 100644 --- a/rclcpp/include/rclcpp/utilities.hpp +++ b/rclcpp/include/rclcpp/utilities.hpp @@ -323,12 +323,12 @@ get_c_string(const std::string & string_in); /// Return the std::vector of C string from the given std::vector. /** - * \param[in] string_in is a std::string + * \param[in] strings_in is a std::vector of std::string * \return the std::vector of C string from the std::vector */ RCLCPP_PUBLIC std::vector -get_c_vector_string(const std::vector & strings); +get_c_vector_string(const std::vector & strings_in); } // namespace rclcpp diff --git a/rclcpp/src/rclcpp/utilities.cpp b/rclcpp/src/rclcpp/utilities.cpp index 535292b76c..f300060010 100644 --- a/rclcpp/src/rclcpp/utilities.cpp +++ b/rclcpp/src/rclcpp/utilities.cpp @@ -215,13 +215,13 @@ get_c_string(const std::string & string_in) } std::vector -get_c_vector_string(const std::vector & strings) +get_c_vector_string(const std::vector & strings_in) { std::vector cstrings; - cstrings.reserve(strings.size()); + cstrings.reserve(strings_in.size()); - for (size_t i = 0; i < strings.size(); ++i) { - cstrings.push_back(strings[i].c_str()); + for (size_t i = 0; i < strings_in.size(); ++i) { + cstrings.push_back(strings_in[i].c_str()); } return cstrings; diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 37956559da..6a12f4d62c 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.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 e37c626617f6ca6bf313918fc2ec2395ed0ad089 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 12:36:23 +0800 Subject: [PATCH 18/24] rcl api changed Signed-off-by: Chen Lihui --- rclcpp/src/rclcpp/subscription_base.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index c8cd3c2aa3..74ae651ea3 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -375,6 +375,7 @@ SubscriptionBase::set_content_filter( std::vector cstrings = get_c_vector_string(expression_parameters); rcl_ret_t ret = rcl_subscription_content_filter_options_init( + subscription_handle_.get(), get_c_string(filter_expression), cstrings.size(), cstrings.data(), @@ -385,7 +386,8 @@ SubscriptionBase::set_content_filter( } RCPPUTILS_SCOPE_EXIT( { - rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), @@ -422,7 +424,8 @@ SubscriptionBase::get_content_filter( RCPPUTILS_SCOPE_EXIT( { - rcl_ret_t ret = rcl_subscription_content_filter_options_fini(&options); + rcl_ret_t ret = rcl_subscription_content_filter_options_fini( + subscription_handle_.get(), &options); if (RCL_RET_OK != ret) { RCLCPP_ERROR( rclcpp::get_logger("rclcpp"), From e965be7d15e9fca3d9f3a2bcb0f7cee318919924 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 12:56:22 +0800 Subject: [PATCH 19/24] increase the filter propagation time Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/test_subscription_content_filter.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 6a12f4d62c..aa923503f6 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -206,7 +206,7 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter(filter_expression_init, expression_parameters_2)); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(5)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 3; @@ -252,7 +252,7 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter(filter_expression_init, expression_parameters_2)); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(5)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; @@ -299,7 +299,7 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter("")); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(1)); + std::this_thread::sleep_for(std::chrono::seconds(5)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; From 556215b0839d46aceda5fc711ff6aefee04a7e49 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 31 Mar 2022 13:49:38 +0800 Subject: [PATCH 20/24] add rclcpp::ContentFilterOptions and use it as the return type of get_content_filter Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 16 +++----- .../subscription_content_filter_options.hpp | 40 +++++++++++++++++++ .../include/rclcpp/subscription_options.hpp | 13 +----- rclcpp/src/rclcpp/subscription_base.cpp | 13 +++--- .../test_subscription_content_filter.cpp | 16 ++++---- 5 files changed, 61 insertions(+), 37 deletions(-) create mode 100644 rclcpp/include/rclcpp/subscription_content_filter_options.hpp diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 3e41a19575..02d1db3e8d 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -39,6 +39,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/serialized_message.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/type_support_decl.hpp" #include "rclcpp/visibility_control.hpp" @@ -503,15 +504,14 @@ class SubscriptionBase : public std::enable_shared_from_this /// Set the filter expression and expression parameters for the subscription. /** * \param[in] filter_expression An filter expression to set. - * \sa SubscriptionOptionsBase::ContentFilterOptions::filter_expression + * \sa ContentFilterOptions::filter_expression * Use empty string ("") can reset (or clear) the content filter setting of a subscription. * \param[in] expression_parameters Array of expression parameters to set. - * \sa SubscriptionOptionsBase::ContentFilterOptions::expression_parameters + * \sa ContentFilterOptions::expression_parameters * \throws RCLBadAlloc if memory cannot be allocated * \throws RCLError if an unexpect error occurs */ RCLCPP_PUBLIC - virtual void set_content_filter( const std::string & filter_expression, @@ -519,17 +519,13 @@ class SubscriptionBase : public std::enable_shared_from_this /// Get the filter expression and expression parameters for the subscription. /** - * \param[out] filter_expression An filter expression to get. - * \param[out] expression_parameters Array of expression parameters to get. + * \return rclcpp::ContentFilterOptions The content filter options to get. * \throws RCLBadAlloc if memory cannot be allocated * \throws RCLError if an unexpect error occurs */ RCLCPP_PUBLIC - virtual - void - get_content_filter( - std::string & filter_expression, - std::vector & expression_parameters) const; + rclcpp::ContentFilterOptions + get_content_filter() const; #endif // CONTENT_FILTER_ENABLE protected: diff --git a/rclcpp/include/rclcpp/subscription_content_filter_options.hpp b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp new file mode 100644 index 0000000000..1f6c6c5321 --- /dev/null +++ b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp @@ -0,0 +1,40 @@ +// 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. +// 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. + +#ifndef RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ +#define RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ + +#include +#include + +namespace rclcpp +{ + +#ifdef CONTENT_FILTER_ENABLE +/// Options to configure content filtered topic in the subscription. +struct ContentFilterOptions +{ + /// Filter expression is similar to the WHERE part of an SQL clause. + std::string filter_expression; + /** + * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) + * in the filter_expression. The maximum expression_parameters size is 100. + */ + std::vector expression_parameters; +}; +#endif // CONTENT_FILTER_ENABLE + +} // namespace rclcpp + +#endif // RCLCPP__SUBSCRIPTION_CONTENT_FILTER_OPTIONS_HPP_ diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index cea75cf849..3d0d43faa0 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -28,6 +28,7 @@ #include "rclcpp/qos.hpp" #include "rclcpp/qos_event.hpp" #include "rclcpp/qos_overriding_options.hpp" +#include "rclcpp/subscription_content_filter_options.hpp" #include "rclcpp/topic_statistics_state.hpp" #include "rclcpp/visibility_control.hpp" @@ -83,18 +84,6 @@ struct SubscriptionOptionsBase QosOverridingOptions qos_overriding_options; #ifdef CONTENT_FILTER_ENABLE - /// Options to configure content filtered topic in the subscription. - struct ContentFilterOptions - { - /// Filter expression is similar to the WHERE part of an SQL clause. - std::string filter_expression; - /** - * Expression parameters is the tokens placeholder ‘parameters’ (i.e., "%n" tokens begin from 0) - * in the filter_expression. The maximum expression_parameters size is 100. - */ - std::vector expression_parameters; - }; - ContentFilterOptions content_filter_options; #endif // CONTENT_FILTER_ENABLE }; diff --git a/rclcpp/src/rclcpp/subscription_base.cpp b/rclcpp/src/rclcpp/subscription_base.cpp index 74ae651ea3..dfe3c30ead 100644 --- a/rclcpp/src/rclcpp/subscription_base.cpp +++ b/rclcpp/src/rclcpp/subscription_base.cpp @@ -406,11 +406,10 @@ SubscriptionBase::set_content_filter( } } -void -SubscriptionBase::get_content_filter( - std::string & filter_expression, - std::vector & expression_parameters) const +rclcpp::ContentFilterOptions +SubscriptionBase::get_content_filter() const { + rclcpp::ContentFilterOptions ret_options; rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); @@ -437,11 +436,13 @@ SubscriptionBase::get_content_filter( rmw_subscription_content_filter_options_t & content_filter_options = options.rmw_subscription_content_filter_options; - filter_expression = content_filter_options.filter_expression; + ret_options.filter_expression = content_filter_options.filter_expression; for (size_t i = 0; i < content_filter_options.expression_parameters.size; ++i) { - expression_parameters.push_back( + ret_options.expression_parameters.push_back( content_filter_options.expression_parameters.data[i]); } + + return ret_options; } #endif // CONTENT_FILTER_ENABLE diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index aa923503f6..81179aaf2f 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -131,10 +131,9 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content auto mock = mocking_utils::patch_and_return( "lib:rclcpp", rcl_subscription_get_content_filter, RCL_RET_ERROR); - std::string filter_expression; - std::vector expression_parameters; + rclcpp::ContentFilterOptions options; EXPECT_THROW( - sub->get_content_filter(filter_expression, expression_parameters), + options = sub->get_content_filter(), rclcpp::exceptions::RCLError); } @@ -150,18 +149,17 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), set_content } TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), get_content_filter) { - std::string filter_expression; - std::vector expression_parameters; + rclcpp::ContentFilterOptions options; if (sub->is_cft_enabled()) { EXPECT_NO_THROW( - sub->get_content_filter(filter_expression, expression_parameters)); + options = sub->get_content_filter()); - EXPECT_EQ(filter_expression, filter_expression_init); - EXPECT_EQ(expression_parameters, expression_parameters_1); + EXPECT_EQ(options.filter_expression, filter_expression_init); + EXPECT_EQ(options.expression_parameters, expression_parameters_1); } else { EXPECT_THROW( - sub->get_content_filter(filter_expression, expression_parameters), + options = sub->get_content_filter(), rclcpp::exceptions::RCLError); } } From 7b362b5ebbc8bb54d9c59ba381976cd1c2f33e6f Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 1 Apr 2022 10:23:37 +0800 Subject: [PATCH 21/24] increase the maximun time to wait message event and content filter propagation Signed-off-by: Chen Lihui --- .../test_subscription_content_filter.cpp | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp index 81179aaf2f..942aa1274e 100644 --- a/rclcpp/test/rclcpp/test_subscription_content_filter.cpp +++ b/rclcpp/test/rclcpp/test_subscription_content_filter.cpp @@ -189,14 +189,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil auto connected = [pub, sub = this->sub]() -> bool { return pub->get_subscription_count() && sub->get_publisher_count(); }; - ASSERT_TRUE(wait_for(connected, 5s)); + ASSERT_TRUE(wait_for(connected, 10s)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 3; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); EXPECT_TRUE(receive); EXPECT_EQ(original_message, output_message); @@ -204,14 +204,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter(filter_expression_init, expression_parameters_2)); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(5)); + std::this_thread::sleep_for(std::chrono::seconds(10)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 3; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); EXPECT_FALSE(receive); } } @@ -231,14 +231,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil auto connected = [pub, sub = this->sub]() -> bool { return pub->get_subscription_count() && sub->get_publisher_count(); }; - ASSERT_TRUE(wait_for(connected, 5s)); + ASSERT_TRUE(wait_for(connected, 10s)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); if (sub->is_cft_enabled()) { EXPECT_FALSE(receive); } else { @@ -250,14 +250,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter(filter_expression_init, expression_parameters_2)); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(5)); + std::this_thread::sleep_for(std::chrono::seconds(10)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); EXPECT_TRUE(receive); EXPECT_EQ(original_message, output_message); } @@ -278,14 +278,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil auto connected = [pub, sub = this->sub]() -> bool { return pub->get_subscription_count() && sub->get_publisher_count(); }; - ASSERT_TRUE(wait_for(connected, 5s)); + ASSERT_TRUE(wait_for(connected, 10s)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); if (sub->is_cft_enabled()) { EXPECT_FALSE(receive); } else { @@ -297,14 +297,14 @@ TEST_F(CLASSNAME(TestContentFilterSubscription, RMW_IMPLEMENTATION), content_fil EXPECT_NO_THROW( sub->set_content_filter("")); // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(5)); + std::this_thread::sleep_for(std::chrono::seconds(10)); test_msgs::msg::BasicTypes original_message; original_message.int32_value = 4; pub->publish(original_message); test_msgs::msg::BasicTypes output_message; - bool receive = wait_for_message(output_message, sub, context, 5s); + bool receive = wait_for_message(output_message, sub, context, 10s); EXPECT_TRUE(receive); EXPECT_EQ(original_message, output_message); } From 1b232aeedc96d065315b95bb902ca86ed4b8357e Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 1 Apr 2022 15:24:21 +0800 Subject: [PATCH 22/24] cft member should be defined in the structure. to check the macro for interfaces is enough. Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_content_filter_options.hpp | 2 -- rclcpp/include/rclcpp/subscription_options.hpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_content_filter_options.hpp b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp index 1f6c6c5321..8de034ddf5 100644 --- a/rclcpp/include/rclcpp/subscription_content_filter_options.hpp +++ b/rclcpp/include/rclcpp/subscription_content_filter_options.hpp @@ -21,7 +21,6 @@ namespace rclcpp { -#ifdef CONTENT_FILTER_ENABLE /// Options to configure content filtered topic in the subscription. struct ContentFilterOptions { @@ -33,7 +32,6 @@ struct ContentFilterOptions */ std::vector expression_parameters; }; -#endif // CONTENT_FILTER_ENABLE } // namespace rclcpp diff --git a/rclcpp/include/rclcpp/subscription_options.hpp b/rclcpp/include/rclcpp/subscription_options.hpp index 3d0d43faa0..da81b8ffb0 100644 --- a/rclcpp/include/rclcpp/subscription_options.hpp +++ b/rclcpp/include/rclcpp/subscription_options.hpp @@ -83,9 +83,7 @@ struct SubscriptionOptionsBase QosOverridingOptions qos_overriding_options; -#ifdef CONTENT_FILTER_ENABLE ContentFilterOptions content_filter_options; -#endif // CONTENT_FILTER_ENABLE }; /// Structure containing optional configuration for Subscriptions. From 50068a7c1f3d587af808ab9e9cf7fca504e84d18 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 1 Apr 2022 18:55:50 +0800 Subject: [PATCH 23/24] set test timeout to 120 Signed-off-by: Chen Lihui --- rclcpp/test/rclcpp/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rclcpp/test/rclcpp/CMakeLists.txt b/rclcpp/test/rclcpp/CMakeLists.txt index 7da5372667..2d7e9a3001 100644 --- a/rclcpp/test/rclcpp/CMakeLists.txt +++ b/rclcpp/test/rclcpp/CMakeLists.txt @@ -740,6 +740,7 @@ if(DEFINE_CONTENT_FILTER) ament_add_gmock(test_subscription_content_filter${target_suffix} test_subscription_content_filter.cpp ENV ${rmw_implementation_env_var} + TIMEOUT 120 ) if(TARGET test_subscription_content_filter${target_suffix}) target_link_libraries(test_subscription_content_filter${target_suffix} ${PROJECT_NAME} mimick) From 98f2f669878793c595b64a365b4616c452884d52 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Sat, 2 Apr 2022 09:56:38 +0800 Subject: [PATCH 24/24] update doc Signed-off-by: Chen Lihui --- rclcpp/include/rclcpp/subscription_base.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rclcpp/include/rclcpp/subscription_base.hpp b/rclcpp/include/rclcpp/subscription_base.hpp index 02d1db3e8d..0ad098b027 100644 --- a/rclcpp/include/rclcpp/subscription_base.hpp +++ b/rclcpp/include/rclcpp/subscription_base.hpp @@ -503,9 +503,9 @@ class SubscriptionBase : public std::enable_shared_from_this /// Set the filter expression and expression parameters for the subscription. /** - * \param[in] filter_expression An filter expression to set. + * \param[in] filter_expression A filter expression to set. * \sa ContentFilterOptions::filter_expression - * Use empty string ("") can reset (or clear) the content filter setting of a subscription. + * An empty string ("") will clear the content filter setting of the subscription. * \param[in] expression_parameters Array of expression parameters to set. * \sa ContentFilterOptions::expression_parameters * \throws RCLBadAlloc if memory cannot be allocated