diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index 0ea40fe..83d5697 100644 --- 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->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") { @@ -54,13 +53,15 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( 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("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("cloud_filtered", + rclcpp::SensorDataQoS(), pub_options); + } else { + cloud_pub_ = this->create_publisher("cloud_filtered", + rclcpp::SensorDataQoS()); + scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); + } 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 index 450848b..08c78a6 100644 --- a/src/scan_to_cloud_filter_chain.hpp +++ b/src/scan_to_cloud_filter_chain.hpp @@ -62,7 +62,7 @@ class ScanToCloudFilterChain : public rclcpp::Node 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_; @@ -70,6 +70,7 @@ class ScanToCloudFilterChain : public rclcpp::Node 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 diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index 582b670..466feaa 100644 --- 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,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("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("scan_filtered", + rclcpp::SensorDataQoS(), pub_options); + } else { + output_pub_ = this->create_publisher("scan_filtered", + rclcpp::SensorDataQoS()); + scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); + } } // Destructor diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp index a7501cb..ae1c217 100644 --- a/src/scan_to_scan_filter_chain.hpp +++ b/src/scan_to_scan_filter_chain.hpp @@ -58,6 +58,7 @@ class ScanToScanFilterChain : public rclcpp::Node rclcpp::Publisher::SharedPtr output_pub_; // Parameters + bool lazy_subscription_; std::string tf_message_filter_target_frame_; double tf_filter_tolerance_;