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 1/3] 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_; From b87292c57f7cb8abfeca8d63c0b934bec54db698 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 25 Jul 2024 03:36:36 +0000 Subject: [PATCH 2/3] Check rclcpp version instead of ROS distribution Co-authored-by: Jonathan Binney --- CMakeLists.txt | 4 ++-- package.xml | 1 - src/scan_to_cloud_filter_chain.cpp | 6 +++--- src/scan_to_cloud_filter_chain.hpp | 2 +- src/scan_to_scan_filter_chain.cpp | 6 +++--- src/scan_to_scan_filter_chain.hpp | 2 +- 6 files changed, 10 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e8737c7..02eb589 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,8 +12,8 @@ ament_auto_find_build_dependencies() # Build ############################################################################## -if($ENV{ROS_DISTRO} STREQUAL "humble") - add_compile_definitions(IS_HUMBLE) +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) diff --git a/package.xml b/package.xml index 0789e83..6e095fc 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,6 @@ 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 f09bc89..d1df808 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -54,7 +54,7 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( read_only_desc.read_only = true; // Declare parameters - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS this->declare_parameter("lazy_subscription", false, read_only_desc); #endif this->declare_parameter("high_fidelity", false, read_only_desc); @@ -64,7 +64,7 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( this->declare_parameter("laser_max_range", DBL_MAX, read_only_desc); // Get parameters - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS this->get_parameter("lazy_subscription", lazy_subscription_); #endif this->get_parameter("high_fidelity", high_fidelity_); @@ -85,7 +85,7 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( this->get_node_timers_interface()); buffer_.setCreateTimerInterface(timer_interface); - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS if (lazy_subscription_) { rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = diff --git a/src/scan_to_cloud_filter_chain.hpp b/src/scan_to_cloud_filter_chain.hpp index 8dffa92..5440d43 100644 --- a/src/scan_to_cloud_filter_chain.hpp +++ b/src/scan_to_cloud_filter_chain.hpp @@ -70,7 +70,7 @@ class ScanToCloudFilterChain : public rclcpp::Node rclcpp::Publisher::SharedPtr cloud_pub_; // Parameters - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS bool lazy_subscription_; #endif bool high_fidelity_; // High fidelity (interpolating time across scan) diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index f640cb0..7324e1e 100644 --- a/src/scan_to_scan_filter_chain.cpp +++ b/src/scan_to_scan_filter_chain.cpp @@ -50,14 +50,14 @@ ScanToScanFilterChain::ScanToScanFilterChain( read_only_desc.read_only = true; // Declare parameters - #ifndef IS_HUMBLE + #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 - #ifndef IS_HUMBLE + #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_); @@ -85,7 +85,7 @@ ScanToScanFilterChain::ScanToScanFilterChain( std::placeholders::_1)); } - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS if (lazy_subscription_) { rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = diff --git a/src/scan_to_scan_filter_chain.hpp b/src/scan_to_scan_filter_chain.hpp index 5153706..05fa84b 100644 --- a/src/scan_to_scan_filter_chain.hpp +++ b/src/scan_to_scan_filter_chain.hpp @@ -58,7 +58,7 @@ class ScanToScanFilterChain : public rclcpp::Node rclcpp::Publisher::SharedPtr output_pub_; // Parameters - #ifndef IS_HUMBLE + #ifdef RCLCPP_SUPPORTS_MATCHED_CALLBACKS bool lazy_subscription_; #endif std::string tf_message_filter_target_frame_; From 2105037c8a55261965cb7150f967ed94cff8150a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Sat, 27 Jul 2024 20:53:32 +0000 Subject: [PATCH 3/3] Keep the old publisher QoS settings --- src/scan_to_cloud_filter_chain.cpp | 10 ++++------ src/scan_to_scan_filter_chain.cpp | 10 ++++------ 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index d1df808..d0cfe82 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -97,16 +97,14 @@ ScanToCloudFilterChain::ScanToCloudFilterChain( scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); } }; - cloud_pub_ = this->create_publisher("cloud_filtered", - rclcpp::SensorDataQoS(), pub_options); + cloud_pub_ = this->create_publisher( + "cloud_filtered", 10, pub_options); } else { - cloud_pub_ = this->create_publisher("cloud_filtered", - rclcpp::SensorDataQoS()); + cloud_pub_ = this->create_publisher("cloud_filtered", 10); scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); } #else - cloud_pub_ = this->create_publisher( - "cloud_filtered", rclcpp::SensorDataQoS()); + cloud_pub_ = this->create_publisher("cloud_filtered", 10); scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); #endif diff --git a/src/scan_to_scan_filter_chain.cpp b/src/scan_to_scan_filter_chain.cpp index 7324e1e..053a301 100644 --- a/src/scan_to_scan_filter_chain.cpp +++ b/src/scan_to_scan_filter_chain.cpp @@ -97,16 +97,14 @@ ScanToScanFilterChain::ScanToScanFilterChain( scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); } }; - output_pub_ = this->create_publisher("scan_filtered", - rclcpp::SensorDataQoS(), pub_options); + output_pub_ = this->create_publisher( + "scan_filtered", 1000, pub_options); } else { - output_pub_ = this->create_publisher("scan_filtered", - rclcpp::SensorDataQoS()); + output_pub_ = this->create_publisher("scan_filtered", 1000); scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); } #else - output_pub_ = this->create_publisher( - "scan_filtered", rclcpp::SensorDataQoS()); + output_pub_ = this->create_publisher("scan_filtered", 1000); scan_sub_.subscribe(this, "scan", rmw_qos_profile_sensor_data); #endif }