From 019dbae16be0553862bd001f1875232914a47704 Mon Sep 17 00:00:00 2001 From: kmhswimgirl Date: Wed, 1 Jul 2026 20:02:50 -0400 Subject: [PATCH 1/3] implemented angle wrap + tests --- .gitignore | 4 + CMakeLists.txt | 19 ++ .../angular_bounds_filter_in_place.h | 41 ++++- test/test_angular_bounds_filter.cpp | 171 ++++++++++++++++++ 4 files changed, 228 insertions(+), 7 deletions(-) create mode 100644 test/test_angular_bounds_filter.cpp diff --git a/.gitignore b/.gitignore index bdc5af04..14546dcc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,6 @@ *~ build +install +log +.vscode +**/__pycache__/ diff --git a/CMakeLists.txt b/CMakeLists.txt index e7638cd5..18972eae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -106,6 +106,25 @@ if(BUILD_TESTING) RESULT_FILE ${RESULT_FILENAME} ) + set(TEST_NAME test_angular_bounds_filter) + set(RESULT_FILENAME ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TEST_NAME}.gtest.xml) + ament_add_gtest_executable(${TEST_NAME} test/${TEST_NAME}.cpp) + target_link_libraries(${TEST_NAME} + laser_scan_filters + pluginlib::pluginlib + rclcpp::rclcpp + filters::filter_chain + ${sensor_msgs_TARGETS} + ) + ament_add_test( + ${TEST_NAME} + COMMAND + $ + --ros-args + --gtest_output=xml:${RESULT_FILENAME} + RESULT_FILE ${RESULT_FILENAME} + ) + find_package(launch_testing_ament_cmake) add_launch_test(test/test_polygon_filter.test.py) add_launch_test(test/test_speckle_filter.test.py) diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index e202796b..390e0012 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -39,6 +39,7 @@ #include #include +#include namespace laser_filters { @@ -70,29 +71,55 @@ namespace laser_filters virtual ~LaserScanAngularBoundsFilterInPlace(){} - bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& filtered_scan){ + bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& filtered_scan) + { filtered_scan = input_scan; //copy entire message + //normalize upper and lower bound angles + const double lower_bound = angles::normalize_angle(lower_angle_); + const double upper_bound = angles::normalize_angle(upper_angle_); + + const bool wrapped = lower_bound > upper_bound; + 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_)){ + for(unsigned int i = 0; i < input_scan.ranges.size(); ++i) + { + double angle = angles::normalize_angle(current_angle); + bool inside; + + if (wrapped) + { + inside = (angle > lower_bound) || (angle < upper_bound); + } + else + { + inside = (angle > lower_bound) && (angle < upper_bound); + } + + if(inside) + { filtered_scan.ranges[i] = replace_value; - if(i < filtered_scan.intensities.size()){ + if(i < filtered_scan.intensities.size()) + { filtered_scan.intensities[i] = 0.0; } count++; } + current_angle += input_scan.angle_increment; } - + if(logging_interface_) + { RCLCPP_DEBUG(logging_interface_->get_logger(), "Filtered out %u points from the laser scan.", count); + } return true; - } }; }; -#endif +#endif \ No newline at end of file diff --git a/test/test_angular_bounds_filter.cpp b/test/test_angular_bounds_filter.cpp new file mode 100644 index 00000000..f859e055 --- /dev/null +++ b/test/test_angular_bounds_filter.cpp @@ -0,0 +1,171 @@ +#include +#include +#include +#include +#include +#include + +using sensor_msgs::msg::LaserScan; + +static LaserScan makeScanMsg( + float angle_min, + float angle_increment, + int num_beams, + float range_value = 1.0f, + float intensity_value = 5.0f) +{ + LaserScan msg; + msg.header.frame_id = "laser"; + msg.angle_min = angle_min; + msg.angle_increment = angle_increment; + msg.angle_max = angle_min + angle_increment * (num_beams - 1); + msg.time_increment = 0.1; + msg.scan_time = 0.1; + msg.range_min = 0.5; + msg.range_max = 10.0; + + msg.ranges.assign(num_beams, range_value); + msg.intensities.assign(num_beams, intensity_value); + return msg; +} + +static void checkRangeVectors(const std::vector &actual, const std::vector &expected) +{ + ASSERT_EQ(actual.size(), expected.size()); + for (size_t i = 0; i < expected.size(); i++) + { + if(std::isnan(expected[i])) + { + EXPECT_TRUE(std::isnan(actual[i])) << "index " << i; + } + else + { + EXPECT_NEAR(actual[i], expected[i], 1e-6) << "index " << i; + } + } +} + +TEST(AngularBoundsFilterInPlace, NonWrappedRange) +{ + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + + filter.lower_angle_ = -0.15; + filter.upper_angle_ = 0.15; + filter.replace_with_nan_ = true; + + LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); + LaserScan output; + + ASSERT_TRUE(filter.update(input, output)); + + std::vector expected = { + 1.0f, 1.0f, 1.0f, 1.0f, + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), + 1.0f, 1.0f, 1.0f + }; + + checkRangeVectors(output.ranges, expected); + + EXPECT_FLOAT_EQ(output.intensities[4], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[5], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[6], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[0], 5.0f); + +} + +TEST(AngularBoundsFilterInPlace, WrappedRange) +{ + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + + filter.lower_angle_ = 2.9; + filter.upper_angle_ = -2.9; + filter.replace_with_nan_ = true; + + LaserScan input = makeScanMsg(-3.0f, 0.5f, 13); + LaserScan output; + + ASSERT_TRUE(filter.update(input, output)); + + std::vector expected = { + std::numeric_limits::quiet_NaN(), + 1.0f, 1.0f, 1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, + 1.0f, 1.0f, + std::numeric_limits::quiet_NaN() + }; + + checkRangeVectors(output.ranges, expected); + + EXPECT_FLOAT_EQ(output.intensities.front(), 0.0f); + EXPECT_FLOAT_EQ(output.intensities.back(), 0.0f); + EXPECT_FLOAT_EQ(output.intensities[1], 5.0f); +} + +TEST(AngularBoundsFilterInPlace, NonWrappedRangeReplaceWithMax) +{ + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + + filter.lower_angle_ = -0.15; + filter.upper_angle_ = 0.15; + filter.replace_with_nan_ = false; + + LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); + LaserScan output; + + ASSERT_TRUE(filter.update(input, output)); + + std::vector expected = { + 1.0f, 1.0f, 1.0f, 1.0f, + input.range_max + 1.0f, + input.range_max + 1.0f, + input.range_max + 1.0f, + 1.0f, 1.0f, 1.0f + }; + + checkRangeVectors(output.ranges, expected); + + EXPECT_FLOAT_EQ(output.intensities[4], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[5], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[6], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[0], 5.0f); +} + +TEST(AngularBoundsFilterInPlace, Boundaries) +{ + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + + filter.lower_angle_ = -0.15; + filter.upper_angle_ = 0.15; + filter.replace_with_nan_ = false; + + LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); + LaserScan output; + + ASSERT_TRUE(filter.update(input, output)); + + std::vector expected = { + 1.0f, 1.0f, 1.0f, 1.0f, + input.range_max + 1.0f, + input.range_max + 1.0f, + input.range_max + 1.0f, + 1.0f, 1.0f, 1.0f + }; + + checkRangeVectors(output.ranges, expected); + + EXPECT_FLOAT_EQ(output.intensities[4], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[5], 0.0f); + EXPECT_FLOAT_EQ(output.intensities[6], 0.0f); + + EXPECT_FLOAT_EQ(output.intensities[3], 5.0f); + EXPECT_FLOAT_EQ(output.intensities[7], 5.0f); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file From e87e137af0adc8696615536efc20e4e28460f813 Mon Sep 17 00:00:00 2001 From: kmhswimgirl Date: Sat, 1 Aug 2026 19:23:49 -0400 Subject: [PATCH 2/3] added wrap_angle parameter added parameter that toggles wrapping angle behavior in order to maintain backward compatibility --- .../angular_bounds_filter_in_place.h | 28 +++++++++++-- test/test_angular_bounds_filter.cpp | 41 ++++++++++++++++++- 2 files changed, 64 insertions(+), 5 deletions(-) diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index 390e0012..1138c57e 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -38,10 +38,18 @@ #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H #include +#include #include #include namespace laser_filters +/* +* Example: +* say upper angle = -3.0 and lower angle = 3.0 (both in radians). +* if wrap_angle is set to true, the beams directly behind the robot will be removed. +* if wrap_angle was false, no beams would be removed since the upper angle is not normalized. +*/ + { class LaserScanAngularBoundsFilterInPlace : public filters::FilterBase { @@ -49,6 +57,7 @@ namespace laser_filters double lower_angle_; double upper_angle_; bool replace_with_nan_; + bool wrap_angle_; bool configure() { @@ -60,11 +69,17 @@ namespace laser_filters RCLCPP_ERROR(logging_interface_->get_logger(), "Both the lower_angle and upper_angle parameters must be set to use this filter."); 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_); + + //toggle to allow for angle wrapping; defaults to false for backward compatibility. + //https://github.com/ros-perception/laser_filters/pull/261 + wrap_angle_ = false; + getParam("wrap_angle", wrap_angle_); + + RCLCPP_DEBUG(logging_interface_->get_logger(), "Angle wrap turned %s", wrap_angle_ ? "on" : "off"); return true; } @@ -91,15 +106,19 @@ namespace laser_filters { double angle = angles::normalize_angle(current_angle); bool inside; - - if (wrapped) + + if (wrapped && wrap_angle_) { inside = (angle > lower_bound) || (angle < upper_bound); } - else + else if (!wrapped && wrap_angle_) { inside = (angle > lower_bound) && (angle < upper_bound); } + else + { + inside = (current_angle > lower_angle_) && (current_angle < upper_angle_); + } if(inside) { @@ -113,6 +132,7 @@ namespace laser_filters current_angle += input_scan.angle_increment; } + if(logging_interface_) { RCLCPP_DEBUG(logging_interface_->get_logger(), "Filtered out %u points from the laser scan.", count); diff --git a/test/test_angular_bounds_filter.cpp b/test/test_angular_bounds_filter.cpp index f859e055..bb2cee15 100644 --- a/test/test_angular_bounds_filter.cpp +++ b/test/test_angular_bounds_filter.cpp @@ -52,6 +52,7 @@ TEST(AngularBoundsFilterInPlace, NonWrappedRange) filter.lower_angle_ = -0.15; filter.upper_angle_ = 0.15; filter.replace_with_nan_ = true; + filter.wrap_angle_ = true; LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); LaserScan output; @@ -82,6 +83,7 @@ TEST(AngularBoundsFilterInPlace, WrappedRange) filter.lower_angle_ = 2.9; filter.upper_angle_ = -2.9; filter.replace_with_nan_ = true; + filter.wrap_angle_ = true; LaserScan input = makeScanMsg(-3.0f, 0.5f, 13); LaserScan output; @@ -110,6 +112,7 @@ TEST(AngularBoundsFilterInPlace, NonWrappedRangeReplaceWithMax) filter.lower_angle_ = -0.15; filter.upper_angle_ = 0.15; filter.replace_with_nan_ = false; + filter.wrap_angle_ = true; LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); LaserScan output; @@ -132,13 +135,14 @@ TEST(AngularBoundsFilterInPlace, NonWrappedRangeReplaceWithMax) EXPECT_FLOAT_EQ(output.intensities[0], 5.0f); } -TEST(AngularBoundsFilterInPlace, Boundaries) +TEST(AngularBoundsFilterInPlace, WrapBoundaries) { laser_filters::LaserScanAngularBoundsFilterInPlace filter; filter.lower_angle_ = -0.15; filter.upper_angle_ = 0.15; filter.replace_with_nan_ = false; + filter.wrap_angle_ = true; LaserScan input = makeScanMsg(-0.5f, 0.1f, 10); LaserScan output; @@ -163,6 +167,41 @@ TEST(AngularBoundsFilterInPlace, Boundaries) EXPECT_FLOAT_EQ(output.intensities[7], 5.0f); } +TEST(AngularBoundsFilterInPlace, WrapAngleParamBehavior) +{ + LaserScan input = makeScanMsg(-3.1415f, 0.1f, 63); + const size_t idx = static_cast(std::round((3.05f - (-3.1415f)) / 0.1f)); // should end up being around 63 beams + + // wrapped interval (3.0 .. -3.0) should filter the sample near +3.05 + { + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + filter.lower_angle_ = 3.0; + filter.upper_angle_ = -3.0; + filter.replace_with_nan_ = true; + filter.wrap_angle_ = true; + + LaserScan output; + ASSERT_TRUE(filter.update(input, output)); + EXPECT_TRUE(std::isnan(output.ranges[idx])); + EXPECT_FLOAT_EQ(output.intensities[idx], 0.0f); + } + + // legacy (non-wrap) behavior; same numeric angles not filtered + { + laser_filters::LaserScanAngularBoundsFilterInPlace filter; + filter.lower_angle_ = 3.0; + filter.upper_angle_ = -3.0; + filter.replace_with_nan_ = true; + filter.wrap_angle_ = false; + + LaserScan output; + ASSERT_TRUE(filter.update(input, output)); + EXPECT_FALSE(std::isnan(output.ranges[idx])); + EXPECT_FLOAT_EQ(output.ranges[idx], 1.0f); // original range value from makeScanMsg + EXPECT_FLOAT_EQ(output.intensities[idx], 5.0f); + } +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); From 364f3291b70d00e72a6abb73f5619314fba123e0 Mon Sep 17 00:00:00 2001 From: kmhswimgirl Date: Sat, 1 Aug 2026 19:25:08 -0400 Subject: [PATCH 3/3] AngularFilterInPlace example added example yaml and launch file for AngularFilterInPlace since one did not exist --- .../angular_filter_in_place_example.launch.py | 19 +++++++++++++++++++ examples/angular_filter_in_place_example.yaml | 11 +++++++++++ 2 files changed, 30 insertions(+) create mode 100644 examples/angular_filter_in_place_example.launch.py create mode 100644 examples/angular_filter_in_place_example.yaml diff --git a/examples/angular_filter_in_place_example.launch.py b/examples/angular_filter_in_place_example.launch.py new file mode 100644 index 00000000..70cc1d0d --- /dev/null +++ b/examples/angular_filter_in_place_example.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + parameters=[ + PathJoinSubstitution([ + get_package_share_directory("laser_filters"), + "examples", "angular_filter_in_place_example.yaml", + ]) + ], + ) + ]) diff --git a/examples/angular_filter_in_place_example.yaml b/examples/angular_filter_in_place_example.yaml new file mode 100644 index 00000000..0df44368 --- /dev/null +++ b/examples/angular_filter_in_place_example.yaml @@ -0,0 +1,11 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: angle_in_place + type: laser_filters/LaserScanAngularBoundsFilterInPlace + params: + lower_angle: 3.0 + upper_angle: -3.0 + replace_with_nan: true + wrap_angle: true + \ No newline at end of file