Add lazy subscription to filter chains

src/scan_to_cloud_filter_chain.cpp

@@ -45,22 +45,23 @@ ScanToCloudFilterChain::ScanToCloudFilterChain(
   laser_max_range_(DBL_MAX),
   buffer_(this->get_clock()),
   tf_(buffer_),
-  sub_(this, "scan", rmw_qos_profile_sensor_data),
-  filter_(sub_, buffer_, "", 50, this->shared_from_this()),
+  filter_(scan_sub_, buffer_, "", 50, this->shared_from_this()),
   cloud_filter_chain_("sensor_msgs::msg::PointCloud2"),
   scan_filter_chain_("sensor_msgs::msg::LaserScan")
 {
   rcl_interfaces::msg::ParameterDescriptor read_only_desc;
   read_only_desc.read_only = true;
 
   // Declare parameters
+  this->declare_parameter("lazy_subscription", false, read_only_desc);
   this->declare_parameter("high_fidelity", false, read_only_desc);
   this->declare_parameter("notifier_tolerance", 0.03, read_only_desc);
   this->declare_parameter("target_frame", "base_link", read_only_desc);
   this->declare_parameter("incident_angle_correction", true, read_only_desc);
   this->declare_parameter("laser_max_range", DBL_MAX, read_only_desc);
-  
+
   // Get parameters
+  this->get_parameter("lazy_subscription", lazy_subscription_);
   this->get_parameter("high_fidelity", high_fidelity_);
   this->get_parameter("notifier_tolerance", tf_tolerance_);
   this->get_parameter("target_frame", target_frame_);
@@ -79,11 +80,24 @@ ScanToCloudFilterChain::ScanToCloudFilterChain(
     this->get_node_timers_interface());
   buffer_.setCreateTimerInterface(timer_interface);
 
-  sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
-
-  filter_.connectInput(sub_);
-
-  cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered", 10);
+  if (lazy_subscription_) {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo & s)
+      {
+        if (s.current_count == 0) {
+          scan_sub_.unsubscribe();
+        } else if (!scan_sub_.getSubscriber()) {
+          scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+        }
+      };
+    cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered",
+      rclcpp::SensorDataQoS(), pub_options);
+  } else {
+    cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered",
+      rclcpp::SensorDataQoS());
+    scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  }
 
   cloud_filter_chain_.configure(
     "cloud_filter_chain",

src/scan_to_cloud_filter_chain.hpp

@@ -62,14 +62,15 @@ class ScanToCloudFilterChain : public rclcpp::Node
   tf2_ros::Buffer buffer_;
   tf2_ros::TransformListener tf_;
 
-  message_filters::Subscriber<sensor_msgs::msg::LaserScan> sub_;
+  message_filters::Subscriber<sensor_msgs::msg::LaserScan> scan_sub_;
   tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan> filter_;
 
   filters::FilterChain<sensor_msgs::msg::PointCloud2> cloud_filter_chain_;
   filters::FilterChain<sensor_msgs::msg::LaserScan> scan_filter_chain_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_pub_;
 
   // Parameters
+  bool lazy_subscription_;
   bool high_fidelity_; // High fidelity (interpolating time across scan)
   double tf_tolerance_;
   std::string target_frame_; // Target frame for high fidelity result

src/scan_to_scan_filter_chain.cpp

@@ -38,7 +38,6 @@ ScanToScanFilterChain::ScanToScanFilterChain(
 : rclcpp::Node("scan_to_scan_filter_chain", ns, options),
   tf_(NULL),
   buffer_(this->get_clock()),
-  scan_sub_(this, "scan", rmw_qos_profile_sensor_data),
   tf_filter_(NULL),
   filter_chain_("sensor_msgs::msg::LaserScan")
 {
@@ -51,10 +50,12 @@ ScanToScanFilterChain::ScanToScanFilterChain(
   read_only_desc.read_only = true;
 
   // Declare parameters
+  this->declare_parameter("lazy_subscription", false, read_only_desc);
   this->declare_parameter("tf_message_filter_target_frame", "", read_only_desc);
   this->declare_parameter("tf_message_filter_tolerance", 0.03, read_only_desc);
 
   // Get parameters
+  this->get_parameter("lazy_subscription", lazy_subscription_);
   this->get_parameter("tf_message_filter_target_frame", tf_message_filter_target_frame_);
   this->get_parameter("tf_message_filter_tolerance", tf_filter_tolerance_);
 
@@ -80,8 +81,24 @@ ScanToScanFilterChain::ScanToScanFilterChain(
         std::placeholders::_1));
   }
 
-  // Advertise output
-  output_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered", 1000);
+  if (lazy_subscription_) {
+    rclcpp::PublisherOptions pub_options;
+    pub_options.event_callbacks.matched_callback =
+      [this](rclcpp::MatchedInfo & s)
+      {
+        if (s.current_count == 0) {
+          scan_sub_.unsubscribe();
+        } else if (!scan_sub_.getSubscriber()) {
+          scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+        }
+      };
+    output_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered",
+      rclcpp::SensorDataQoS(), pub_options);
+  } else {
+    output_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered",
+      rclcpp::SensorDataQoS());
+    scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  }
 }
 
 // Destructor

src/scan_to_scan_filter_chain.hpp

@@ -58,6 +58,7 @@ class ScanToScanFilterChain : public rclcpp::Node
   rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr output_pub_;
 
   // Parameters
+  bool lazy_subscription_;
   std::string tf_message_filter_target_frame_;
   double tf_filter_tolerance_;