diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp
index 397f4ab6d..c5fc4bfd2 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp
@@ -73,8 +73,6 @@ class PointCloudXyzNode : public rclcpp::Node
 
   image_geometry::PinholeCameraModel model_;
 
-  void connectCb();
-
   void depthCb(
     const Image::ConstSharedPtr & depth_msg,
     const CameraInfo::ConstSharedPtr & info_msg);
diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp
index 2cfc44e03..8cf5ae852 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyz_radial.hpp
@@ -73,8 +73,6 @@ class PointCloudXyzRadialNode : public rclcpp::Node
 
   cv::Mat transform_;
 
-  void connectCb();
-
   void depthCb(
     const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg);
diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp
index cd2cbd6ea..1a9e754a2 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi.hpp
@@ -77,8 +77,6 @@ class PointCloudXyziNode : public rclcpp::Node
 
   image_geometry::PinholeCameraModel model_;
 
-  void connectCb();
-
   void imageCb(
     const Image::ConstSharedPtr & depth_msg,
     const Image::ConstSharedPtr & intensity_msg,
diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp
index 98398f227..d4838e120 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzi_radial.hpp
@@ -90,8 +90,6 @@ class PointCloudXyziRadialNode : public rclcpp::Node
 
   cv::Mat transform_;
 
-  void connectCb();
-
   void imageCb(
     const Image::ConstSharedPtr & depth_msg,
     const Image::ConstSharedPtr & intensity_msg_in,
diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp
index e18db0cfc..f2b99c165 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb.hpp
@@ -81,8 +81,6 @@ class PointCloudXyzrgbNode : public rclcpp::Node
 
   image_geometry::PinholeCameraModel model_;
 
-  void connectCb();
-
   void imageCb(
     const Image::ConstSharedPtr & depth_msg,
     const Image::ConstSharedPtr & rgb_msg,
diff --git a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp
index ef07c31d5..6db3759f7 100644
--- a/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp
+++ b/depth_image_proc/include/depth_image_proc/point_cloud_xyzrgb_radial.hpp
@@ -93,8 +93,6 @@ class PointCloudXyzrgbRadialNode : public rclcpp::Node
 
   image_geometry::PinholeCameraModel model_;
 
-  void connectCb();
-
   void imageCb(
     const Image::ConstSharedPtr & depth_msg,
     const Image::ConstSharedPtr & rgb_msg,
diff --git a/depth_image_proc/src/convert_metric.cpp b/depth_image_proc/src/convert_metric.cpp
index f52f3e560..e526ad2da 100644
--- a/depth_image_proc/src/convert_metric.cpp
+++ b/depth_image_proc/src/convert_metric.cpp
@@ -58,42 +58,30 @@ class ConvertMetricNode : public rclcpp::Node
   std::mutex connect_mutex_;
   image_transport::Publisher pub_depth_;
 
-  void connectCb();
-
   void depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg);
 };
 
 ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options)
 : Node("ConvertMetricNode", options)
 {
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // image_transport::SubscriberStatusCallback connect_cb =
-  //     std::bind(&ConvertMetricNode::connectCb, this);
-  connectCb();
-
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
-  pub_depth_ = image_transport::create_publisher(this, "image");
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void ConvertMetricNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_depth_.getNumSubscribers() == 0)
-  if (0) {
-    sub_raw_.shutdown();
-  } else if (!sub_raw_) {
-    image_transport::TransportHints hints(this, "raw");
-    sub_raw_ = image_transport::create_subscription(
-      this, "image_raw",
-      std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
-      hints.getTransport());
-  }
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (pub_depth_.getNumSubscribers() == 0) {
+        sub_raw_.shutdown();
+      } else if (!sub_raw_) {
+        image_transport::TransportHints hints(this, "raw");
+        sub_raw_ = image_transport::create_subscription(
+          this, "image_raw",
+          std::bind(&ConvertMetricNode::depthCb, this, std::placeholders::_1),
+          hints.getTransport());
+      }
+    };
+  pub_depth_ =
+    image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options);
 }
 
 void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)
diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp
index 8da179831..f2435f672 100644
--- a/depth_image_proc/src/point_cloud_xyz.cpp
+++ b/depth_image_proc/src/point_cloud_xyz.cpp
@@ -54,38 +54,31 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzNode::connectCb, this);
-  connectCb();
-
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS());
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.shutdown();
+      } else if (!sub_depth_) {
+        auto custom_qos = rmw_qos_profile_system_default;
+        custom_qos.depth = queue_size_;
+
+        sub_depth_ = image_transport::create_camera_subscription(
+          this,
+          "image_rect",
+          std::bind(
+            &PointCloudXyzNode::depthCb, this, std::placeholders::_1,
+            std::placeholders::_2),
+          "raw",
+          custom_qos);
+      }
+    };
+  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
 }
 
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyzNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_->getNumSubscribers() == 0)
-  if (0) {
-    sub_depth_.shutdown();
-  } else if (!sub_depth_) {
-    auto custom_qos = rmw_qos_profile_system_default;
-    custom_qos.depth = queue_size_;
-
-    sub_depth_ = image_transport::create_camera_subscription(
-      this,
-      "image_rect",
-      std::bind(&PointCloudXyzNode::depthCb, this, std::placeholders::_1, std::placeholders::_2),
-      "raw",
-      custom_qos);
-  }
-}
 
 void PointCloudXyzNode::depthCb(
   const Image::ConstSharedPtr & depth_msg,
diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp
index db5692709..cf0a19aba 100644
--- a/depth_image_proc/src/point_cloud_xyz_radial.cpp
+++ b/depth_image_proc/src/point_cloud_xyz_radial.cpp
@@ -53,40 +53,30 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt
   // Read parameters
   queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb =
-  //   boost::bind(&PointCloudXyzRadialNode::connectCb, this);
-  connectCb();
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.shutdown();
+      } else if (!sub_depth_) {
+        auto custom_qos = rmw_qos_profile_system_default;
+        custom_qos.depth = queue_size_;
+
+        sub_depth_ = image_transport::create_camera_subscription(
+          this,
+          "image_raw",
+          std::bind(
+            &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
+            std::placeholders::_2),
+          "raw",
+          custom_qos);
+      }
+    };
   pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "points", rclcpp::SensorDataQoS());
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyzRadialNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_.getNumSubscribers() == 0)
-  if (0) {
-    sub_depth_.shutdown();
-  } else if (!sub_depth_) {
-    auto custom_qos = rmw_qos_profile_system_default;
-    custom_qos.depth = queue_size_;
-
-    sub_depth_ = image_transport::create_camera_subscription(
-      this,
-      "image_raw",
-      std::bind(
-        &PointCloudXyzRadialNode::depthCb, this, std::placeholders::_1,
-        std::placeholders::_2),
-      "raw",
-      custom_qos);
-  }
+    "points", rclcpp::SensorDataQoS(), pub_options);
 }
 
 void PointCloudXyzRadialNode::depthCb(
diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp
index 8fe52c9f9..51f1bec68 100644
--- a/depth_image_proc/src/point_cloud_xyzi.cpp
+++ b/depth_image_proc/src/point_cloud_xyzi.cpp
@@ -71,40 +71,31 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
       std::placeholders::_2,
       std::placeholders::_3));
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyziNode::connectCb, this);
-  connectCb();
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
-  pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SensorDataQoS());
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyziNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_->getNumSubscribers() == 0)
-  if (0) {
-    sub_depth_.unsubscribe();
-    sub_intensity_.unsubscribe();
-    sub_info_.unsubscribe();
-  } else if (!sub_depth_.getSubscriber()) {
-    // parameter for depth_image_transport hint
-    std::string depth_image_transport_param = "depth_image_transport";
-
-    // depth image can use different transport.(e.g. compressedDepth)
-    image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
-    sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport());
-
-    // intensity uses normal ros transport hints.
-    image_transport::TransportHints hints(this, "raw");
-    sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport());
-    sub_info_.subscribe(this, "intensity/camera_info");
-  }
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.unsubscribe();
+        sub_intensity_.unsubscribe();
+        sub_info_.unsubscribe();
+      } else if (!sub_depth_.getSubscriber()) {
+        // parameter for depth_image_transport hint
+        std::string depth_image_transport_param = "depth_image_transport";
+
+        // depth image can use different transport.(e.g. compressedDepth)
+        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
+        sub_depth_.subscribe(this, "depth/image_rect", depth_hints.getTransport());
+
+        // intensity uses normal ros transport hints.
+        image_transport::TransportHints hints(this, "raw");
+        sub_intensity_.subscribe(this, "intensity/image_rect", hints.getTransport());
+        sub_info_.subscribe(this, "intensity/camera_info");
+      }
+    };
+  pub_point_cloud_ = create_publisher<PointCloud>("points", rclcpp::SensorDataQoS(), pub_options);
 }
 
 void PointCloudXyziNode::imageCb(
diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp
index 313826bfd..a838bc58d 100644
--- a/depth_image_proc/src/point_cloud_xyzi_radial.cpp
+++ b/depth_image_proc/src/point_cloud_xyzi_radial.cpp
@@ -69,44 +69,32 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o
       std::placeholders::_2,
       std::placeholders::_3));
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb =
-  //   boost::bind(&PointCloudXyziRadialNode::connectCb, this);
-  connectCb();
-
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // pub_point_cloud_ = nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.unsubscribe();
+        sub_intensity_.unsubscribe();
+        sub_info_.unsubscribe();
+      } else if (!sub_depth_.getSubscriber()) {
+        // parameter for depth_image_transport hint
+        std::string depth_image_transport_param = "depth_image_transport";
+
+        // depth image can use different transport.(e.g. compressedDepth)
+        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
+        sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport());
+
+        // intensity uses normal ros transport hints.
+        image_transport::TransportHints hints(this, "raw");
+        sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport());
+        sub_info_.subscribe(this, "intensity/camera_info");
+      }
+    };
   pub_point_cloud_ = create_publisher<sensor_msgs::msg::PointCloud2>(
-    "points", rclcpp::SensorDataQoS());
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyziRadialNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_.getNumSubscribers() == 0)
-  if (0) {
-    sub_depth_.unsubscribe();
-    sub_intensity_.unsubscribe();
-    sub_info_.unsubscribe();
-  } else if (!sub_depth_.getSubscriber()) {
-    // parameter for depth_image_transport hint
-    std::string depth_image_transport_param = "depth_image_transport";
-
-    // depth image can use different transport.(e.g. compressedDepth)
-    image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
-    sub_depth_.subscribe(this, "depth/image_raw", depth_hints.getTransport());
-
-    // intensity uses normal ros transport hints.
-    image_transport::TransportHints hints(this, "raw");
-    sub_intensity_.subscribe(this, "intensity/image_raw", hints.getTransport());
-    sub_info_.subscribe(this, "intensity/camera_info");
-  }
+    "points", rclcpp::SensorDataQoS(), pub_options);
 }
 
 void PointCloudXyziRadialNode::imageCb(
diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp
index 8bc94ae45..97cdcb75e 100644
--- a/depth_image_proc/src/point_cloud_xyzrgb.cpp
+++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp
@@ -80,58 +80,46 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
         std::placeholders::_3));
   }
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNode::connectCb, this);
-  connectCb();
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available
-  // pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS());
-  // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyzrgbNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_->getNumSubscribers() == 0)
-  if (0) {
-    // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-    sub_depth_.unsubscribe();
-    sub_rgb_.unsubscribe();
-    sub_info_.unsubscribe();
-  } else if (!sub_depth_.getSubscriber()) {
-    // parameter for depth_image_transport hint
-    std::string depth_image_transport_param = "depth_image_transport";
-    image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.unsubscribe();
+        sub_rgb_.unsubscribe();
+        sub_info_.unsubscribe();
+      } else if (!sub_depth_.getSubscriber()) {
+        // parameter for depth_image_transport hint
+        std::string depth_image_transport_param = "depth_image_transport";
+        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
 
-    rclcpp::SubscriptionOptions sub_opts;
-    // Update the subscription options to allow reconfigurable qos settings.
-    sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {
-      {
-        // Here all policies that are desired to be reconfigurable are listed.
-        rclcpp::QosPolicyKind::Depth,
-        rclcpp::QosPolicyKind::Durability,
-        rclcpp::QosPolicyKind::History,
-        rclcpp::QosPolicyKind::Reliability,
-      }};
+        rclcpp::SubscriptionOptions sub_opts;
+        // Update the subscription options to allow reconfigurable qos settings.
+        sub_opts.qos_overriding_options = rclcpp::QosOverridingOptions {
+          {
+            // Here all policies that are desired to be reconfigurable are listed.
+            rclcpp::QosPolicyKind::Depth,
+            rclcpp::QosPolicyKind::Durability,
+            rclcpp::QosPolicyKind::History,
+            rclcpp::QosPolicyKind::Reliability,
+          }};
 
-    // depth image can use different transport.(e.g. compressedDepth)
-    sub_depth_.subscribe(
-      this, "depth_registered/image_rect",
-      depth_hints.getTransport(), rmw_qos_profile_default, sub_opts);
+        // depth image can use different transport.(e.g. compressedDepth)
+        sub_depth_.subscribe(
+          this, "depth_registered/image_rect",
+          depth_hints.getTransport(), rmw_qos_profile_default, sub_opts);
 
-    // rgb uses normal ros transport hints.
-    image_transport::TransportHints hints(this, "raw");
-    sub_rgb_.subscribe(
-      this, "rgb/image_rect_color",
-      hints.getTransport(), rmw_qos_profile_default, sub_opts);
-    sub_info_.subscribe(this, "rgb/camera_info");
-  }
+        // rgb uses normal ros transport hints.
+        image_transport::TransportHints hints(this, "raw");
+        sub_rgb_.subscribe(
+          this, "rgb/image_rect_color",
+          hints.getTransport(), rmw_qos_profile_default, sub_opts);
+        sub_info_.subscribe(this, "rgb/camera_info");
+      }
+    };
+  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
 }
 
 void PointCloudXyzrgbNode::imageCb(
diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
index 56230eaf9..0ba393829 100644
--- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
+++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp
@@ -84,44 +84,31 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions
         std::placeholders::_3));
   }
 
-  // Monitor whether anyone is subscribed to the output
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // ros::SubscriberStatusCallback connect_cb =
-  //   boost::bind(&PointCloudXyzrgbRadialNode::connectCb, this);
-  connectCb();
-  // TODO(ros2) Implement when SubscriberStatusCallback is available
-  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
-  // std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available
-  // pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
-  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS());
-  // TODO(ros2) Implement connect_cb when SubscriberStatusCallback is available
-}
-
-// Handles (un)subscribing when clients (un)subscribe
-void PointCloudXyzrgbRadialNode::connectCb()
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-  // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-  // if (pub_point_cloud_->getNumSubscribers() == 0)
-  if (0) {
-    // TODO(ros2) Implement getNumSubscribers when rcl/rmw support it
-    sub_depth_.unsubscribe();
-    sub_rgb_.unsubscribe();
-    sub_info_.unsubscribe();
-  } else if (!sub_depth_.getSubscriber()) {
-    // parameter for depth_image_transport hint
-    std::string depth_image_transport_param = "depth_image_transport";
-    image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo & s)
+    {
+      std::lock_guard<std::mutex> lock(connect_mutex_);
+      if (s.current_count == 0) {
+        sub_depth_.unsubscribe();
+        sub_rgb_.unsubscribe();
+        sub_info_.unsubscribe();
+      } else if (!sub_depth_.getSubscriber()) {
+        // parameter for depth_image_transport hint
+        std::string depth_image_transport_param = "depth_image_transport";
+        image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param);
 
-    // depth image can use different transport.(e.g. compressedDepth)
-    sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport());
+        // depth image can use different transport.(e.g. compressedDepth)
+        sub_depth_.subscribe(this, "depth_registered/image_rect", depth_hints.getTransport());
 
-    // rgb uses normal ros transport hints.
-    image_transport::TransportHints hints(this, "raw");
-    sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport());
-    sub_info_.subscribe(this, "rgb/camera_info");
-  }
+        // rgb uses normal ros transport hints.
+        image_transport::TransportHints hints(this, "raw");
+        sub_rgb_.subscribe(this, "rgb/image_rect_color", hints.getTransport());
+        sub_info_.subscribe(this, "rgb/camera_info");
+      }
+    };
+  pub_point_cloud_ = create_publisher<PointCloud2>("points", rclcpp::SensorDataQoS(), pub_options);
 }
 
 void PointCloudXyzrgbRadialNode::imageCb(
diff --git a/image_proc/include/image_proc/rectify.hpp b/image_proc/include/image_proc/rectify.hpp
index 4b80b25b2..c1f9915b8 100644
--- a/image_proc/include/image_proc/rectify.hpp
+++ b/image_proc/include/image_proc/rectify.hpp
@@ -56,13 +56,11 @@ class RectifyNode
 
   int queue_size_;
   int interpolation;
-  std::mutex connect_mutex_;
   image_transport::Publisher pub_rect_;
 
   // Processing state (note: only safe because we're using single-threaded NodeHandle!)
   image_geometry::PinholeCameraModel model_;
 
-  void subscribeToCamera(const rmw_qos_profile_t & qos_profile);
   void imageCb(
     const sensor_msgs::msg::Image::ConstSharedPtr & image_msg,
     const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg);
diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp
index c03bd3f75..0cf0f4b44 100644
--- a/image_proc/src/crop_decimate.cpp
+++ b/image_proc/src/crop_decimate.cpp
@@ -127,11 +127,22 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
   int interpolation = this->declare_parameter("interpolation", 0);
   interpolation_ = static_cast<CropDecimateModes>(interpolation);
 
-  pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile);
-  sub_ = image_transport::create_camera_subscription(
-    this, "in/image_raw", std::bind(
-      &CropDecimateNode::imageCb, this,
-      std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
+  // Create image pub with connection callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      if (pub_.getNumSubscribers() == 0) {
+        sub_.shutdown();
+      } else if (!sub_) {
+        auto qos_profile = getTopicQosProfile(this, "in/image_raw");
+        sub_ = image_transport::create_camera_subscription(
+          this, "in/image_raw", std::bind(
+            &CropDecimateNode::imageCb, this,
+            std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
+      }
+    };
+  pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options);
 }
 
 void CropDecimateNode::imageCb(
diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp
index 24cb168b4..b784198f3 100644
--- a/image_proc/src/debayer.cpp
+++ b/image_proc/src/debayer.cpp
@@ -53,14 +53,24 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
 : Node("DebayerNode", options)
 {
   auto qos_profile = getTopicQosProfile(this, "image_raw");
-  sub_raw_ = image_transport::create_subscription(
-    this, "image_raw",
-    std::bind(
-      &DebayerNode::imageCb, this,
-      std::placeholders::_1), "raw", qos_profile);
-
-  pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile);
-  pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile);
+  // Create publisher options to setup callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) {
+        sub_raw_.shutdown();
+      } else if (!sub_raw_) {
+        auto qos_profile = getTopicQosProfile(this, "image_raw");
+        sub_raw_ = image_transport::create_subscription(
+          this, "image_raw",
+          std::bind(
+            &DebayerNode::imageCb, this,
+            std::placeholders::_1), "raw", qos_profile);
+      }
+    };
+  pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options);
+  pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile, pub_options);
   debayer_ = this->declare_parameter("debayer", 3);
 }
 
diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp
index faeb738bb..d814a3062 100644
--- a/image_proc/src/rectify.cpp
+++ b/image_proc/src/rectify.cpp
@@ -54,28 +54,23 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
   auto qos_profile = getTopicQosProfile(this, "image");
   queue_size_ = this->declare_parameter("queue_size", 5);
   interpolation = this->declare_parameter("interpolation", 1);
-  pub_rect_ = image_transport::create_publisher(this, "image_rect");
-  subscribeToCamera(qos_profile);
-}
 
-// Handles (un)subscribing when clients (un)subscribe
-void RectifyNode::subscribeToCamera(const rmw_qos_profile_t & qos_profile)
-{
-  std::lock_guard<std::mutex> lock(connect_mutex_);
-
-  /*
-  *  SubscriberStatusCallback not yet implemented
-  *
-  if (pub_rect_.getNumSubscribers() == 0)
-    sub_camera_.shutdown();
-  else if (!sub_camera_)
-  {
-  */
-  sub_camera_ = image_transport::create_camera_subscription(
-    this, "image", std::bind(
-      &RectifyNode::imageCb,
-      this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
-  // }
+  // Create publisher with connect callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      if (pub_rect_.getNumSubscribers() == 0) {
+        sub_camera_.shutdown();
+      } else if (!sub_camera_) {
+        auto qos_profile = getTopicQosProfile(this, "image");
+        sub_camera_ = image_transport::create_camera_subscription(
+          this, "image", std::bind(
+            &RectifyNode::imageCb,
+            this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
+      }
+    };
+  pub_rect_ = image_transport::create_publisher(this, "image_rect", qos_profile, pub_options);
 }
 
 void RectifyNode::imageCb(
diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp
index fc488f787..8795360d6 100644
--- a/image_proc/src/resize.cpp
+++ b/image_proc/src/resize.cpp
@@ -52,15 +52,25 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("ResizeNode", options)
 {
   auto qos_profile = getTopicQosProfile(this, "image/image_raw");
-  // Create image pub
-  pub_image_ = image_transport::create_camera_publisher(this, "resize/image_raw", qos_profile);
-  // Create image sub
-  sub_image_ = image_transport::create_camera_subscription(
-    this, "image/image_raw",
-    std::bind(
-      &ResizeNode::imageCb, this,
-      std::placeholders::_1,
-      std::placeholders::_2), "raw", qos_profile);
+  // Create image pub with connection callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      if (pub_image_.getNumSubscribers() == 0) {
+        sub_image_.shutdown();
+      } else if (!sub_image_) {
+        auto qos_profile = getTopicQosProfile(this, "image/image_raw");
+        sub_image_ = image_transport::create_camera_subscription(
+          this, "image/image_raw",
+          std::bind(
+            &ResizeNode::imageCb, this,
+            std::placeholders::_1,
+            std::placeholders::_2), "raw", qos_profile);
+      }
+    };
+  pub_image_ = image_transport::create_camera_publisher(
+    this, "resize/image_raw", qos_profile, pub_options);
 
   interpolation_ = this->declare_parameter("interpolation", 1);
   use_scale_ = this->declare_parameter("use_scale", true);
@@ -74,12 +84,6 @@ void ResizeNode::imageCb(
   sensor_msgs::msg::Image::ConstSharedPtr image_msg,
   sensor_msgs::msg::CameraInfo::ConstSharedPtr info_msg)
 {
-  // getNumSubscribers has a bug/doesn't work
-  // Eventually revisit and figure out how to make this work
-  // if (pub_image_.getNumSubscribers() < 1) {
-  //  return;
-  //}
-
   TRACEPOINT(
     image_proc_resize_init,
     static_cast<const void *>(this),