Merge pull request #195 from bjsowa/lazy-subscription

Add Lazy subscription
ros-perception · Jul 28, 2024 · 9dbd42e · 9dbd42e
2 parents eaaaeab + 2105037
commit 9dbd42e
Show file tree

Hide file tree

Showing 5 changed files with 67 additions and 10 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -12,6 +12,10 @@ ament_auto_find_build_dependencies()
 # Build
 ##############################################################################
 
+if(${rclcpp_VERSION_MAJOR} GREATER_EQUAL 20)
+    add_compile_definitions(RCLCPP_SUPPORTS_MATCHED_CALLBACKS)
+endif()
+
 ament_auto_add_library(laser_scan_filters SHARED src/laser_scan_filters.cpp)
 ament_auto_add_library(laser_filter_chains SHARED
                          src/scan_to_cloud_filter_chain.cpp

diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp
@@ -45,8 +45,7 @@ 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->get_node_logging_interface(),
+  filter_(scan_sub_, buffer_, "", 50, this->get_node_logging_interface(),
     this->get_node_clock_interface()),
   cloud_filter_chain_("sensor_msgs::msg::PointCloud2"),
   scan_filter_chain_("sensor_msgs::msg::LaserScan")
@@ -55,13 +54,19 @@ ScanToCloudFilterChain::ScanToCloudFilterChain(
   read_only_desc.read_only = true;
 
   // Declare parameters
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  this->declare_parameter("lazy_subscription", false, read_only_desc);
+  #endif
   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
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  this->get_parameter("lazy_subscription", lazy_subscription_);
+  #endif
   this->get_parameter("high_fidelity", high_fidelity_);
   this->get_parameter("notifier_tolerance", tf_tolerance_);
   this->get_parameter("target_frame", target_frame_);
@@ -80,11 +85,28 @@ ScanToCloudFilterChain::ScanToCloudFilterChain(
     this->get_node_timers_interface());
   buffer_.setCreateTimerInterface(timer_interface);
 
-  sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
-
-  filter_.connectInput(sub_);
-
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  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", 10, pub_options);
+  } else {
+    cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered", 10);
+    scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  }
+  #else
   cloud_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("cloud_filtered", 10);
+  scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  #endif
 
   cloud_filter_chain_.configure(
     "cloud_filter_chain",

diff --git a/src/scan_to_cloud_filter_chain.hpp b/src/scan_to_cloud_filter_chain.hpp
@@ -62,14 +62,17 @@ 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
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  bool lazy_subscription_;
+  #endif
   bool high_fidelity_; // High fidelity (interpolating time across scan)
   double tf_tolerance_;
   std::string target_frame_; // Target frame for high fidelity result

diff --git a/src/scan_to_scan_filter_chain.cpp b/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,16 @@ ScanToScanFilterChain::ScanToScanFilterChain(
   read_only_desc.read_only = true;
 
   // Declare parameters
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  this->declare_parameter("lazy_subscription", false, read_only_desc);
+  #endif
   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
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  this->get_parameter("lazy_subscription", lazy_subscription_);
+  #endif
   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 +85,28 @@ ScanToScanFilterChain::ScanToScanFilterChain(
         std::placeholders::_1));
   }
 
-  // Advertise output
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  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", 1000, pub_options);
+  } else {
+    output_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered", 1000);
+    scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  }
+  #else
   output_pub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan_filtered", 1000);
+  scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data);
+  #endif
 }
 
 // Destructor

diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp
@@ -58,6 +58,9 @@ class ScanToScanFilterChain : public rclcpp::Node
   rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr output_pub_;
 
   // Parameters
+  #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS
+  bool lazy_subscription_;
+  #endif
   std::string tf_message_filter_target_frame_;
   double tf_filter_tolerance_;