From 280113d5727b4c811f7c7c5a27e172b883e6d2aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Wed, 24 Jul 2024 00:06:47 +0200 Subject: [PATCH] Add lazy subscription to filter chains --- CMakeLists.txt | 4 +++ package.xml | 1 + src/scan_to_cloud_filter_chain.cpp | 40 ++++++++++++++++++++++++------ src/scan_to_cloud_filter_chain.hpp | 5 +++- src/scan_to_scan_filter_chain.cpp | 33 +++++++++++++++++++++--- src/scan_to_scan_filter_chain.hpp | 3 +++ 6 files changed, 74 insertions(+), 12 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f74c63e..e8737c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,10 @@ ament_auto_find_build_dependencies() # Build ############################################################################## +if($ENV{ROS_DISTRO} STREQUAL "humble") + add_compile_definitions(IS_HUMBLE) +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/package.xml b/package.xml index 6e095fc..0789e83 100644 --- a/package.xml +++ b/package.xml @@ -21,6 +21,7 @@ rclcpp rclcpp_lifecycle rclcpp_components + ros_environment sensor_msgs tf2 tf2_ros diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index f200183..f09bc89 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->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 + #ifndef IS_HUMBLE + 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 + #ifndef IS_HUMBLE + 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,30 @@ 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); + #ifndef IS_HUMBLE + 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); + } + #else + cloud_pub_ = this->create_publisher( + "cloud_filtered", rclcpp::SensorDataQoS()); + 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 index 450848b..8dffa92 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,9 @@ class ScanToCloudFilterChain : public rclcpp::Node rclcpp::Publisher::SharedPtr cloud_pub_; // Parameters + #ifndef IS_HUMBLE + 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 index 1609ac1..f640cb0 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,16 @@ ScanToScanFilterChain::ScanToScanFilterChain( read_only_desc.read_only = true; // Declare parameters + #ifndef IS_HUMBLE + 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 + #ifndef IS_HUMBLE + 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,30 @@ ScanToScanFilterChain::ScanToScanFilterChain( std::placeholders::_1)); } - // Advertise output - output_pub_ = this->create_publisher("scan_filtered", 1000); + #ifndef IS_HUMBLE + 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); + } + #else + output_pub_ = this->create_publisher( + "scan_filtered", rclcpp::SensorDataQoS()); + 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 index a7501cb..5153706 100644 --- 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::SharedPtr output_pub_; // Parameters + #ifndef IS_HUMBLE + bool lazy_subscription_; + #endif std::string tf_message_filter_target_frame_; double tf_filter_tolerance_;