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/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 diff --git a/include/laser_filters/angular_bounds_filter_in_place.h b/include/laser_filters/angular_bounds_filter_in_place.h index e202796b..1138c57e 100644 --- a/include/laser_filters/angular_bounds_filter_in_place.h +++ b/include/laser_filters/angular_bounds_filter_in_place.h @@ -38,9 +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 { @@ -48,6 +57,7 @@ namespace laser_filters double lower_angle_; double upper_angle_; bool replace_with_nan_; + bool wrap_angle_; bool configure() { @@ -59,40 +69,77 @@ 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; } 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 && wrap_angle_) + { + inside = (angle > lower_bound) || (angle < upper_bound); + } + else if (!wrapped && wrap_angle_) + { + inside = (angle > lower_bound) && (angle < upper_bound); + } + else + { + inside = (current_angle > lower_angle_) && (current_angle < upper_angle_); + } + + 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..bb2cee15 --- /dev/null +++ b/test/test_angular_bounds_filter.cpp @@ -0,0 +1,210 @@ +#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; + filter.wrap_angle_ = 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; + filter.wrap_angle_ = 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; + filter.wrap_angle_ = 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, + 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, 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; + + 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); +} + +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); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file