add a demo of content filter listener (#557)

* add a demo of content filter listener

Signed-off-by: Chen Lihui <[email protected]>

* add warn message if content filter is not supported by DDS

Signed-off-by: Chen Lihui <[email protected]>

* keep `like` to make the demo simple

and comment that rmw_connextdds not supported currently

Signed-off-by: Chen Lihui <[email protected]>

* remove the macro as it will be removed in the rclcpp

Signed-off-by: Chen Lihui <[email protected]>

* use = instead of like

Signed-off-by: Chen Lihui <[email protected]>

* address reviews

Signed-off-by: Chen Lihui <[email protected]>

* not make this configurable from the cli

Signed-off-by: Chen Lihui <[email protected]>

* remove comment and update log output

Signed-off-by: Chen Lihui <[email protected]>

* use temperature instead of chatter

Signed-off-by: Chen Lihui <[email protected]>

* rename

Signed-off-by: Chen Lihui <[email protected]>
(cherry picked from commit a1bc6dc)

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

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 <chrono>
+#include <cstdio>
+#include <memory>
+#include <utility>
+
+#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<float, 3> 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<std_msgs::msg::Float32>();
+        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<std_msgs::msg::Float32>("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<std_msgs::msg::Float32> msg_;
+  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace demo_nodes_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringPublisher)

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<float, 2> 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<std_msgs::msg::Float32>("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<std_msgs::msg::Float32>::SharedPtr sub_;
+};
+
+}  // namespace demo_nodes_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringSubscriber)

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'

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]

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]

demo_nodes_cpp/test/content_filtering_subscriber.txt

@@ -0,0 +1 @@
+Content filter is not enabled since it's not supported

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