Add lazy feature

pcl_ros/include/pcl_ros/filters/filter.hpp

@@ -126,6 +126,10 @@ class Filter : public PCLNode<sensor_msgs::msg::PointCloud2>
   virtual void
   unsubscribe();
 
+  /** \brief Create publishers for output PointCloud2 data. */
+  virtual void
+  createPublishers();
+
   /** \brief Call the child filter () method, optionally transform the result, and publish it.
     * \param input the input point cloud dataset.
     * \param indices a pointer to the vector of point indices to use.

pcl_ros/include/pcl_ros/pcl_node.hpp

@@ -53,7 +53,6 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/buffer.h>
-// #include <nodelet_topic_tools/nodelet_lazy.h>  // TODO(daisukes): lazy subscription
 
 #include <memory>
 #include <string>

pcl_ros/src/pcl_ros/filters/crop_box.cpp

@@ -156,8 +156,7 @@ pcl_ros::CropBox::CropBox(const rclcpp::NodeOptions & options)
 
   config_callback(get_parameters(param_names));
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void

pcl_ros/src/pcl_ros/filters/extract_indices.cpp

@@ -56,8 +56,8 @@ pcl_ros::ExtractIndices::ExtractIndices(const rclcpp::NodeOptions & options)
   if (!result.successful) {
     throw std::runtime_error(result.reason);
   }
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+
+  createPublishers();
 }
 
 void

pcl_ros/src/pcl_ros/filters/filter.cpp

@@ -176,10 +176,24 @@ pcl_ros::Filter::unsubscribe()
 pcl_ros::Filter::Filter(std::string node_name, const rclcpp::NodeOptions & options)
 : PCLNode(node_name, options)
 {
-  pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_);
   RCLCPP_DEBUG(this->get_logger(), "Node successfully created.");
 }
 
+//////////////////////////////////////////////////////////////////////////////////////////////
+void
+pcl_ros::Filter::createPublishers()
+{
+  auto pub_options = rclcpp::PublisherOptions();
+  pub_options.event_callbacks.matched_callback = [this](rclcpp::MatchedInfo & /*info*/) {
+      if (pub_output_->get_subscription_count() == 0) {
+        unsubscribe();
+      } else {
+        subscribe();
+      }
+    };
+  pub_output_ = create_publisher<PointCloud2>("output", max_queue_size_, pub_options);
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void
 pcl_ros::Filter::use_frame_params()

pcl_ros/src/pcl_ros/filters/passthrough.cpp

@@ -64,8 +64,7 @@ pcl_ros::PassThrough::PassThrough(const rclcpp::NodeOptions & options)
 
   config_callback(get_parameters(param_names));
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void pcl_ros::PassThrough::filter(

pcl_ros/src/pcl_ros/filters/project_inliers.cpp

@@ -75,8 +75,7 @@ pcl_ros::ProjectInliers::ProjectInliers(const rclcpp::NodeOptions & options)
   impl_.setCopyAllFields(copy_all_fields);
   impl_.setCopyAllData(copy_all_data);
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void

pcl_ros/src/pcl_ros/filters/radius_outlier_removal.cpp

@@ -79,8 +79,7 @@ pcl_ros::RadiusOutlierRemoval::RadiusOutlierRemoval(const rclcpp::NodeOptions &
 
   config_callback(get_parameters(param_names));
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void

pcl_ros/src/pcl_ros/filters/statistical_outlier_removal.cpp

@@ -88,8 +88,7 @@ pcl_ros::StatisticalOutlierRemoval::StatisticalOutlierRemoval(const rclcpp::Node
 
   config_callback(get_parameters(param_names));
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void

pcl_ros/src/pcl_ros/filters/voxel_grid.cpp

@@ -85,8 +85,7 @@ pcl_ros::VoxelGrid::VoxelGrid(const rclcpp::NodeOptions & options)
 
   config_callback(get_parameters(param_names));
 
-  // TODO(daisukes): lazy subscription after rclcpp#2060
-  subscribe();
+  createPublishers();
 }
 
 void