From afe8ccf6ac1d840bfd124f8ca575c9b5a0107b81 Mon Sep 17 00:00:00 2001 From: Chen Lihui Date: Tue, 3 May 2022 00:34:56 +0800 Subject: [PATCH] add a demo of content filter listener (#557) * add a demo of content filter listener Signed-off-by: Chen Lihui * add warn message if content filter is not supported by DDS Signed-off-by: Chen Lihui * keep `like` to make the demo simple and comment that rmw_connextdds not supported currently Signed-off-by: Chen Lihui * remove the macro as it will be removed in the rclcpp Signed-off-by: Chen Lihui * use = instead of like Signed-off-by: Chen Lihui * address reviews Signed-off-by: Chen Lihui * not make this configurable from the cli Signed-off-by: Chen Lihui * remove comment and update log output Signed-off-by: Chen Lihui * use temperature instead of chatter Signed-off-by: Chen Lihui * rename Signed-off-by: Chen Lihui (cherry picked from commit a1bc6dcbd71c12dceb33ee69e76b455b30f2177f) --- demo_nodes_cpp/CMakeLists.txt | 9 +++ .../topics/content_filtering_publisher.cpp | 79 ++++++++++++++++++ .../topics/content_filtering_subscriber.cpp | 81 +++++++++++++++++++ .../test/content_filtering_publisher.txt | 26 ++++++ ...nt_filtering_subscriber-rmw_connextdds.txt | 12 +++ ..._filtering_subscriber-rmw_fastrtps_cpp.txt | 12 +++ .../test/content_filtering_subscriber.txt | 1 + .../test/test_executables_tutorial.py.in | 6 ++ 8 files changed, 226 insertions(+) create mode 100644 demo_nodes_cpp/src/topics/content_filtering_publisher.cpp create mode 100644 demo_nodes_cpp/src/topics/content_filtering_subscriber.cpp create mode 100644 demo_nodes_cpp/test/content_filtering_publisher.txt create mode 100644 demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt create mode 100644 demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt create mode 100644 demo_nodes_cpp/test/content_filtering_subscriber.txt diff --git a/demo_nodes_cpp/CMakeLists.txt b/demo_nodes_cpp/CMakeLists.txt index e4b572551..a1ee6b246 100644 --- a/demo_nodes_cpp/CMakeLists.txt +++ b/demo_nodes_cpp/CMakeLists.txt @@ -68,6 +68,8 @@ add_library(parameters_library SHARED src/parameters/parameter_events_async.cpp src/parameters/even_parameters_node.cpp) add_library(topics_library SHARED + src/topics/content_filtering_publisher.cpp + src/topics/content_filtering_subscriber.cpp src/topics/talker.cpp src/topics/talker_loaned_message.cpp src/topics/talker_serialized_message.cpp @@ -109,6 +111,12 @@ rclcpp_components_register_node(parameters_library PLUGIN "demo_nodes_cpp::EvenParameterNode" EXECUTABLE even_parameters_node) +rclcpp_components_register_node(topics_library + PLUGIN "demo_nodes_cpp::ContentFilteringPublisher" + EXECUTABLE content_filtering_publisher) +rclcpp_components_register_node(topics_library + PLUGIN "demo_nodes_cpp::ContentFilteringSubscriber" + EXECUTABLE content_filtering_subscriber) rclcpp_components_register_node(topics_library PLUGIN "demo_nodes_cpp::Talker" EXECUTABLE talker) @@ -147,6 +155,7 @@ if(BUILD_TESTING) # Add each test case. Multi-executable tests can be specified in # semicolon-separated strings, like exe1;exe2. set(tutorial_tests + "content_filtering_publisher:content_filtering_subscriber" list_parameters_async list_parameters parameter_events_async diff --git a/demo_nodes_cpp/src/topics/content_filtering_publisher.cpp b/demo_nodes_cpp/src/topics/content_filtering_publisher.cpp new file mode 100644 index 000000000..be1925822 --- /dev/null +++ b/demo_nodes_cpp/src/topics/content_filtering_publisher.cpp @@ -0,0 +1,79 @@ +// 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. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "std_msgs/msg/float32.hpp" + +#include "demo_nodes_cpp/visibility_control.h" + +using namespace std::chrono_literals; + +namespace demo_nodes_cpp +{ +// The simulated temperature data starts from -100.0 and ends at 150.0 with a step size of 10.0 +constexpr std::array TEMPERATURE_SETTING {-100.0f, 150.0f, 10.0f}; + +// Create a ContentFilteringPublisher class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class ContentFilteringPublisher : public rclcpp::Node +{ +public: + DEMO_NODES_CPP_PUBLIC + explicit ContentFilteringPublisher(const rclcpp::NodeOptions & options) + : Node("content_filtering_publisher", options) + { + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto publish_message = + [this]() -> void + { + msg_ = std::make_unique(); + msg_->data = temperature_; + temperature_ += TEMPERATURE_SETTING[2]; + if (temperature_ > TEMPERATURE_SETTING[1]) { + temperature_ = TEMPERATURE_SETTING[0]; + } + RCLCPP_INFO(this->get_logger(), "Publishing: '%f'", msg_->data); + // Put the message into a queue to be processed by the middleware. + // This call is non-blocking. + pub_->publish(std::move(msg_)); + }; + // Create a publisher with a custom Quality of Service profile. + // Uniform initialization is suggested so it can be trivially changed to + // rclcpp::KeepAll{} if the user wishes. + // (rclcpp::KeepLast(7) -> rclcpp::KeepAll() fails to compile) + rclcpp::QoS qos(rclcpp::KeepLast{7}); + pub_ = this->create_publisher("temperature", qos); + + // Use a timer to schedule periodic message publishing. + timer_ = this->create_wall_timer(1s, publish_message); + } + +private: + float temperature_ = TEMPERATURE_SETTING[0]; + std::unique_ptr msg_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace demo_nodes_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringPublisher) diff --git a/demo_nodes_cpp/src/topics/content_filtering_subscriber.cpp b/demo_nodes_cpp/src/topics/content_filtering_subscriber.cpp new file mode 100644 index 000000000..ff298611b --- /dev/null +++ b/demo_nodes_cpp/src/topics/content_filtering_subscriber.cpp @@ -0,0 +1,81 @@ +// 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. + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "rcpputils/join.hpp" + +#include "std_msgs/msg/float32.hpp" + +#include "demo_nodes_cpp/visibility_control.h" + +namespace demo_nodes_cpp +{ +// Emergency temperature data less than -30 or greater than 100 +constexpr std::array EMERGENCY_TEMPERATURE {-30.0f, 100.0f}; + +// Create a ContentFilteringSubscriber class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class ContentFilteringSubscriber : public rclcpp::Node +{ +public: + DEMO_NODES_CPP_PUBLIC + explicit ContentFilteringSubscriber(const rclcpp::NodeOptions & options) + : Node("content_filtering_subscriber", options) + { + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + // Create a callback function for when messages are received. + auto callback = + [this](const std_msgs::msg::Float32 & msg) -> void + { + if (msg.data < EMERGENCY_TEMPERATURE[0] || msg.data > EMERGENCY_TEMPERATURE[1]) { + RCLCPP_INFO( + this->get_logger(), + "I receive an emergency temperature data: [%f]", msg.data); + } else { + RCLCPP_INFO(this->get_logger(), "I receive a temperature data: [%f]", msg.data); + } + }; + + // Initialize a subscription with a content filter to receive emergency temperature data that + // are less than -30 or greater than 100. + rclcpp::SubscriptionOptions sub_options; + sub_options.content_filter_options.filter_expression = "data < %0 OR data > %1"; + sub_options.content_filter_options.expression_parameters = { + std::to_string(EMERGENCY_TEMPERATURE[0]), + std::to_string(EMERGENCY_TEMPERATURE[1]) + }; + + sub_ = create_subscription("temperature", 10, callback, sub_options); + + if (!sub_->is_cft_enabled()) { + RCLCPP_WARN( + this->get_logger(), "Content filter is not enabled since it's not supported"); + } else { + RCLCPP_INFO( + this->get_logger(), + "subscribed to topic \"%s\" with content filter options \"%s, {%s}\"", + sub_->get_topic_name(), + sub_options.content_filter_options.filter_expression.c_str(), + rcpputils::join(sub_options.content_filter_options.expression_parameters, ", ").c_str()); + } + } + +private: + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace demo_nodes_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringSubscriber) diff --git a/demo_nodes_cpp/test/content_filtering_publisher.txt b/demo_nodes_cpp/test/content_filtering_publisher.txt new file mode 100644 index 000000000..6c302a2a5 --- /dev/null +++ b/demo_nodes_cpp/test/content_filtering_publisher.txt @@ -0,0 +1,26 @@ +Publishing: '-100.000000' +Publishing: '-90.000000' +Publishing: '-80.000000' +Publishing: '-70.000000' +Publishing: '-60.000000' +Publishing: '-50.000000' +Publishing: '-40.000000' +Publishing: '-30.000000' +Publishing: '-20.000000' +Publishing: '-10.000000' +Publishing: '0.000000' +Publishing: '10.000000' +Publishing: '20.000000' +Publishing: '30.000000' +Publishing: '40.000000' +Publishing: '50.000000' +Publishing: '60.000000' +Publishing: '70.000000' +Publishing: '80.000000' +Publishing: '90.000000' +Publishing: '100.000000' +Publishing: '110.000000' +Publishing: '120.000000' +Publishing: '130.000000' +Publishing: '140.000000' +Publishing: '150.000000' diff --git a/demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt b/demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt new file mode 100644 index 000000000..ff822b2e4 --- /dev/null +++ b/demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt @@ -0,0 +1,12 @@ +I receive an emergency temperature data: [-100.000000] +I receive an emergency temperature data: [-90.000000] +I receive an emergency temperature data: [-80.000000] +I receive an emergency temperature data: [-70.000000] +I receive an emergency temperature data: [-60.000000] +I receive an emergency temperature data: [-50.000000] +I receive an emergency temperature data: [-40.000000] +I receive an emergency temperature data: [110.000000] +I receive an emergency temperature data: [120.000000] +I receive an emergency temperature data: [130.000000] +I receive an emergency temperature data: [140.000000] +I receive an emergency temperature data: [150.000000] diff --git a/demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt b/demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt new file mode 100644 index 000000000..ff822b2e4 --- /dev/null +++ b/demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt @@ -0,0 +1,12 @@ +I receive an emergency temperature data: [-100.000000] +I receive an emergency temperature data: [-90.000000] +I receive an emergency temperature data: [-80.000000] +I receive an emergency temperature data: [-70.000000] +I receive an emergency temperature data: [-60.000000] +I receive an emergency temperature data: [-50.000000] +I receive an emergency temperature data: [-40.000000] +I receive an emergency temperature data: [110.000000] +I receive an emergency temperature data: [120.000000] +I receive an emergency temperature data: [130.000000] +I receive an emergency temperature data: [140.000000] +I receive an emergency temperature data: [150.000000] diff --git a/demo_nodes_cpp/test/content_filtering_subscriber.txt b/demo_nodes_cpp/test/content_filtering_subscriber.txt new file mode 100644 index 000000000..882cf66d3 --- /dev/null +++ b/demo_nodes_cpp/test/content_filtering_subscriber.txt @@ -0,0 +1 @@ +Content filter is not enabled since it's not supported diff --git a/demo_nodes_cpp/test/test_executables_tutorial.py.in b/demo_nodes_cpp/test/test_executables_tutorial.py.in index 9a9c028eb..3c1bea202 100644 --- a/demo_nodes_cpp/test/test_executables_tutorial.py.in +++ b/demo_nodes_cpp/test/test_executables_tutorial.py.in @@ -52,6 +52,12 @@ class TestExecutablesTutorial(unittest.TestCase): ) output_files = '@DEMO_NODES_CPP_EXPECTED_OUTPUT@'.split(';') for process, output_file in zip(processes_under_test, output_files): + # Some DDS features (e.g content filter) might not be implemented in all DDS, + # the different outputs are expected by different rmw_implementations. + special_output = output_file + '-@rmw_implementation@' + if os.path.isfile(special_output + '.txt'): + output_file = special_output + proc_output.assertWaitFor( expected_output=launch_testing.tools.expected_output_from_file( path=output_file