Add matched event demo for rclcpp and rclpy

demo_nodes_cpp/CMakeLists.txt

@@ -80,10 +80,13 @@ add_library(topics_library SHARED
   src/topics/listener.cpp
   src/topics/listener_serialized_message.cpp
   src/topics/listener_best_effort.cpp)
+add_library(events_library SHARED
+  src/events/matched_event_detect.cpp)
 add_demo_dependencies(timers_library)
 add_demo_dependencies(services_library)
 add_demo_dependencies(parameters_library)
 add_demo_dependencies(topics_library)
+add_demo_dependencies(events_library)
 
 rclcpp_components_register_node(timers_library
   PLUGIN "demo_nodes_cpp::OneOffTimerNode"
@@ -149,11 +152,17 @@ rclcpp_components_register_node(topics_library
   PLUGIN "demo_nodes_cpp::ListenerBestEffort"
   EXECUTABLE listener_best_effort)
 
+rclcpp_components_register_node(events_library
+  PLUGIN "demo_nodes_cpp::MatchedEventDetectNode"
+  EXECUTABLE matched_event_detect
+)
+
 install(TARGETS
   timers_library
   services_library
   parameters_library
   topics_library
+  events_library
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)

demo_nodes_cpp/src/events/matched_event_detect.cpp

@@ -0,0 +1,111 @@
+// Copyright 2023 Sony Group Corporation.
+//
+// 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 "rcl/event.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+#include "std_msgs/msg/string.hpp"
+
+#include "demo_nodes_cpp/visibility_control.h"
+
+namespace demo_nodes_cpp
+{
+
+// This demo program shows detected matched event.
+// Matched event occur while connection between publisher and subscription is done.
+// Run this demo at one console by below command
+// $ ros2 run demo_nodes_cpp matched_event_detect
+// It will create 2 topics.
+// - 'pub_matched_event_detect' for detecting matched event of publisher
+// - 'sub_matched_event_detect' for detecting matched event of subscription
+//
+// For checking matched event of publisher
+// On another console, run below command
+// $ ros2 topic echo /pub_matched_event_detect
+// You can run above command many times on different consoles to check the number of connected
+// subscription by the output of demo program.
+//
+// For checking matched event of subscription
+// On another console, run below command
+// ros2 topic pub -r 1 /sub_matched_event_detect std_msgs/String "{data: '123'}"
+// You can run above command many times on different consoles to check the number of connected
+// publisher by the output of demo program.
+
+class MatchedEventDetectNode : public rclcpp::Node
+{
+public:
+  DEMO_NODES_CPP_PUBLIC explicit MatchedEventDetectNode(const rclcpp::NodeOptions & options)
+  : Node("matched_event_detection_node", options)
+  {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rmw_matched_status_t & s) {
+        if (connect_subscription_) {
+          if (s.current_count == 0) {
+            RCLCPP_INFO(this->get_logger(), "Last subscription is disconnected.");
+            connect_subscription_ = false;
+          } else {
+            RCLCPP_INFO(
+              this->get_logger(),
+              "Current number of connected subscription is %lu", s.current_count);
+          }
+        } else {
+          if (s.current_count != 0) {
+            RCLCPP_INFO(this->get_logger(), "First subscription is connected.");
+            connect_subscription_ = true;
+          }
+        }
+      };
+
+    pub_ = create_publisher<std_msgs::msg::String>(
+      "pub_matched_event_detect", 10, pub_options);
+
+    rclcpp::SubscriptionOptions sub_options;
+    sub_options.event_callbacks.matched_callback =
+      [this](rmw_matched_status_t & s) {
+        if (connect_publisher_) {
+          if (s.current_count == 0) {
+            RCLCPP_INFO(this->get_logger(), "Last publisher is disconnected.");
+            connect_publisher_ = false;
+          } else {
+            RCLCPP_INFO(
+              this->get_logger(),
+              "Current number of connected publisher is %lu", s.current_count);
+          }
+        } else {
+          if (s.current_count != 0) {
+            RCLCPP_INFO(this->get_logger(), "First publisher is connected.");
+            connect_publisher_ = true;
+          }
+        }
+      };
+    sub_ = create_subscription<std_msgs::msg::String>(
+      "sub_matched_event_detect",
+      10,
+      [](std_msgs::msg::String::ConstSharedPtr) {},
+      sub_options);
+  }
+
+private:
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
+  bool connect_subscription_{false};
+  bool connect_publisher_{false};
+};
+
+}  // namespace demo_nodes_cpp
+
+RCLCPP_COMPONENTS_REGISTER_NODE(demo_nodes_cpp::MatchedEventDetectNode)

demo_nodes_py/demo_nodes_py/events/matched_event_detect.py

@@ -0,0 +1,108 @@
+# Copyright 2023 Sony Group Corporation.
+#
+# 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.
+
+import rclpy
+from rclpy.event_handler import PublisherEventCallbacks
+from rclpy.event_handler import QoSPublisherMatchedInfo
+from rclpy.event_handler import QoSSubscriptionMatchedInfo
+from rclpy.event_handler import SubscriptionEventCallbacks
+from rclpy.executors import ExternalShutdownException
+from rclpy.node import Node
+
+from std_msgs.msg import String
+
+'''
+ This demo program shows detected matched event.
+ Matched event occur while connection between publisher and subscription is done.
+ Run this demo at one console by below command
+ $ ros2 run demo_nodes_py matched_event_detect
+ It will create 2 topics.
+ - 'pub_matched_event_detect' for detecting matched event of publisher
+ - 'sub_matched_event_detect' for detecting matched event of subscription
+
+ For checking matched event of publisher
+ On another console, run below command
+ $ ros2 topic echo /pub_matched_event_detect
+ You can run above command many times on different consoles to check the number of connected
+ subscription by the output of demo program.
+
+ For checking matched event of subscription
+ On another console, run below command
+ ros2 topic pub -r 1 /sub_matched_event_detect std_msgs/String "{data: '123'}"
+ You can run above command many times on different consoles to check the number of connected
+ publisher by the output of demo program.
+'''
+
+
+class MatchedEventDetectNode(Node):
+
+    def __init__(self):
+        super().__init__('matched_event_detection_node')
+        self.connect_subscription = False  # used for publisher event
+        self.connect_publisher = False  # used for subscription event
+
+        pub_event_callback = PublisherEventCallbacks(matched=self.pub_matched_event_callback)
+        self.pub = self.create_publisher(String, 'pub_matched_event_detect', 10,
+                                         event_callbacks=pub_event_callback)
+
+        sub_event_callback = SubscriptionEventCallbacks(matched=self.sub_matched_event_callback)
+        self.sub = self.create_subscription(String, 'sub_matched_event_detect', self.sub_callback,
+                                            10, event_callbacks=sub_event_callback)
+
+    def pub_matched_event_callback(self, info: QoSPublisherMatchedInfo):
+        if self.connect_subscription:
+            if info.current_count == 0:
+                self.get_logger().info('Last subscription is disconnected.')
+                self.connect_subscription = False
+            else:
+                self.get_logger().info('Current number of connected subscription is '
+                                       + str(info.current_count))
+        else:
+            if info.current_count != 0:
+                self.get_logger().info('First subscription is connected.')
+                self.connect_subscription = True
+
+    def sub_matched_event_callback(self, info: QoSSubscriptionMatchedInfo):
+        if self.connect_subscription:
+            if info.current_count == 0:
+                self.get_logger().info('Last publisher is disconnected.')
+                self.connect_subscription = False
+            else:
+                self.get_logger().info('Current number of connected publisher is '
+                                       + str(info.current_count))
+        else:
+            if info.current_count != 0:
+                self.get_logger().info('First publisher is connected.')
+                self.connect_subscription = True
+
+    def sub_callback(self, msg):
+        True
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = MatchedEventDetectNode()
+
+    try:
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == '__main__':
+    main()

demo_nodes_py/setup.py

@@ -43,6 +43,7 @@
             'async_param_client = demo_nodes_py.parameters.async_param_client:main',
             'set_parameters_callback = demo_nodes_py.parameters.set_parameters_callback:main',
             'introspection = demo_nodes_py.services.introspection:main',
+            'matched_event_detect = demo_nodes_py.events.matched_event_detect:main'
         ],
     },
 )