Skip to content

Commit a1bc6dc

Browse files
author
Chen Lihui
authored
add a demo of content filter listener (#557)
* add a demo of content filter listener Signed-off-by: Chen Lihui <lihui.chen@sony.com> * add warn message if content filter is not supported by DDS Signed-off-by: Chen Lihui <lihui.chen@sony.com> * keep `like` to make the demo simple and comment that rmw_connextdds not supported currently Signed-off-by: Chen Lihui <lihui.chen@sony.com> * remove the macro as it will be removed in the rclcpp Signed-off-by: Chen Lihui <lihui.chen@sony.com> * use = instead of like Signed-off-by: Chen Lihui <lihui.chen@sony.com> * address reviews Signed-off-by: Chen Lihui <lihui.chen@sony.com> * not make this configurable from the cli Signed-off-by: Chen Lihui <lihui.chen@sony.com> * remove comment and update log output Signed-off-by: Chen Lihui <lihui.chen@sony.com> * use temperature instead of chatter Signed-off-by: Chen Lihui <lihui.chen@sony.com> * rename Signed-off-by: Chen Lihui <lihui.chen@sony.com>
1 parent 657aad5 commit a1bc6dc

8 files changed

+226
-0
lines changed

‎demo_nodes_cpp/CMakeLists.txt

+9
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,8 @@ add_library(parameters_library SHARED
6868
src/parameters/parameter_events_async.cpp
6969
src/parameters/even_parameters_node.cpp)
7070
add_library(topics_library SHARED
71+
src/topics/content_filtering_publisher.cpp
72+
src/topics/content_filtering_subscriber.cpp
7173
src/topics/talker.cpp
7274
src/topics/talker_loaned_message.cpp
7375
src/topics/talker_serialized_message.cpp
@@ -109,6 +111,12 @@ rclcpp_components_register_node(parameters_library
109111
PLUGIN "demo_nodes_cpp::EvenParameterNode"
110112
EXECUTABLE even_parameters_node)
111113

114+
rclcpp_components_register_node(topics_library
115+
PLUGIN "demo_nodes_cpp::ContentFilteringPublisher"
116+
EXECUTABLE content_filtering_publisher)
117+
rclcpp_components_register_node(topics_library
118+
PLUGIN "demo_nodes_cpp::ContentFilteringSubscriber"
119+
EXECUTABLE content_filtering_subscriber)
112120
rclcpp_components_register_node(topics_library
113121
PLUGIN "demo_nodes_cpp::Talker"
114122
EXECUTABLE talker)
@@ -147,6 +155,7 @@ if(BUILD_TESTING)
147155
# Add each test case. Multi-executable tests can be specified in
148156
# semicolon-separated strings, like exe1;exe2.
149157
set(tutorial_tests
158+
"content_filtering_publisher:content_filtering_subscriber"
150159
list_parameters_async
151160
list_parameters
152161
parameter_events_async

‎demo_nodes_cpp/src/topics/content_filtering_publisher.cpp

+79
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
// Copyright 2022 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <chrono>
16+
#include <cstdio>
17+
#include <memory>
18+
#include <utility>
19+
20+
#include "rclcpp/rclcpp.hpp"
21+
#include "rclcpp_components/register_node_macro.hpp"
22+
23+
#include "std_msgs/msg/float32.hpp"
24+
25+
#include "demo_nodes_cpp/visibility_control.h"
26+
27+
using namespace std::chrono_literals;
28+
29+
namespace demo_nodes_cpp
30+
{
31+
// The simulated temperature data starts from -100.0 and ends at 150.0 with a step size of 10.0
32+
constexpr std::array<float, 3> TEMPERATURE_SETTING {-100.0f, 150.0f, 10.0f};
33+
34+
// Create a ContentFilteringPublisher class that subclasses the generic rclcpp::Node base class.
35+
// The main function below will instantiate the class as a ROS node.
36+
class ContentFilteringPublisher : public rclcpp::Node
37+
{
38+
public:
39+
DEMO_NODES_CPP_PUBLIC
40+
explicit ContentFilteringPublisher(const rclcpp::NodeOptions & options)
41+
: Node("content_filtering_publisher", options)
42+
{
43+
// Create a function for when messages are to be sent.
44+
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
45+
auto publish_message =
46+
[this]() -> void
47+
{
48+
msg_ = std::make_unique<std_msgs::msg::Float32>();
49+
msg_->data = temperature_;
50+
temperature_ += TEMPERATURE_SETTING[2];
51+
if (temperature_ > TEMPERATURE_SETTING[1]) {
52+
temperature_ = TEMPERATURE_SETTING[0];
53+
}
54+
RCLCPP_INFO(this->get_logger(), "Publishing: '%f'", msg_->data);
55+
// Put the message into a queue to be processed by the middleware.
56+
// This call is non-blocking.
57+
pub_->publish(std::move(msg_));
58+
};
59+
// Create a publisher with a custom Quality of Service profile.
60+
// Uniform initialization is suggested so it can be trivially changed to
61+
// rclcpp::KeepAll{} if the user wishes.
62+
// (rclcpp::KeepLast(7) -> rclcpp::KeepAll() fails to compile)
63+
rclcpp::QoS qos(rclcpp::KeepLast{7});
64+
pub_ = this->create_publisher<std_msgs::msg::Float32>("temperature", qos);
65+
66+
// Use a timer to schedule periodic message publishing.
67+
timer_ = this->create_wall_timer(1s, publish_message);
68+
}
69+
70+
private:
71+
float temperature_ = TEMPERATURE_SETTING[0];
72+
std::unique_ptr<std_msgs::msg::Float32> msg_;
73+
rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr pub_;
74+
rclcpp::TimerBase::SharedPtr timer_;
75+
};
76+
77+
} // namespace demo_nodes_cpp
78+
79+
RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringPublisher)

‎demo_nodes_cpp/src/topics/content_filtering_subscriber.cpp

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2022 Open Source Robotics Foundation, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "rclcpp/rclcpp.hpp"
16+
#include "rclcpp_components/register_node_macro.hpp"
17+
#include "rcpputils/join.hpp"
18+
19+
#include "std_msgs/msg/float32.hpp"
20+
21+
#include "demo_nodes_cpp/visibility_control.h"
22+
23+
namespace demo_nodes_cpp
24+
{
25+
// Emergency temperature data less than -30 or greater than 100
26+
constexpr std::array<float, 2> EMERGENCY_TEMPERATURE {-30.0f, 100.0f};
27+
28+
// Create a ContentFilteringSubscriber class that subclasses the generic rclcpp::Node base class.
29+
// The main function below will instantiate the class as a ROS node.
30+
class ContentFilteringSubscriber : public rclcpp::Node
31+
{
32+
public:
33+
DEMO_NODES_CPP_PUBLIC
34+
explicit ContentFilteringSubscriber(const rclcpp::NodeOptions & options)
35+
: Node("content_filtering_subscriber", options)
36+
{
37+
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
38+
// Create a callback function for when messages are received.
39+
auto callback =
40+
[this](const std_msgs::msg::Float32 & msg) -> void
41+
{
42+
if (msg.data < EMERGENCY_TEMPERATURE[0] || msg.data > EMERGENCY_TEMPERATURE[1]) {
43+
RCLCPP_INFO(
44+
this->get_logger(),
45+
"I receive an emergency temperature data: [%f]", msg.data);
46+
} else {
47+
RCLCPP_INFO(this->get_logger(), "I receive a temperature data: [%f]", msg.data);
48+
}
49+
};
50+
51+
// Initialize a subscription with a content filter to receive emergency temperature data that
52+
// are less than -30 or greater than 100.
53+
rclcpp::SubscriptionOptions sub_options;
54+
sub_options.content_filter_options.filter_expression = "data < %0 OR data > %1";
55+
sub_options.content_filter_options.expression_parameters = {
56+
std::to_string(EMERGENCY_TEMPERATURE[0]),
57+
std::to_string(EMERGENCY_TEMPERATURE[1])
58+
};
59+
60+
sub_ = create_subscription<std_msgs::msg::Float32>("temperature", 10, callback, sub_options);
61+
62+
if (!sub_->is_cft_enabled()) {
63+
RCLCPP_WARN(
64+
this->get_logger(), "Content filter is not enabled since it's not supported");
65+
} else {
66+
RCLCPP_INFO(
67+
this->get_logger(),
68+
"subscribed to topic \"%s\" with content filter options \"%s, {%s}\"",
69+
sub_->get_topic_name(),
70+
sub_options.content_filter_options.filter_expression.c_str(),
71+
rcpputils::join(sub_options.content_filter_options.expression_parameters, ", ").c_str());
72+
}
73+
}
74+
75+
private:
76+
rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
77+
};
78+
79+
} // namespace demo_nodes_cpp
80+
81+
RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::ContentFilteringSubscriber)

‎demo_nodes_cpp/test/content_filtering_publisher.txt

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
Publishing: '-100.000000'
2+
Publishing: '-90.000000'
3+
Publishing: '-80.000000'
4+
Publishing: '-70.000000'
5+
Publishing: '-60.000000'
6+
Publishing: '-50.000000'
7+
Publishing: '-40.000000'
8+
Publishing: '-30.000000'
9+
Publishing: '-20.000000'
10+
Publishing: '-10.000000'
11+
Publishing: '0.000000'
12+
Publishing: '10.000000'
13+
Publishing: '20.000000'
14+
Publishing: '30.000000'
15+
Publishing: '40.000000'
16+
Publishing: '50.000000'
17+
Publishing: '60.000000'
18+
Publishing: '70.000000'
19+
Publishing: '80.000000'
20+
Publishing: '90.000000'
21+
Publishing: '100.000000'
22+
Publishing: '110.000000'
23+
Publishing: '120.000000'
24+
Publishing: '130.000000'
25+
Publishing: '140.000000'
26+
Publishing: '150.000000'

‎demo_nodes_cpp/test/content_filtering_subscriber-rmw_connextdds.txt

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
I receive an emergency temperature data: [-100.000000]
2+
I receive an emergency temperature data: [-90.000000]
3+
I receive an emergency temperature data: [-80.000000]
4+
I receive an emergency temperature data: [-70.000000]
5+
I receive an emergency temperature data: [-60.000000]
6+
I receive an emergency temperature data: [-50.000000]
7+
I receive an emergency temperature data: [-40.000000]
8+
I receive an emergency temperature data: [110.000000]
9+
I receive an emergency temperature data: [120.000000]
10+
I receive an emergency temperature data: [130.000000]
11+
I receive an emergency temperature data: [140.000000]
12+
I receive an emergency temperature data: [150.000000]

‎demo_nodes_cpp/test/content_filtering_subscriber-rmw_fastrtps_cpp.txt

+12
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,12 @@
1+
I receive an emergency temperature data: [-100.000000]
2+
I receive an emergency temperature data: [-90.000000]
3+
I receive an emergency temperature data: [-80.000000]
4+
I receive an emergency temperature data: [-70.000000]
5+
I receive an emergency temperature data: [-60.000000]
6+
I receive an emergency temperature data: [-50.000000]
7+
I receive an emergency temperature data: [-40.000000]
8+
I receive an emergency temperature data: [110.000000]
9+
I receive an emergency temperature data: [120.000000]
10+
I receive an emergency temperature data: [130.000000]
11+
I receive an emergency temperature data: [140.000000]
12+
I receive an emergency temperature data: [150.000000]

‎demo_nodes_cpp/test/content_filtering_subscriber.txt

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
Content filter is not enabled since it's not supported

‎demo_nodes_cpp/test/test_executables_tutorial.py.in

+6
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,12 @@ class TestExecutablesTutorial(unittest.TestCase):
5252
)
5353
output_files = '@DEMO_NODES_CPP_EXPECTED_OUTPUT@'.split(';')
5454
for process, output_file in zip(processes_under_test, output_files):
55+
# Some DDS features (e.g content filter) might not be implemented in all DDS,
56+
# the different outputs are expected by different rmw_implementations.
57+
special_output = output_file + '-@rmw_implementation@'
58+
if os.path.isfile(special_output + '.txt'):
59+
output_file = special_output
60+
5561
proc_output.assertWaitFor(
5662
expected_output=launch_testing.tools.expected_output_from_file(
5763
path=output_file

0 commit comments

Comments
 (0)