diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index e3b284a..52e84db 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -68,7 +68,7 @@ class ScanToCloudFilterChain tf2_ros::Buffer buffer_; tf2_ros::TransformListener tf_; - message_filters::Subscriber sub_; + message_filters::Subscriber scan_sub_; tf2_ros::MessageFilter filter_; filters::FilterChain cloud_filter_chain_; @@ -76,6 +76,7 @@ class ScanToCloudFilterChain rclcpp::Publisher::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,8 +88,7 @@ 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") { @@ -96,6 +96,7 @@ class ScanToCloudFilterChain 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); @@ -103,6 +104,7 @@ class ScanToCloudFilterChain 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("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("cloud_filtered", rclcpp::SensorDataQoS(), pub_options); + } else { + cloud_pub_ = nh_->create_publisher("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());