implement lazy subscribers

ros-perception · Nov 14, 2023 · e8e8c84 · e8e8c84
1 parent fcd3939
commit e8e8c84
Show file tree

Hide file tree

Showing 18 changed files with 260 additions and 302 deletions.
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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -58,42 +58,32 @@ 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
@@ -54,38 +54,32 @@ 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
@@ -53,40 +53,32 @@ 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);
-  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);
-  }
+  // 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(), 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
@@ -71,40 +71,34 @@ 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
@@ -69,44 +69,34 @@ 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);
-  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");
-  }
+  // 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(), pub_options);
 }
 
 void PointCloudXyziRadialNode::imageCb(