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); - } - } -}