From 0f33136a21f41ce4f17e9eb0cf1f6d176e4f4f00 Mon Sep 17 00:00:00 2001 From: Yohan Belanger Date: Wed, 21 Aug 2024 15:15:28 -0400 Subject: [PATCH 1/3] using NaN instead of range_max+1 to remove scans --- include/laser_filters/angular_bounds_filter_in_place.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index cbdc1902..a2766007 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -72,7 +72,7 @@ namespace laser_filters //loop through the scan and remove ranges at angles between lower_angle_ and upper_angle_ for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){ if((current_angle > lower_angle_) && (current_angle < upper_angle_)){ - filtered_scan.ranges[i] = input_scan.range_max + 1.0; + filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); if(i < filtered_scan.intensities.size()){ filtered_scan.intensities[i] = 0.0; } From b1375fecc46c51056693714b4127534d91721054 Mon Sep 17 00:00:00 2001 From: Yohan Belanger Date: Wed, 21 Aug 2024 15:15:28 -0400 Subject: [PATCH 2/3] adding replace_with_nan param --- include/laser_filters/angular_bounds_filter_in_place.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index a2766007..07476572 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -47,6 +47,7 @@ namespace laser_filters public: double lower_angle_; double upper_angle_; + bool replace_with_nan_; bool configure() { @@ -59,6 +60,9 @@ namespace laser_filters return false; } + replace_with_nan_ = false; + getParam("replace_with_nan", replace_with_nan_); + return true; } @@ -69,10 +73,11 @@ namespace laser_filters double current_angle = input_scan.angle_min; unsigned int count = 0; + float replace_value = replace_with_nan_ ? std::numeric_limits::quiet_NaN() : input_scan.range_max + 1.0; //loop through the scan and remove ranges at angles between lower_angle_ and upper_angle_ for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){ if((current_angle > lower_angle_) && (current_angle < upper_angle_)){ - filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); + filtered_scan.ranges[i] = replace_value; if(i < filtered_scan.intensities.size()){ filtered_scan.intensities[i] = 0.0; } From f7236ba9b92f68aa08f4287fec8532957a158801 Mon Sep 17 00:00:00 2001 From: Yohan Belanger Date: Mon, 26 Aug 2024 09:38:10 -0400 Subject: [PATCH 3/3] added comment --- include/laser_filters/angular_bounds_filter_in_place.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index 07476572..e202796b 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -60,6 +60,8 @@ namespace laser_filters return false; } + //toggle to use NaN for filtering scans; defaults to false for backward compatibility. + //https://github.com/ros-perception/laser_filters/pull/202 replace_with_nan_ = false; getParam("replace_with_nan", replace_with_nan_);