implemented angle wrap

.gitignore

@@ -1,2 +1,6 @@
 *~
 build
+install
+log
+.vscode
+**/__pycache__/

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
+            $<TARGET_FILE:${TEST_NAME}>
+            --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)

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",
+                ])
+            ],
+        )
+    ])

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 
+

include/laser_filters/angular_bounds_filter_in_place.h

@@ -38,16 +38,26 @@
 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H
 
 #include <filters/filter_base.hpp>
+#include <rclcpp/logging.hpp>
 #include <sensor_msgs/msg/laser_scan.hpp>
+#include <angles/angles.h>
 
 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<sensor_msgs::msg::LaserScan>
   {
     public:
       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<float>::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

test/test_angular_bounds_filter.cpp

@@ -0,0 +1,210 @@
+#include <gtest/gtest.h>
+#include <laser_filters/angular_bounds_filter_in_place.h>
+#include <rclcpp/parameter.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <pluginlib/class_loader.hpp>
+
+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<float> &actual, const std::vector<float> &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<float> expected = {
+        1.0f, 1.0f, 1.0f, 1.0f,
+        std::numeric_limits<float>::quiet_NaN(),
+        std::numeric_limits<float>::quiet_NaN(),
+        std::numeric_limits<float>::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<float> expected = {
+        std::numeric_limits<float>::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<float>::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<float> 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<float> 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<size_t>(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();
+}