From 01f3ddbe40180b57219be5e68df62cf638821e36 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Thu, 1 Feb 2024 15:19:46 -0500
Subject: [PATCH] implement proper lazy subscribers (#136)

Fixes #119, works in Jazzy and later
---
 openni2_camera/src/openni2_driver.cpp | 40 +++++++++++++++++++++------
 1 file changed, 31 insertions(+), 9 deletions(-)

diff --git a/openni2_camera/src/openni2_driver.cpp b/openni2_camera/src/openni2_driver.cpp
index 8ae99c2..71f98cc 100644
--- a/openni2_camera/src/openni2_driver.cpp
+++ b/openni2_camera/src/openni2_driver.cpp
@@ -139,11 +139,6 @@ void OpenNI2Driver::periodic()
     initialized_ = true;
   }
 
-  // TODO: check subscriber counts, enable cameras
-  colorConnectCb();
-  depthConnectCb();
-  irConnectCb();
-
   if (enable_reconnect_)
     monitorConnection();
 }
@@ -162,18 +157,45 @@ void OpenNI2Driver::advertiseROSTopics()
   // Asus Xtion PRO does not have an RGB camera
   if (device_->hasColorSensor())
   {
-    pub_color_ = it.advertiseCamera("rgb/image_raw", 1);
+    // Create publisher with connect callback
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo&)
+      {
+        colorConnectCb();
+      };
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default;
+    custom_qos.depth = 1;
+    pub_color_ = image_transport::create_camera_publisher(this, "rgb/image_raw", custom_qos, pub_options);
   }
 
   if (device_->hasIRSensor())
   {
-    pub_ir_ = it.advertiseCamera("ir/image", 1);
+    // Create publisher with connect callback
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo&)
+      {
+        irConnectCb();
+      };
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default;
+    custom_qos.depth = 1;
+    pub_ir_ = image_transport::create_camera_publisher(this, "ir/image_raw", custom_qos, pub_options);
   }
 
   if (device_->hasDepthSensor())
   {
-    pub_depth_raw_ = it.advertiseCamera("depth/image_raw", 1);
-    pub_depth_ = it.advertiseCamera("depth/image", 1);
+    // Create publisher with connect callback
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo&)
+      {
+        depthConnectCb();
+      };
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default;
+    custom_qos.depth = 1;
+    pub_depth_raw_ = image_transport::create_camera_publisher(this, "depth/image_raw", custom_qos, pub_options);
+    pub_depth_ = image_transport::create_camera_publisher(this, "depth/image", custom_qos, pub_options);
     pub_projector_info_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("projector/camera_info", 1);
   }