From 41a4cdc58dcf81ced434164de9666c254fce19e3 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 1 Aug 2022 12:34:36 +0800 Subject: [PATCH 1/7] call common content filter Signed-off-by: Chen Lihui --- rcl/CMakeLists.txt | 3 + rcl/package.xml | 1 + rcl/src/rcl/subscription.c | 132 +++++++++++++++++++++++++++-- rcl/src/rcl/subscription_impl.h | 3 + rcl/test/rcl/test_subscription.cpp | 101 ++++++++++++++++++++-- 5 files changed, 226 insertions(+), 14 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index 11462f119..814603ced 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rosidl_runtime_c REQUIRED) find_package(tracetools REQUIRED) +find_package(common_content_filter REQUIRED) include(cmake/rcl_set_symbol_visibility_hidden.cmake) include(cmake/get_default_rcl_logging_implementation.cmake) @@ -81,6 +82,7 @@ ament_target_dependencies(${PROJECT_NAME} ${RCL_LOGGING_IMPL} "rosidl_runtime_c" "tracetools" + "common_content_filter" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -121,6 +123,7 @@ ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(tracetools) +ament_export_dependencies(common_content_filter) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rcl/package.xml b/rcl/package.xml index 03df8701b..58ee03b2c 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -23,6 +23,7 @@ rmw_implementation rosidl_runtime_c tracetools + common_content_filter ament_cmake_gtest ament_lint_auto diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index fd5984ded..fb92c3267 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -21,6 +21,8 @@ extern "C" #include +#include "common_content_filter/api.h" + #include "rcl/error_handling.h" #include "rcl/node.h" #include "rcutils/logging_macros.h" @@ -42,6 +44,54 @@ rcl_get_zero_initialized_subscription() return null_subscription; } +static +bool +rcl_subscription_common_content_filter_set( + const rcl_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options) +{ + if (!subscription->impl->common_content_filter) { + subscription->impl->common_content_filter = + common_content_filter_create(subscription->impl->type_support); + if (!subscription->impl->common_content_filter) { + RCL_SET_ERROR_MSG("Failed to create common content filter"); + return false; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "common content filter is created for topic '%s'", + rcl_subscription_get_topic_name(subscription)); + } + + if (!common_content_filter_set( + subscription->impl->common_content_filter, + options)) + { + RCL_SET_ERROR_MSG("Failed to set common content filter"); + return false; + } + + return true; +} + +static +bool +rcl_subscription_common_content_filter_is_relevant( + const rcl_subscription_t * subscription, + void * data, + bool serialized) +{ + if (subscription->impl->common_content_filter && + common_content_filter_is_enabled(subscription->impl->common_content_filter)) + { + return common_content_filter_evaluate( + subscription->impl->common_content_filter, + data, + serialized); + } + + return true; +} + rcl_ret_t rcl_subscription_init( rcl_subscription_t * subscription, @@ -120,6 +170,22 @@ rcl_subscription_init( options->qos.avoid_ros_namespace_conventions; // options subscription->impl->options = *options; + subscription->impl->type_support = type_support; + + if (options->rmw_subscription_options.content_filter_options) { + // Content filter topic not supported (or not enabled as some failed cases) on DDS. + // TODO(iuhilnehc-ynos): enable common content filter with an environment variable + // (e.g. FORCE_COMMON_CONTENT_FILTER) regardless of whether cft is enabled on DDS. + if (!subscription->impl->rmw_handle->is_cft_enabled) { + if (!rcl_subscription_common_content_filter_set( + subscription, + options->rmw_subscription_options.content_filter_options)) + { + goto fail; + } + } + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); ret = RCL_RET_OK; TRACEPOINT( @@ -147,6 +213,10 @@ rcl_subscription_init( RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } + if (subscription->impl->common_content_filter) { + common_content_filter_destroy(subscription->impl->common_content_filter); + } + allocator->deallocate(subscription->impl, allocator->state); subscription->impl = NULL; } @@ -190,6 +260,10 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } + if (subscription->impl->common_content_filter) { + common_content_filter_destroy(subscription->impl->common_content_filter); + } + allocator.deallocate(subscription->impl, allocator.state); subscription->impl = NULL; } @@ -432,7 +506,9 @@ rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription) if (!rcl_subscription_is_valid(subscription)) { return false; } - return subscription->impl->rmw_handle->is_cft_enabled; + return subscription->impl->rmw_handle->is_cft_enabled || + (subscription->impl->common_content_filter && + common_content_filter_is_enabled(subscription->impl->common_content_filter)); } rcl_ret_t @@ -454,8 +530,13 @@ rcl_subscription_set_content_filter( &options->rmw_subscription_content_filter_options); if (ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return rcl_convert_rmw_ret_to_rcl_ret(ret); + rcl_reset_error(); + if (!rcl_subscription_common_content_filter_set( + subscription, + &options->rmw_subscription_content_filter_options)) + { + return RMW_RET_ERROR; + } } // copy options into subscription_options @@ -489,8 +570,19 @@ rcl_subscription_get_content_filter( subscription->impl->rmw_handle, allocator, &options->rmw_subscription_content_filter_options); - - return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + // If options can be get from DDS, it's unnecessary to get them from common content filter. + if (rmw_ret != RMW_RET_OK) { + rcl_reset_error(); + if (!common_content_filter_get( + subscription->impl->common_content_filter, + allocator, + &options->rmw_subscription_content_filter_options)) + { + RCL_SET_ERROR_MSG("Failed to get content filter"); + return RMW_RET_ERROR; + } + } + return RMW_RET_OK; } rcl_ret_t @@ -525,6 +617,16 @@ rcl_take( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + + // filter ros message with common content filter + if (!rcl_subscription_common_content_filter_is_relevant( + subscription, + ros_message, + false)) + { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; } @@ -604,6 +706,16 @@ rcl_take_serialized_message( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + + // filter ros message with common content filter + if (!rcl_subscription_common_content_filter_is_relevant( + subscription, + serialized_message, + true)) + { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; } @@ -640,6 +752,16 @@ rcl_take_loaned_message( if (!taken) { return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } + + // filter ros message with common content filter + if (!rcl_subscription_common_content_filter_is_relevant( + subscription, + *loaned_message, + false)) + { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; } diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 0fe962ab4..50373f5a8 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -18,12 +18,15 @@ #include "rmw/rmw.h" #include "rcl/subscription.h" +#include "rosidl_runtime_c/message_type_support_struct.h" struct rcl_subscription_impl_s { rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; + void * common_content_filter; + const rosidl_message_type_support_t * type_support; }; #endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 46bd15213..c451db815 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -18,6 +18,8 @@ #include #include +#include "common_content_filter/api.h" + #include "rcl/subscription.h" #include "rcl/rcl.h" #include "rmw/rmw.h" @@ -937,7 +939,19 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + // this event can be triggered if using the common content filter + bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); + if (ready) { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + // data filtered inside rcl_take, expect the result with RCL_RET_SUBSCRIPTION_TAKE_FAILED + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + } } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); @@ -1024,7 +1038,17 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); + if (ready) { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + } } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); @@ -1228,9 +1252,8 @@ TEST_F( const char * filter_expression2 = "int32_value = %0"; const char * expression_parameters2[] = {"4"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); - bool is_cft_support = - (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 || - std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps_cpp") == 0); + // common content filter will be the fallback if content filter is unsupported on DDS + bool is_cft_support = true; { rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); @@ -1271,7 +1294,18 @@ TEST_F( } if (is_cft_support) { - ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + // It will be triggered if using the common content filter. + bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); + if (ready) { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; + } } else { ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); @@ -1597,7 +1631,7 @@ TEST_F( RCL_RET_OK, rcl_subscription_content_filter_options_init( &subscription, - "data = '0'", + "int32_value = 0", 0, nullptr, &options @@ -1615,7 +1649,7 @@ TEST_F( auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RMW_RET_OK, rcl_subscription_set_content_filter( &subscription, &options)); rcl_reset_error(); @@ -1624,6 +1658,8 @@ TEST_F( { auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); + auto mock2 = mocking_utils::patch_and_return( + "lib:rcl", common_content_filter_set, false); EXPECT_EQ( RMW_RET_ERROR, rcl_subscription_set_content_filter( @@ -1655,20 +1691,67 @@ TEST_F( rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini(&subscription, &options) + ); + }); { auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED); EXPECT_EQ( - RMW_RET_UNSUPPORTED, + RMW_RET_ERROR, rcl_subscription_get_content_filter( &subscription, &options)); rcl_reset_error(); } + { + // set content filter options + rcl_subscription_content_filter_options_t options2 = + rcl_get_zero_initialized_subscription_content_filter_options(); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + &subscription, + "int32_value = 0", + 0, + nullptr, + &options2 + ) + ); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini(&subscription, &options2) + ); + }); + + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_OK, + rcl_subscription_set_content_filter( + &subscription, &options2)); + + auto mock2 = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED); + + EXPECT_EQ( + RMW_RET_OK, + rcl_subscription_get_content_filter( + &subscription, &options)); + } + { auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); + auto mock2 = mocking_utils::patch_and_return( + "lib:rcl", common_content_filter_get, false); EXPECT_EQ( RMW_RET_ERROR, rcl_subscription_get_content_filter( From ea4a6ac779f080ed59297f91622d4147a4779d99 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Fri, 24 Feb 2023 10:56:37 +0800 Subject: [PATCH 2/7] address review Co-authored-by: Tomoya.Fujita Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 6 ++- rcl/test/rcl/test_subscription.cpp | 81 ++++++------------------------ 2 files changed, 18 insertions(+), 69 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index fb92c3267..e1892e9c1 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -173,7 +173,8 @@ rcl_subscription_init( subscription->impl->type_support = type_support; if (options->rmw_subscription_options.content_filter_options) { - // Content filter topic not supported (or not enabled as some failed cases) on DDS. + // Content filter topic not supported (or not enabled as some failed cases) on rmw + // implementation. // TODO(iuhilnehc-ynos): enable common content filter with an environment variable // (e.g. FORCE_COMMON_CONTENT_FILTER) regardless of whether cft is enabled on DDS. if (!subscription->impl->rmw_handle->is_cft_enabled) { @@ -570,7 +571,8 @@ rcl_subscription_get_content_filter( subscription->impl->rmw_handle, allocator, &options->rmw_subscription_content_filter_options); - // If options can be get from DDS, it's unnecessary to get them from common content filter. + // If options can be get from rmw implementation, it's unnecessary to get them from common + // content filter. if (rmw_ret != RMW_RET_OK) { rcl_reset_error(); if (!common_content_filter_get( diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 6fe6143c2..9994a9d3a 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -926,7 +926,9 @@ TEST_F( rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - bool is_cft_support = rcl_subscription_is_cft_enabled(&subscription); + + // CFT must be enabled because rcl CFT will be effective even if rmw CFT is not supported. + ASSERT_TRUE(rcl_subscription_is_cft_enabled(&subscription)); ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); // publish with a non-filtered data @@ -940,7 +942,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_cft_support) { + { // this event can be triggered if using the common content filter bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); if (ready) { @@ -954,20 +956,6 @@ TEST_F( ret = rcl_take(&subscription, &msg, nullptr, nullptr); ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; } - } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); - - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__Strings__fini(&msg); - }); - ret = rcl_take(&subscription, &msg, nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ( - std::string(test_string), - std::string(msg.string_value.data, msg.string_value.size)); } constexpr char test_filtered_string[] = "FilteredData"; @@ -1014,14 +1002,9 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (is_cft_support) { - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - // waiting to allow for filter propagation - std::this_thread::sleep_for(std::chrono::seconds(10)); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - rcl_reset_error(); - } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); EXPECT_EQ( RCL_RET_OK, @@ -1040,7 +1023,7 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_cft_support) { + { bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); if (ready) { test_msgs__msg__Strings msg; @@ -1052,20 +1035,6 @@ TEST_F( ret = rcl_take(&subscription, &msg, nullptr, nullptr); ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; } - } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); - - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__Strings__fini(&msg); - }); - ret = rcl_take(&subscription, &msg, nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ( - std::string(test_filtered_string), - std::string(msg.string_value.data, msg.string_value.size)); } constexpr char test_filtered_other_string[] = "FilteredOtherData"; @@ -1101,7 +1070,7 @@ TEST_F( ret = rcl_subscription_get_content_filter( &subscription, &content_filter_options); - if (is_cft_support) { + { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rmw_subscription_content_filter_options_t * options = @@ -1119,9 +1088,6 @@ TEST_F( &subscription, &content_filter_options) ); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - rcl_reset_error(); } } @@ -1140,15 +1106,12 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (is_cft_support) { + { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation std::this_thread::sleep_for(std::chrono::seconds(10)); ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); - } else { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - rcl_reset_error(); } EXPECT_EQ( @@ -1258,8 +1221,7 @@ TEST_F( const char * filter_expression2 = "int32_value = %0"; const char * expression_parameters2[] = {"4"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); - // common content filter will be the fallback if content filter is unsupported on DDS - bool is_cft_support = true; + // rcl CFT will be the fallback if rmw CFT is unsupported on DDS { rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options(); @@ -1274,10 +1236,7 @@ TEST_F( ret = rcl_subscription_set_content_filter( &subscription, &options); - if (!is_cft_support) { - ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); - rcl_reset_error(); - } else { + { ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; // waiting to allow for filter propagation std::this_thread::sleep_for(std::chrono::seconds(10)); @@ -1300,8 +1259,8 @@ TEST_F( ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - if (is_cft_support) { - // It will be triggered if using the common content filter. + { + // It will be triggered if using the rcl CFT. bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); if (ready) { test_msgs__msg__BasicTypes msg; @@ -1313,18 +1272,6 @@ TEST_F( ret = rcl_take(&subscription, &msg, nullptr, nullptr); ASSERT_EQ(RCL_RET_SUBSCRIPTION_TAKE_FAILED, ret) << rcl_get_error_string().str; } - } else { - ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); - - test_msgs__msg__BasicTypes msg; - test_msgs__msg__BasicTypes__init(&msg); - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - test_msgs__msg__BasicTypes__fini(&msg); - }); - ret = rcl_take(&subscription, &msg, nullptr, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(test_value == msg.int32_value); } // publish filtered data From 92d3ca4cbc6c7d8f47676be51df0478aa2b42928 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 27 Feb 2023 10:28:24 +0800 Subject: [PATCH 3/7] use `rcl_content_filter_fallback` instead of `common_content_filter` Signed-off-by: Chen Lihui --- rcl/CMakeLists.txt | 6 ++-- rcl/package.xml | 2 +- rcl/src/rcl/subscription.c | 52 +++++++++++++++--------------- rcl/src/rcl/subscription_impl.h | 2 +- rcl/test/rcl/test_subscription.cpp | 6 ++-- 5 files changed, 34 insertions(+), 34 deletions(-) diff --git a/rcl/CMakeLists.txt b/rcl/CMakeLists.txt index ff183ded0..fd021db84 100644 --- a/rcl/CMakeLists.txt +++ b/rcl/CMakeLists.txt @@ -4,6 +4,7 @@ project(rcl) find_package(ament_cmake_ros REQUIRED) +find_package(rcl_content_filter_fallback REQUIRED) find_package(rcl_interfaces REQUIRED) find_package(rcl_logging_interface REQUIRED) find_package(rcl_yaml_param_parser REQUIRED) @@ -12,7 +13,6 @@ find_package(rmw REQUIRED) find_package(rmw_implementation REQUIRED) find_package(rosidl_runtime_c REQUIRED) find_package(tracetools REQUIRED) -find_package(common_content_filter REQUIRED) include(cmake/rcl_set_symbol_visibility_hidden.cmake) include(cmake/get_default_rcl_logging_implementation.cmake) @@ -74,6 +74,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$") # specific order: dependents before dependencies ament_target_dependencies(${PROJECT_NAME} + "rcl_content_filter_fallback" "rcl_interfaces" "rcl_logging_interface" "rcl_yaml_param_parser" @@ -83,7 +84,6 @@ ament_target_dependencies(${PROJECT_NAME} ${RCL_LOGGING_IMPL} "rosidl_runtime_c" "tracetools" - "common_content_filter" ) # Causes the visibility macros to use dllexport rather than dllimport, @@ -115,6 +115,7 @@ ament_export_targets(${PROJECT_NAME}) # specific order: dependents before dependencies ament_export_dependencies(ament_cmake) +ament_export_dependencies(rcl_content_filter_fallback) ament_export_dependencies(rcl_interfaces) ament_export_dependencies(rcl_logging_interface) ament_export_dependencies(rcl_yaml_param_parser) @@ -124,7 +125,6 @@ ament_export_dependencies(rcutils) ament_export_dependencies(${RCL_LOGGING_IMPL}) ament_export_dependencies(rosidl_runtime_c) ament_export_dependencies(tracetools) -ament_export_dependencies(common_content_filter) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rcl/package.xml b/rcl/package.xml index 5f1766b3d..644a7ef02 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -19,6 +19,7 @@ rmw + rcl_content_filter_fallback rcl_interfaces rcl_logging_interface rcl_logging_spdlog @@ -27,7 +28,6 @@ rmw_implementation rosidl_runtime_c tracetools - common_content_filter ament_cmake_gtest ament_lint_auto diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index e1892e9c1..189700d39 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -21,7 +21,7 @@ extern "C" #include -#include "common_content_filter/api.h" +#include "rcl_content_filter_fallback/api.h" #include "rcl/error_handling.h" #include "rcl/node.h" @@ -46,14 +46,14 @@ rcl_get_zero_initialized_subscription() static bool -rcl_subscription_common_content_filter_set( +rcl_subscription_rcl_content_filter_fallback_set( const rcl_subscription_t * subscription, const rmw_subscription_content_filter_options_t * options) { - if (!subscription->impl->common_content_filter) { - subscription->impl->common_content_filter = - common_content_filter_create(subscription->impl->type_support); - if (!subscription->impl->common_content_filter) { + if (!subscription->impl->rcl_content_filter_fallback) { + subscription->impl->rcl_content_filter_fallback = + rcl_content_filter_fallback_create(subscription->impl->type_support); + if (!subscription->impl->rcl_content_filter_fallback) { RCL_SET_ERROR_MSG("Failed to create common content filter"); return false; } @@ -62,8 +62,8 @@ rcl_subscription_common_content_filter_set( rcl_subscription_get_topic_name(subscription)); } - if (!common_content_filter_set( - subscription->impl->common_content_filter, + if (!rcl_content_filter_fallback_set( + subscription->impl->rcl_content_filter_fallback, options)) { RCL_SET_ERROR_MSG("Failed to set common content filter"); @@ -75,16 +75,16 @@ rcl_subscription_common_content_filter_set( static bool -rcl_subscription_common_content_filter_is_relevant( +rcl_subscription_rcl_content_filter_fallback_is_relevant( const rcl_subscription_t * subscription, void * data, bool serialized) { - if (subscription->impl->common_content_filter && - common_content_filter_is_enabled(subscription->impl->common_content_filter)) + if (subscription->impl->rcl_content_filter_fallback && + rcl_content_filter_fallback_is_enabled(subscription->impl->rcl_content_filter_fallback)) { - return common_content_filter_evaluate( - subscription->impl->common_content_filter, + return rcl_content_filter_fallback_evaluate( + subscription->impl->rcl_content_filter_fallback, data, serialized); } @@ -178,7 +178,7 @@ rcl_subscription_init( // TODO(iuhilnehc-ynos): enable common content filter with an environment variable // (e.g. FORCE_COMMON_CONTENT_FILTER) regardless of whether cft is enabled on DDS. if (!subscription->impl->rmw_handle->is_cft_enabled) { - if (!rcl_subscription_common_content_filter_set( + if (!rcl_subscription_rcl_content_filter_fallback_set( subscription, options->rmw_subscription_options.content_filter_options)) { @@ -214,8 +214,8 @@ rcl_subscription_init( RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); } - if (subscription->impl->common_content_filter) { - common_content_filter_destroy(subscription->impl->common_content_filter); + if (subscription->impl->rcl_content_filter_fallback) { + rcl_content_filter_fallback_destroy(subscription->impl->rcl_content_filter_fallback); } allocator->deallocate(subscription->impl, allocator->state); @@ -261,8 +261,8 @@ rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) result = RCL_RET_ERROR; } - if (subscription->impl->common_content_filter) { - common_content_filter_destroy(subscription->impl->common_content_filter); + if (subscription->impl->rcl_content_filter_fallback) { + rcl_content_filter_fallback_destroy(subscription->impl->rcl_content_filter_fallback); } allocator.deallocate(subscription->impl, allocator.state); @@ -508,8 +508,8 @@ rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription) return false; } return subscription->impl->rmw_handle->is_cft_enabled || - (subscription->impl->common_content_filter && - common_content_filter_is_enabled(subscription->impl->common_content_filter)); + (subscription->impl->rcl_content_filter_fallback && + rcl_content_filter_fallback_is_enabled(subscription->impl->rcl_content_filter_fallback)); } rcl_ret_t @@ -532,7 +532,7 @@ rcl_subscription_set_content_filter( if (ret != RMW_RET_OK) { rcl_reset_error(); - if (!rcl_subscription_common_content_filter_set( + if (!rcl_subscription_rcl_content_filter_fallback_set( subscription, &options->rmw_subscription_content_filter_options)) { @@ -575,8 +575,8 @@ rcl_subscription_get_content_filter( // content filter. if (rmw_ret != RMW_RET_OK) { rcl_reset_error(); - if (!common_content_filter_get( - subscription->impl->common_content_filter, + if (!rcl_content_filter_fallback_get( + subscription->impl->rcl_content_filter_fallback, allocator, &options->rmw_subscription_content_filter_options)) { @@ -621,7 +621,7 @@ rcl_take( } // filter ros message with common content filter - if (!rcl_subscription_common_content_filter_is_relevant( + if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, ros_message, false)) @@ -710,7 +710,7 @@ rcl_take_serialized_message( } // filter ros message with common content filter - if (!rcl_subscription_common_content_filter_is_relevant( + if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, serialized_message, true)) @@ -756,7 +756,7 @@ rcl_take_loaned_message( } // filter ros message with common content filter - if (!rcl_subscription_common_content_filter_is_relevant( + if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, *loaned_message, false)) diff --git a/rcl/src/rcl/subscription_impl.h b/rcl/src/rcl/subscription_impl.h index 50373f5a8..62212411a 100644 --- a/rcl/src/rcl/subscription_impl.h +++ b/rcl/src/rcl/subscription_impl.h @@ -25,7 +25,7 @@ struct rcl_subscription_impl_s rcl_subscription_options_t options; rmw_qos_profile_t actual_qos; rmw_subscription_t * rmw_handle; - void * common_content_filter; + void * rcl_content_filter_fallback; const rosidl_message_type_support_t * type_support; }; diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 9994a9d3a..e7edacd0e 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -18,7 +18,7 @@ #include #include -#include "common_content_filter/api.h" +#include "rcl_content_filter_fallback/api.h" #include "rcl/subscription.h" #include "rcl/rcl.h" @@ -1614,7 +1614,7 @@ TEST_F( auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); auto mock2 = mocking_utils::patch_and_return( - "lib:rcl", common_content_filter_set, false); + "lib:rcl", rcl_content_filter_fallback_set, false); EXPECT_EQ( RMW_RET_ERROR, rcl_subscription_set_content_filter( @@ -1706,7 +1706,7 @@ TEST_F( auto mock = mocking_utils::patch_and_return( "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); auto mock2 = mocking_utils::patch_and_return( - "lib:rcl", common_content_filter_get, false); + "lib:rcl", rcl_content_filter_fallback_get, false); EXPECT_EQ( RMW_RET_ERROR, rcl_subscription_get_content_filter( From db624a3403abe814350a672240a01fc3b2d5203a Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 27 Feb 2023 10:47:26 +0800 Subject: [PATCH 4/7] use the renamed header file Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 189700d39..55606b2ad 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -21,7 +21,7 @@ extern "C" #include -#include "rcl_content_filter_fallback/api.h" +#include "rcl_content_filter_fallback/rcl_content_filter_fallback.h" #include "rcl/error_handling.h" #include "rcl/node.h" From 8c85795b2c718e2deb7e9fcdff39e4f0685ce8ca Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 27 Feb 2023 11:02:56 +0800 Subject: [PATCH 5/7] use rcl content filter fallback instead of common content filter Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 20 ++++++++++---------- rcl/test/rcl/test_subscription.cpp | 2 +- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 55606b2ad..bc0eeff83 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -54,11 +54,11 @@ rcl_subscription_rcl_content_filter_fallback_set( subscription->impl->rcl_content_filter_fallback = rcl_content_filter_fallback_create(subscription->impl->type_support); if (!subscription->impl->rcl_content_filter_fallback) { - RCL_SET_ERROR_MSG("Failed to create common content filter"); + RCL_SET_ERROR_MSG("Failed to create rcl content filter fallback"); return false; } RCUTILS_LOG_DEBUG_NAMED( - ROS_PACKAGE_NAME, "common content filter is created for topic '%s'", + ROS_PACKAGE_NAME, "rcl content filter fallback is created for topic '%s'", rcl_subscription_get_topic_name(subscription)); } @@ -66,7 +66,7 @@ rcl_subscription_rcl_content_filter_fallback_set( subscription->impl->rcl_content_filter_fallback, options)) { - RCL_SET_ERROR_MSG("Failed to set common content filter"); + RCL_SET_ERROR_MSG("Failed to set options for rcl content filter fallback"); return false; } @@ -175,7 +175,7 @@ rcl_subscription_init( if (options->rmw_subscription_options.content_filter_options) { // Content filter topic not supported (or not enabled as some failed cases) on rmw // implementation. - // TODO(iuhilnehc-ynos): enable common content filter with an environment variable + // TODO(iuhilnehc-ynos): enable rcl content filter fallback with an environment variable // (e.g. FORCE_COMMON_CONTENT_FILTER) regardless of whether cft is enabled on DDS. if (!subscription->impl->rmw_handle->is_cft_enabled) { if (!rcl_subscription_rcl_content_filter_fallback_set( @@ -571,8 +571,8 @@ rcl_subscription_get_content_filter( subscription->impl->rmw_handle, allocator, &options->rmw_subscription_content_filter_options); - // If options can be get from rmw implementation, it's unnecessary to get them from common - // content filter. + // If options can be get from rmw implementation, it's unnecessary to get them from rcl + // content filter fallback. if (rmw_ret != RMW_RET_OK) { rcl_reset_error(); if (!rcl_content_filter_fallback_get( @@ -580,7 +580,7 @@ rcl_subscription_get_content_filter( allocator, &options->rmw_subscription_content_filter_options)) { - RCL_SET_ERROR_MSG("Failed to get content filter"); + RCL_SET_ERROR_MSG("Failed to get options from rcl content filter fallback"); return RMW_RET_ERROR; } } @@ -620,7 +620,7 @@ rcl_take( return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // filter ros message with common content filter + // filter ros message with rcl content filter fallback if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, ros_message, @@ -709,7 +709,7 @@ rcl_take_serialized_message( return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // filter ros message with common content filter + // filter ros message with rcl content filter fallback if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, serialized_message, @@ -755,7 +755,7 @@ rcl_take_loaned_message( return RCL_RET_SUBSCRIPTION_TAKE_FAILED; } - // filter ros message with common content filter + // filter ros message with rcl content filter fallback if (!rcl_subscription_rcl_content_filter_fallback_is_relevant( subscription, *loaned_message, diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index e7edacd0e..8df37c9ac 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -943,7 +943,7 @@ TEST_F( } { - // this event can be triggered if using the common content filter + // this event can be triggered if using the rcl content filter fallback bool ready = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000); if (ready) { test_msgs__msg__Strings msg; From 59055fda2994928baa07657d05dc3f0a29efc193 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Mon, 27 Feb 2023 13:34:01 +0800 Subject: [PATCH 6/7] update for the included header file name Signed-off-by: Chen Lihui --- rcl/test/rcl/test_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 8df37c9ac..2459b6269 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -18,7 +18,7 @@ #include #include -#include "rcl_content_filter_fallback/api.h" +#include "rcl_content_filter_fallback/rcl_content_filter_fallback.h" #include "rcl/subscription.h" #include "rcl/rcl.h" From 79cbfc6da6644ece5fc25a40a47173c9ff829edb Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Thu, 2 Mar 2023 17:14:20 +0800 Subject: [PATCH 7/7] update comments Signed-off-by: Chen Lihui --- rcl/src/rcl/subscription.c | 2 +- rcl/test/rcl/test_subscription.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/subscription.c b/rcl/src/rcl/subscription.c index 9a438c427..b3b869e68 100644 --- a/rcl/src/rcl/subscription.c +++ b/rcl/src/rcl/subscription.c @@ -176,7 +176,7 @@ rcl_subscription_init( // Content filter topic not supported (or not enabled as some failed cases) on rmw // implementation. // TODO(iuhilnehc-ynos): enable rcl content filter fallback with an environment variable - // (e.g. FORCE_COMMON_CONTENT_FILTER) regardless of whether cft is enabled on DDS. + // (e.g. FORCE_RCL_CONTENT_FILTER) regardless of whether cft is enabled on RMW implementation. if (!subscription->impl->rmw_handle->is_cft_enabled) { if (!rcl_subscription_rcl_content_filter_fallback_set( subscription, diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 4246bf443..386b6b3df 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -1250,7 +1250,7 @@ TEST_F( const char * filter_expression2 = "int32_value = %0"; const char * expression_parameters2[] = {"4"}; size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); - // rcl CFT will be the fallback if rmw CFT is unsupported on DDS + // rcl CFT will be the fallback if rmw CFT is unsupported on implementation { rcl_subscription_content_filter_options_t options = rcl_get_zero_initialized_subscription_content_filter_options();