Add lazy subscription to scan_to_cloud filter chain

ros-perception · Jul 16, 2024 · b7f6131 · b7f6131
1 parent 1be9623
commit b7f6131
Showing 1 changed file with 21 additions and 8 deletions.
diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp
@@ -68,14 +68,15 @@ class ScanToCloudFilterChain
   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
@@ -87,22 +88,23 @@ class ScanToCloudFilterChain
                                                          laser_max_range_(DBL_MAX),
                                                          buffer_(nh_->get_clock()),
                                                          tf_(buffer_),
-                                                         sub_(nh_, "scan", rmw_qos_profile_sensor_data),
-                                                         filter_(sub_, buffer_, "", 50, nh_),
+                                                         filter_(scan_sub_, buffer_, "", 50, nh_),
                                                          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
+    nh_->declare_parameter("lazy_subscription", false, read_only_desc);
     nh_->declare_parameter("high_fidelity", false, read_only_desc);
     nh_->declare_parameter("notifier_tolerance", 0.03, read_only_desc);
     nh_->declare_parameter("target_frame", "base_link", read_only_desc);
     nh_->declare_parameter("incident_angle_correction", true, read_only_desc);
     nh_->declare_parameter("laser_max_range", DBL_MAX, read_only_desc);
 
     // Get parameters
+    nh_->get_parameter("lazy_subscription", lazy_subscription_);
     nh_->get_parameter("high_fidelity", high_fidelity_);
     nh_->get_parameter("notifier_tolerance", tf_tolerance_);
     nh_->get_parameter("target_frame", target_frame_);
@@ -117,12 +119,23 @@ class ScanToCloudFilterChain
       nh_->get_node_base_interface(),
       nh_->get_node_timers_interface());
     buffer_.setCreateTimerInterface(timer_interface);
-
-    sub_.subscribe(nh_, "scan", rmw_qos_profile_sensor_data);
-
-    filter_.connectInput(sub_);
 
-    cloud_pub_ = nh_->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(nh_, "scan", rmw_qos_profile_sensor_data);
+          }
+        };
+      cloud_pub_ = nh_->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered", rclcpp::SensorDataQoS(), pub_options);
+    } else {
+      cloud_pub_ = nh_->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered", rclcpp::SensorDataQoS());
+      scan_sub_.subscribe(nh_, "scan", rmw_qos_profile_sensor_data);
+    }
 
     cloud_filter_chain_.configure("cloud_filter_chain", nh_->get_node_logging_interface(), nh_->get_node_parameters_interface());