Add matched event demo for rclcpp and rclpy

demo_nodes_cpp/CMakeLists.txt

@@ -55,6 +55,9 @@ custom_executable(parameters parameter_events)
 custom_executable(parameters parameter_event_handler)
 custom_executable(parameters set_and_get_parameters_async)
 
+# Tutorials of events
+custom_executable(events matched_event_detect)
+
 add_library(timers_library SHARED
   src/timers/one_off_timer.cpp
   src/timers/reuse_timer.cpp)
@@ -175,6 +178,7 @@ if(BUILD_TESTING)
     parameter_events
     set_and_get_parameters_async
     set_and_get_parameters
+    matched_event_detect
     "talker:listener")
   set(service_tutorial_tests
     "add_two_ints_server:add_two_ints_client"

demo_nodes_cpp/README.md

@@ -27,6 +27,7 @@ This package consists of the following examples:
 22. `listener`
 23. `parameter_events_async` 
 24. `listener_best_effort`
+25. `matched_event_detect`
 
 ## **Build**
 
@@ -274,6 +275,18 @@ This runs `loaned_message_talker` ROS 2 node that publishes unique messages whic
 ros2 run demo_nodes_cpp talker_loaned_message
 ```
 
+### Matched Event Detect
+
+This runs 3 ROS2 nodes.  
+`matched_event_detect_node` node that set matched event callback for publisher and subscription separately to output connection or disconnection information.  
+`multi_sub_node` create/destroy subscriptions which connect the publisher of `matched_event_detect_node`.  
+`multi_pub_node` create/destroy publishers which connect the subscription of `matched_event_detect_node`.
+
+```bash
+# Open new terminal
+ros2 run demo_nodes_cpp matched_event_detect
+```
+
 ## **Verify**
 
 ### Basic Talker & Listener
@@ -671,6 +684,29 @@ When executed correctly, strings should be printed to terminal similar to what i
 
 > Note that Fast-DDS does not support the loaned messages. The loaned message API is used in iceoryx right now, which workes with CycloneDDS.
 
+### Matched Event Detect
+
+When executed correctly, strings should be printed to terminal similar to what is shown below:
+```bash
+#  In terminal running matched_event_detect
+[INFO] [1679887690.127684740] [multi_sub_node]: Create a new subscription.
+[INFO] [1679887690.128090105] [matched_event_detect_node]: First subscription is connected.
+[INFO] [1679887690.128836774] [multi_sub_node]: Create a new subscription.
+[INFO] [1679887690.129157780] [matched_event_detect_node]: The changed number of connected subscription is 1 and current number of connected subscription is 2.
+[INFO] [1679887690.129193220] [multi_sub_node]: Destroy a subscription.
+[INFO] [1679887690.130552475] [matched_event_detect_node]: The changed number of connected subscription is -1 and current number of connected subscription is 1.
+[INFO] [1679887690.130588555] [multi_sub_node]: Destroy a subscription.
+[INFO] [1679887690.131355128] [matched_event_detect_node]: Last subscription is disconnected.
+[INFO] [1679887690.132014952] [multi_pub_node]: Create a new publisher.
+[INFO] [1679887690.132262901] [matched_event_detect_node]: First publisher is connected.
+[INFO] [1679887690.132898522] [multi_pub_node]: Create a new publisher.
+[INFO] [1679887690.133143624] [matched_event_detect_node]: The changed number of connected publisher is 1 and current number of connected publisher is 2.
+[INFO] [1679887690.133178687] [multi_pub_node]: Destroy a publisher.
+[INFO] [1679887690.134139929] [matched_event_detect_node]: The changed number of connected publisher is -1 and current number of connected publisher is 1.
+[INFO] [1679887690.134176647] [multi_pub_node]: Destroy a publisher.
+[INFO] [1679887690.134887946] [matched_event_detect_node]: Last publisher is disconnected.
+```
+
 ## **FAQ**
 
 `Q`: Encountered the following error in terminal when running **Loaned Message Talker**: 

demo_nodes_cpp/src/events/matched_event_detect.cpp

@@ -0,0 +1,249 @@
+// 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 <chrono>
+#include <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+// This demo program shows matched event work.
+// Matched event occurs when publisher and subscription establishes the connection.
+// Class MatchedEventDetectNode output connection information of publisher and subscription.
+// Class MultiSubNode is used to create/destroy subscription to connect/disconnect the publisher of
+// MatchedEventDetectNode.
+// Class MultiPubNode is used to created/destroy publisher to connect/disconnect the subscription
+// of MatchedEventDetectNode.
+
+class MatchedEventDetectNode : public rclcpp::Node
+{
+public:
+  explicit MatchedEventDetectNode(
+    const std::string & pub_topic_name,
+    const std::string & sub_topic_name)
+  : Node("matched_event_detect_node")
+  {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo & s) {
+        if (any_subscription_connected_) {
+          if (s.current_count == 0) {
+            RCLCPP_INFO(this->get_logger(), "Last subscription is disconnected.");
+            any_subscription_connected_ = false;
+          } else {
+            RCLCPP_INFO(
+              this->get_logger(),
+              "The changed number of connected subscription is %d and current number of connected"
+              " subscription is %lu.", s.current_count_change, s.current_count);
+          }
+        } else {
+          if (s.current_count != 0) {
+            RCLCPP_INFO(this->get_logger(), "First subscription is connected.");
+            any_subscription_connected_ = true;
+          }
+        }
+      };
+
+    pub_ = create_publisher<std_msgs::msg::String>(
+      pub_topic_name, 10, pub_options);
+
+    rclcpp::SubscriptionOptions sub_options;
+    sub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo & s) {
+        if (any_publisher_connected_) {
+          if (s.current_count == 0) {
+            RCLCPP_INFO(this->get_logger(), "Last publisher is disconnected.");
+            any_publisher_connected_ = false;
+          } else {
+            RCLCPP_INFO(
+              this->get_logger(),
+              "The changed number of connected publisher is %d and current number of connected"
+              " publisher is %lu.", s.current_count_change, s.current_count);
+          }
+        } else {
+          if (s.current_count != 0) {
+            RCLCPP_INFO(this->get_logger(), "First publisher is connected.");
+            any_publisher_connected_ = true;
+          }
+        }
+      };
+    sub_ = create_subscription<std_msgs::msg::String>(
+      sub_topic_name,
+      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 any_subscription_connected_{false};
+  bool any_publisher_connected_{false};
+};
+
+class MultiSubNode : public rclcpp::Node
+{
+public:
+  explicit MultiSubNode(const std::string & topic_name)
+  : Node("multi_sub_node"),
+    topic_name_(topic_name)
+  {}
+
+  rclcpp::Subscription<std_msgs::msg::String>::WeakPtr create_one_sub(void)
+  {
+    auto sub = create_subscription<std_msgs::msg::String>(
+      topic_name_,
+      10,
+      [](std_msgs::msg::String::ConstSharedPtr) {});
+    RCLCPP_INFO(this->get_logger(), "Create a new subscription.");
+    subs_.emplace_back(sub);
+
+    return sub;
+  }
+
+  void destroy_one_sub(rclcpp::Subscription<std_msgs::msg::String>::WeakPtr sub)
+  {
+    auto sub_shared_ptr = sub.lock();
+    if (sub_shared_ptr == nullptr) {
+      return;
+    }
+
+    for (auto s = subs_.begin(); s != subs_.end(); s++) {
+      if (*s == sub_shared_ptr) {
+        RCLCPP_INFO(this->get_logger(), "Destroy a subscription.");
+        subs_.erase(s);
+        break;
+      }
+    }
+  }
+
+private:
+  std::string topic_name_;
+  std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subs_;
+};
+
+class MultiPubNode : public rclcpp::Node
+{
+public:
+  explicit MultiPubNode(const std::string & topic_name)
+  : Node("multi_pub_node"),
+    topic_name_(topic_name)
+  {}
+
+  rclcpp::Publisher<std_msgs::msg::String>::WeakPtr create_one_pub(void)
+  {
+    auto pub = create_publisher<std_msgs::msg::String>(topic_name_, 10);
+    RCLCPP_INFO(this->get_logger(), "Create a new publisher.");
+    pubs_.emplace_back(pub);
+
+    return pub;
+  }
+
+  void destroy_one_pub(rclcpp::Publisher<std_msgs::msg::String>::WeakPtr pub)
+  {
+    auto pub_shared_ptr = pub.lock();
+    if (pub_shared_ptr == nullptr) {
+      return;
+    }
+
+    for (auto s = pubs_.begin(); s != pubs_.end(); s++) {
+      if (*s == pub_shared_ptr) {
+        RCLCPP_INFO(this->get_logger(), "Destroy a publisher.");
+        pubs_.erase(s);
+        break;
+      }
+    }
+  }
+
+private:
+  std::string topic_name_;
+  std::vector<rclcpp::Publisher<std_msgs::msg::String>::SharedPtr> pubs_;
+};
+
+int main(int argc, char ** argv)
+{
+  setvbuf(stdout, NULL, _IONBF, BUFSIZ);
+  rclcpp::init(argc, argv);
+
+  std::string topic_name_for_detect_pub_matched_event = "pub_topic_matched_event_detect";
+  std::string topic_name_for_detect_sub_matched_event = "sub_topic_matched_event_detect";
+
+  rclcpp::executors::SingleThreadedExecutor executor;
+
+  auto matched_event_detect_node = std::make_shared<MatchedEventDetectNode>(
+    topic_name_for_detect_pub_matched_event,
+    topic_name_for_detect_sub_matched_event);
+
+  auto multi_sub_node = std::make_shared<MultiSubNode>(
+    topic_name_for_detect_pub_matched_event);
+
+  auto multi_pub_node = std::make_shared<MultiPubNode>(
+    topic_name_for_detect_sub_matched_event);
+
+  executor.add_node(matched_event_detect_node);
+  executor.add_node(multi_sub_node);
+  executor.add_node(multi_pub_node);
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // First subscription is connected.
+  auto sub1 = multi_sub_node->create_one_sub();
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // The changed number of connected subscription is 1 and current number of connected subscription
+  // is 2.
+  auto sub2 = multi_sub_node->create_one_sub();
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // The changed number of connected subscription is -1 and current number of connected subscription
+  // is 1.
+  multi_sub_node->destroy_one_sub(sub1);
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // Last subscription is disconnected.
+  multi_sub_node->destroy_one_sub(sub2);
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // First publisher is connected.
+  auto pub1 = multi_pub_node->create_one_pub();
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // The changed number of connected publisher is 1 and current number of connected publisher
+  // is 2.
+  auto pub2 = multi_pub_node->create_one_pub();
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // The changed number of connected publisher is -1 and current number of connected publisher
+  // is 1.
+  multi_pub_node->destroy_one_pub(pub1);
+  executor.spin_some(200ms);
+
+  // MatchedEventDetectNode will output:
+  // Last publisher is disconnected.
+  multi_pub_node->destroy_one_pub(pub2);
+  executor.spin_some(200ms);
+
+  rclcpp::shutdown();
+  return 0;
+}

demo_nodes_cpp/test/matched_event_detect.txt

@@ -0,0 +1,16 @@
+Create a new subscription.
+First subscription is connected.
+Create a new subscription.
+The changed number of connected subscription is 1 and current number of connected subscription is 2.
+Destroy a subscription.
+The changed number of connected subscription is -1 and current number of connected subscription is 1.
+Destroy a subscription.
+Last subscription is disconnected.
+Create a new publisher.
+First publisher is connected.
+Create a new publisher.
+The changed number of connected publisher is 1 and current number of connected publisher is 2.
+Destroy a publisher.
+The changed number of connected publisher is -1 and current number of connected publisher is 1.
+Destroy a publisher.
+Last publisher is disconnected.