From 62aa041eddbe27b9ee70d3ff4b019deb258f3f04 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 14:18:11 +0100 Subject: [PATCH 01/25] Port polygon filter to ROS2 --- CMakeLists.txt | 5 +- examples/polygon_filter_example.launch.py | 20 + examples/polygon_filter_example.yaml | 10 + include/laser_filters/polygon_filter.h | 613 ++++++++++++++++++++++ laser_filters_plugins.xml | 10 + package.xml | 1 + src/laser_scan_filters.cpp | 3 + test/test_polygon_filter.test.py | 97 ++++ test/test_polygon_filter.yaml | 10 + 9 files changed, 768 insertions(+), 1 deletion(-) create mode 100644 examples/polygon_filter_example.launch.py create mode 100644 examples/polygon_filter_example.yaml create mode 100644 include/laser_filters/polygon_filter.h create mode 100755 test/test_polygon_filter.test.py create mode 100644 test/test_polygon_filter.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 5fa17f07..cfe132e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,7 @@ endforeach() ############################################################################## pluginlib_export_plugin_description_file(filters laser_filters_plugins.xml) -ament_auto_package(INSTALL_TO_SHARE examples) +ament_auto_package(INSTALL_TO_SHARE examples test) ############################################################################## # Test @@ -62,4 +62,7 @@ if(BUILD_TESTING) --gtest_output=xml:${RESULT_FILENAME} RESULT_FILE ${RESULT_FILENAME} ) + + find_package(launch_testing_ament_cmake) + add_launch_test(test/test_polygon_filter.test.py) endif() diff --git a/examples/polygon_filter_example.launch.py b/examples/polygon_filter_example.launch.py new file mode 100644 index 00000000..4ccfbd3a --- /dev/null +++ b/examples/polygon_filter_example.launch.py @@ -0,0 +1,20 @@ +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", "polygon_filter_example.yaml"] + ) + ], + ) + ] + ) diff --git a/examples/polygon_filter_example.yaml b/examples/polygon_filter_example.yaml new file mode 100644 index 00000000..90bf7d9b --- /dev/null +++ b/examples/polygon_filter_example.yaml @@ -0,0 +1,10 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: polygon_filter + type: laser_filters/LaserScanPolygonFilter + params: + polygon_frame: base_link + polygon: '[[0.0, 0.0], [0.1, 0.1], [0.1, 0.0], [0.0, -0.1]]' + invert: false + footprint_topic: base_footprint_exclude diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h new file mode 100644 index 00000000..9a645c3c --- /dev/null +++ b/include/laser_filters/polygon_filter.h @@ -0,0 +1,613 @@ +/* + * Software License Agreement (BSD License) + * + * Robot Operating System code by Eurotec B.V. + * Copyright (c) 2020, Eurotec B.V. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above + * copyright notice, this list of conditions and the following + * disclaimer. + * + * 2. Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED + * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF + * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * + * polygon_filter.h + */ + +#ifndef POLYGON_FILTER_H +#define POLYGON_FILTER_H + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + + +typedef tf2::Vector3 Point; +using std::placeholders::_1; + +/** @brief Same as sign(x) but returns 0 if x is 0. */ +inline double sign0(double x) +{ + return x < 0.0 ? -1.0 : (x > 0.0 ? 1.0 : 0.0); +} + +void padPolygon(geometry_msgs::msg::Polygon& polygon, double padding) +{ + // pad polygon in place + for (unsigned int i = 0; i < polygon.points.size(); i++) + { + geometry_msgs::msg::Point32& pt = polygon.points[ i ]; + pt.x += sign0(pt.x) * padding; + pt.y += sign0(pt.y) * padding; + } +} + +std::vector > parseVVF(const std::string& input, std::string& error_return) +{ // Source: https://github.com/ros-planning/navigation/blob/melodic-devel/costmap_2d/src/array_parser.cpp + std::vector > result; + + std::stringstream input_ss(input); + int depth = 0; + std::vector current_vector; + while (!!input_ss && !input_ss.eof()) + { + switch (input_ss.peek()) + { + case EOF: + break; + case '[': + depth++; + if (depth > 2) + { + error_return = "Array depth greater than 2"; + return result; + } + input_ss.get(); + current_vector.clear(); + break; + case ']': + depth--; + if (depth < 0) + { + error_return = "More close ] than open ["; + return result; + } + input_ss.get(); + if (depth == 1) + { + result.push_back(current_vector); + } + break; + case ',': + case ' ': + case '\t': + input_ss.get(); + break; + default: // All other characters should be part of the numbers. + if (depth != 2) + { + std::stringstream err_ss; + err_ss << "Numbers at depth other than 2. Char was '" << char(input_ss.peek()) << "'."; + error_return = err_ss.str(); + return result; + } + float value; + input_ss >> value; + if (!!input_ss) + { + current_vector.push_back(value); + } + break; + } + } + + if (depth != 0) + { + error_return = "Unterminated vector string."; + } + else + { + error_return = ""; + } + + return result; +} + +geometry_msgs::msg::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::msg::Polygon& last_polygon) +{ + std::string error; + std::vector > vvf = parseVVF(polygon_string, error); + + if (error != "") + { + // RCLCPP_ERROR(logging_interface_->get_logger(), "Error parsing polygon parameter: '%s'", error.c_str()); + // RCLCPP_ERROR(logging_interface_->get_logger(), " Polygon string was '%s'.", polygon_string.c_str()); + return last_polygon; + } + + geometry_msgs::msg::Polygon polygon; + geometry_msgs::msg::Point32 point; + + // convert vvf into points. + if (vvf.size() < 3 && vvf.size() > 0) + { + // RCLCPP_WARN(logging_interface_->get_logger(), "You must specify at least three points for the robot polygon"); + return last_polygon; + } + + for (unsigned int i = 0; i < vvf.size(); i++) + { + if (vvf[ i ].size() == 2) + { + point.x = vvf[ i ][ 0 ]; + point.y = vvf[ i ][ 1 ]; + point.z = 0; + polygon.points.push_back(point); + } + else + { + // RCLCPP_ERROR(logging_interface_->get_logger(), "Points in the polygon specification must be pairs of numbers. Found a point with %d numbers.", + // int(vvf[ i ].size())); + return last_polygon; + } + } + + return polygon; +} + +std::string polygonToString(geometry_msgs::msg::Polygon polygon) +{ + std::string polygon_string = "["; + bool first = true; + for (auto point : polygon.points) { + if (!first) { + polygon_string += ", "; + } + first = false; + polygon_string += "[" + std::to_string(point.x) + ", " + std::to_string(point.y) + "]"; + } + polygon_string += "]"; + return polygon_string; +} + +using namespace std::literals; +namespace laser_filters +{ +/** + * @brief This is a filter that removes points in a laser scan inside of a polygon. + */ +class LaserScanPolygonFilterBase : public filters::FilterBase, public rclcpp_lifecycle::LifecycleNode { +public: + + LaserScanPolygonFilterBase() : rclcpp_lifecycle::LifecycleNode("laser_scan_polygon_filter"), buffer_(get_clock()), tf_(buffer_){}; + + virtual bool configure() + { + // dynamic reconfigure parameters callback: + on_set_parameters_callback_handle_ = add_on_set_parameters_callback( + std::bind(&LaserScanPolygonFilterBase::reconfigureCB, this, std::placeholders::_1)); + + std::string polygon_string; + invert_filter_ = false; + polygon_padding_ = 0; + std::string footprint_topic; + if(!filters::FilterBase::getParam(std::string("footprint_topic"), footprint_topic)) + { + RCLCPP_WARN(logging_interface_->get_logger(), "Footprint topic not set, assuming default: base_footprint_exclude"); + } + // Set default footprint topic + if(footprint_topic=="") + { + footprint_topic = "base_footprint_exclude"; + } + if (!filters::FilterBase::getParam(std::string("polygon"), polygon_string)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: PolygonFilter was not given polygon.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("polygon_frame"), polygon_frame_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: PolygonFilter was not given polygon_frame.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("invert"), invert_filter_)) + { + RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter invert filter not set, assuming false.\n"); + }if (!filters::FilterBase::getParam(std::string("polygon_padding"), polygon_padding_)) + { + RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter polygon_padding not set, assuming 0. \n"); + } + polygon_ = makePolygonFromString(polygon_string, polygon_); + padPolygon(polygon_, polygon_padding_); + + footprint_sub_ = create_subscription(footprint_topic, 1, std::bind(&LaserScanPolygonFilterBase::footprintCB, this, std::placeholders::_1)); + polygon_pub_ = create_publisher("polygon", rclcpp::QoS(1).transient_local().keep_last(1)); + is_polygon_published_ = false; + + return true; + } + + void footprintCB(const geometry_msgs::msg::Polygon::SharedPtr polygon) + { + if(polygon->points.size() < 3) + { + RCLCPP_WARN(logging_interface_->get_logger(), "Footprint needs at least three points for the robot polygon, ignoring message"); + return; + } + polygon_ = *polygon; + padPolygon(polygon_, polygon_padding_); + is_polygon_published_ = false; + } + virtual bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& output_scan) { return false; } + +protected: + rclcpp::Publisher::SharedPtr polygon_pub_; + rclcpp::Subscription::SharedPtr footprint_sub_; + boost::recursive_mutex own_mutex_; + // configuration + std::string polygon_frame_; + geometry_msgs::msg::Polygon polygon_; + double polygon_padding_; + bool invert_filter_; + bool is_polygon_published_ = false; + + + // tf listener to transform scans into the right frame + tf2_ros::Buffer buffer_; + tf2_ros::TransformListener tf_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; + virtual rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) + { + boost::recursive_mutex::scoped_lock lock(own_mutex_); + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (auto parameter : parameters) + { + if(parameter.get_name() == "polygon"&& parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING){ + std::string polygon_string = parameter.as_string(); + polygon_ = makePolygonFromString(polygon_string, polygon_); + } + else if(parameter.get_name() == "polygon_frame" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING){ + polygon_frame_ = parameter.as_string(); + } + else if(parameter.get_name() == "invert" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL){ + invert_filter_ = parameter.as_bool(); + } + else if(parameter.get_name() == "polygon_padding" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE){ + polygon_padding_ = parameter.as_double(); + } + else{ + RCLCPP_WARN(logging_interface_->get_logger(), "Unknown parameter"); + } + } + padPolygon(polygon_, polygon_padding_); + is_polygon_published_ = false; + return result; + } + + // checks if points in polygon + bool inPolygon(const Point& point) const + { + int i, j; + bool c = false; + + for (i = 0, j = polygon_.points.size() - 1; i < polygon_.points.size(); j = i++) + { + if ((polygon_.points.at(i).y > point.y() != (polygon_.points.at(j).y > point.y()) && + (point.x() < (polygon_.points[j].x - polygon_.points[i].x) * (point.y() - polygon_.points[i].y) / + (polygon_.points[j].y - polygon_.points[i].y) + + polygon_.points[i].x))) + c = !c; + } + return c; + } + + void publishPolygon() + { + if (!is_polygon_published_) + { + geometry_msgs::msg::PolygonStamped polygon_stamped; + polygon_stamped.header.frame_id = polygon_frame_; + polygon_stamped.header.stamp = get_clock()->now(); + polygon_stamped.polygon = polygon_; + polygon_pub_->publish(polygon_stamped); + is_polygon_published_ = true; + } + } +}; + +class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { +public: + bool configure() override + { + bool result = LaserScanPolygonFilterBase::configure(); + return result; + } + + bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& output_scan) override + { + auto start = std::chrono::high_resolution_clock::now(); + + boost::recursive_mutex::scoped_lock lock(own_mutex_); + + publishPolygon(); + + output_scan = input_scan; + + sensor_msgs::msg::PointCloud2 laser_cloud; + + std::string error_msg; + + bool success = buffer_.canTransform( + polygon_frame_, + input_scan.header.frame_id, + rclcpp::Time(input_scan.header.stamp) + std::chrono::duration(input_scan.ranges.size() * input_scan.time_increment), + 1.0s, + &error_msg + ); + if(!success){ + RCLCPP_WARN(logging_interface_->get_logger(), "Could not get transform, irgnoring laser scan! %s", error_msg.c_str()); + return false; + } + + try{ + projector_.transformLaserScanToPointCloud(polygon_frame_, input_scan, laser_cloud, buffer_); + } + catch(tf2::TransformException& ex){ + RCLCPP_INFO_THROTTLE(logging_interface_->get_logger(), *get_clock(), 300, "Ignoring Scan: Waiting for TF"); + return false; + } + + const int i_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "index"); + const int x_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "x"); + const int y_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "y"); + const int z_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "z"); + + if (i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1) + { + RCLCPP_INFO_THROTTLE(logging_interface_->get_logger(), *get_clock(), 300, "x, y, z and index fields are required, skipping scan"); + } + + const int i_idx_offset = laser_cloud.fields[i_idx_c].offset; + const int x_idx_offset = laser_cloud.fields[x_idx_c].offset; + const int y_idx_offset = laser_cloud.fields[y_idx_c].offset; + const int z_idx_offset = laser_cloud.fields[z_idx_c].offset; + + const int pstep = laser_cloud.point_step; + const long int pcount = laser_cloud.width * laser_cloud.height; + const long int limit = pstep * pcount; + + int i_idx, x_idx, y_idx, z_idx; + for (i_idx = i_idx_offset, x_idx = x_idx_offset, y_idx = y_idx_offset, z_idx = z_idx_offset; + + x_idx < limit; + + i_idx += pstep, x_idx += pstep, y_idx += pstep, z_idx += pstep) + { + // TODO works only for float data types and with an index field + // I'm working on it, see https://github.com/ros/common_msgs/pull/78 + float x = *((float*)(&laser_cloud.data[x_idx])); + float y = *((float*)(&laser_cloud.data[y_idx])); + float z = *((float*)(&laser_cloud.data[z_idx])); + int index = *((int*)(&laser_cloud.data[i_idx])); + + Point point(x, y, z); + + if (!invert_filter_) + { + if (inPolygon(point)) + { + output_scan.ranges[index] = std::numeric_limits::quiet_NaN(); + } + } + else + { + if (!inPolygon(point)) + { + output_scan.ranges[index] = std::numeric_limits::quiet_NaN(); + } + } + } + + auto end = std::chrono::high_resolution_clock::now(); + auto update_elapsed = std::chrono::duration_cast(end - start).count(); + + RCLCPP_DEBUG(logging_interface_->get_logger(), "LaserScanPolygonFilter update took %lu microseconds", update_elapsed); + + return true; + } + +private: + laser_geometry::LaserProjection projector_; +}; + +/** + * @brief This is a filter that removes points in a laser scan inside of a polygon. + * It assumes that the transform between the scanner and the robot base remains unchanged, + * i.e. the position and orientation of the laser filter should not change. + * A typical use case for this filter is to filter out parts of the robot body or load that it may carry. + */ +class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { +public: + bool configure() override + { + bool result = LaserScanPolygonFilterBase::configure(); + is_polygon_transformed_ = false; + transform_timeout_ = 5; // Default + if (!filters::FilterBase::getParam(std::string("transform_timeout"), transform_timeout_)) + { + RCLCPP_INFO(logging_interface_->get_logger(), "Error: PolygonFilter transform_timeout not set, assuming 5. \n"); + } + return result; + } + + bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& output_scan) override + { + boost::recursive_mutex::scoped_lock lock(own_mutex_); + publishPolygon(); + + if (!is_polygon_transformed_) + { + if (!transformPolygon(input_scan.header.frame_id)) return false; + } + + output_scan = input_scan; + checkCoSineMap(input_scan); + + size_t i = 0; + size_t i_max = input_scan.ranges.size(); + + while (i < i_max) + { + float range = input_scan.ranges[i]; + + float x = co_sine_map_(i, 0) * range; + float y = co_sine_map_(i, 1) * range; + Point point(x, y, 0); + + if (invert_filter_ != inPolygon(point)) + { + output_scan.ranges[i] = std::numeric_limits::quiet_NaN(); + } + + ++i; + } + + return true; + } + + void footprintCB(const geometry_msgs::msg::Polygon::SharedPtr polygon) + { + is_polygon_transformed_ = false; + LaserScanPolygonFilterBase::footprintCB(polygon); + } + +protected: + bool transformPolygon(const std::string &input_scan_frame_id) + { + std::string error_msg; + RCLCPP_DEBUG(logging_interface_->get_logger(), + "waitForTransform %s -> %s", + polygon_frame_.c_str(), input_scan_frame_id.c_str() + ); + + geometry_msgs::msg::TransformStamped transform; + try + { + transform = buffer_.lookupTransform(input_scan_frame_id, + polygon_frame_, + tf2::TimePointZero, + tf2::durationFromSec(transform_timeout_)); + } + catch(tf2::TransformException& ex) + { + RCLCPP_WARN_THROTTLE(logging_interface_->get_logger(), + *get_clock(), 1000, + "StaticLaserScanPolygonFilter", + "Could not get transform, ignoring laser scan! %s", ex.what()); + return false; + } + + RCLCPP_INFO(logging_interface_->get_logger(), "Obtained transform"); + for (int i = 0; i < polygon_.points.size(); ++i) + { + geometry_msgs::msg::PointStamped point_in = createPointStamped(polygon_.points[i].x, polygon_.points[i].y, 0, transform.header.stamp, polygon_frame_); + geometry_msgs::msg::PointStamped point_out; + tf2::doTransform(point_in, point_out, transform); + polygon_.points[i].x = point_out.point.x; + polygon_.points[i].y = point_out.point.y; + } + is_polygon_transformed_ = true; + return true; + } + + rcl_interfaces::msg::SetParametersResult reconfigureCB(std::vector parameters) override + { + is_polygon_transformed_ = false; + return LaserScanPolygonFilterBase::reconfigureCB(parameters); + } + +private: + double transform_timeout_; + Eigen::ArrayXXd co_sine_map_; + float co_sine_map_angle_min_; + float co_sine_map_angle_max_; + bool is_polygon_transformed_; + + void checkCoSineMap(const sensor_msgs::msg::LaserScan& scan_in) + { + size_t n_pts = scan_in.ranges.size(); + + if ( + co_sine_map_.rows() != (int)n_pts || + co_sine_map_angle_min_ != scan_in.angle_min || + co_sine_map_angle_max_ != scan_in.angle_max + ) { + RCLCPP_DEBUG(logging_interface_->get_logger(), "No precomputed map given. Computing one."); + co_sine_map_ = Eigen::ArrayXXd(n_pts, 2); + co_sine_map_angle_min_ = scan_in.angle_min; + co_sine_map_angle_max_ = scan_in.angle_max; + + // Spherical->Cartesian projection + for (size_t i = 0; i < n_pts; ++i) + { + co_sine_map_(i, 0) = cos(scan_in.angle_min + (double) i * scan_in.angle_increment); + co_sine_map_(i, 1) = sin(scan_in.angle_min + (double) i * scan_in.angle_increment); + } + } + } + geometry_msgs::msg::PointStamped createPointStamped(const double &x, + const double &y, + const double &z, + const builtin_interfaces::msg::Time &stamp, + const std::string &frame_id) + { + geometry_msgs::msg::PointStamped point; + point.point.x = x; + point.point.y = y; + point.point.z = z; + point.header.stamp = stamp; + point.header.frame_id = frame_id; + return point; + } +}; +} +#endif /* polygon_filter.h */ diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 71f0be58..180ba06c 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -78,5 +78,15 @@ DEPRECATED: This is a filter which filters points out of a laser scan which are inside the inscribed radius. + + This is a filter that removes points in a laser scan inside of a polygon. + + + + This is a filter that removes points in a laser scan inside of a polygon static relative to robot base. + + diff --git a/package.xml b/package.xml index 6983876f..444ebec0 100644 --- a/package.xml +++ b/package.xml @@ -23,6 +23,7 @@ sensor_msgs tf2 tf2_ros + tf2_kdl ament_cmake_gtest diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 485d3c14..6069b30b 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -39,6 +39,7 @@ #include "laser_filters/angular_bounds_filter_in_place.h" #include "laser_filters/box_filter.h" #include "laser_filters/speckle_filter.h" +#include "laser_filters/polygon_filter.h" #include @@ -57,3 +58,5 @@ PLUGINLIB_EXPORT_CLASS(laser_filters::InterpolationFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMaskFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSpeckleFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::StaticLaserScanPolygonFilter, filters::FilterBase) \ No newline at end of file diff --git a/test/test_polygon_filter.test.py b/test/test_polygon_filter.test.py new file mode 100755 index 00000000..838c6eab --- /dev/null +++ b/test/test_polygon_filter.test.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020, Eurotec, Netherlands +# All rights reserved. +# +# \author Rein Appeldoorn + +from threading import Thread, Event +import math +import unittest +import launch +import launch.actions +import launch.substitutions +import launch_testing +import launch_ros.actions +import os +from ament_index_python.packages import get_package_share_directory +import pytest +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan + + +@pytest.mark.launch_test +def generate_test_description(): + config = os.path.join(get_package_share_directory("laser_filters"), "test", "test_polygon_filter.yaml") + + node = launch_ros.actions.Node(package="laser_filters", executable="scan_to_scan_filter_chain", parameters=[config]) + return launch.LaunchDescription([node, launch_testing.actions.ReadyToTest()]) + + +class TestPolygonFilter(unittest.TestCase): + def test_polygon_filter(self): + rclpy.init() + node = TestFixture() + self.assertTrue(node.wait_for_subscriber(10)) + node.start_subscriber() + node.publish_laser_scan() + + msgs_received_flag = node._msg_event_object.wait(timeout=10.0) + assert msgs_received_flag, "Did not receive msgs !" + expected_scan_ranges = [1.0, 1.0, 1.0, 1.0, float("nan"), float("nan"), float("nan"), 1, 1, 1, 1] + for scan_range, expected_scan_range in zip(node._received_message.ranges, expected_scan_ranges): + if math.isnan(expected_scan_range) or math.isnan(scan_range): + self.assertEqual( + math.isnan(expected_scan_range), + math.isnan(scan_range), + "failed %f and %f" % (expected_scan_range, scan_range), + ) + pass + else: + self.assertEqual(scan_range, expected_scan_range) + node.destroy_node() + rclpy.shutdown() + + +class TestFixture(Node): + def __init__(self): + super().__init__("test_polygon_filter") + self._msg_event_object = Event() + self._publisher = self.create_publisher(LaserScan, "scan", 10) + + def wait_for_subscriber(self, timeout): + timer_period = 0.1 + t = 0.0 + rate = self.create_rate(1 / timer_period, self.get_clock()) + while rclpy.ok() and t < timeout: + rclpy.spin_once(self) + if self._publisher.get_subscription_count() > 0: + return True + rate.sleep() + t += timer_period + return False + + def publish_laser_scan(self): + num_beams = 11 + msg = LaserScan() + msg.header.frame_id = "laser" + msg.header.stamp = self.get_clock().now().to_msg() + msg.angle_min = -math.pi / 2 + msg.angle_max = math.pi / 2 + msg.angle_increment = math.pi / (num_beams - 1) + msg.ranges = [1.0] * num_beams + msg.range_max = 100.0 + self._publisher.publish(msg) + + def start_subscriber(self): + # Create a subscriber + self.subscription = self.create_subscription(LaserScan, "scan_filtered", self.callback, 10) + + # Add a spin thread + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def callback(self, message): + self._msg_event_object.set() + self._received_message = message diff --git a/test/test_polygon_filter.yaml b/test/test_polygon_filter.yaml new file mode 100644 index 00000000..d6fc64ff --- /dev/null +++ b/test/test_polygon_filter.yaml @@ -0,0 +1,10 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: polygon_filter + type: laser_filters/LaserScanPolygonFilter + params: + polygon_frame: laser + polygon: '[[0.0, 0.5], [2.0, 0.5], [2.0, -0.5], [0.0, -0.5]]' + invert: false + footprint_topic: base_footprint_exclude From 84b7bbeac6c7c61e0eea4ed4faedf7f99e9b5d96 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 14:28:20 +0100 Subject: [PATCH 02/25] Port scan blob filter to ROS2 --- examples/scan_blob_filter_example.launch.py | 20 +++ examples/scan_blob_filter_example.yaml | 8 + include/laser_filters/scan_blob_filter.h | 158 ++++++++++++++++++++ laser_filters_plugins.xml | 4 + src/laser_scan_filters.cpp | 2 + 5 files changed, 192 insertions(+) create mode 100644 examples/scan_blob_filter_example.launch.py create mode 100644 examples/scan_blob_filter_example.yaml create mode 100644 include/laser_filters/scan_blob_filter.h diff --git a/examples/scan_blob_filter_example.launch.py b/examples/scan_blob_filter_example.launch.py new file mode 100644 index 00000000..deb5369c --- /dev/null +++ b/examples/scan_blob_filter_example.launch.py @@ -0,0 +1,20 @@ +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", "scan_blob_filter_example.yaml"] + ) + ], + ) + ] + ) diff --git a/examples/scan_blob_filter_example.yaml b/examples/scan_blob_filter_example.yaml new file mode 100644 index 00000000..eb9f4abb --- /dev/null +++ b/examples/scan_blob_filter_example.yaml @@ -0,0 +1,8 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: scan_blob_filter + type: laser_filters/ScanBlobFilter + params: + max_radius: 0.25 # maximum radius to be considered as blob object + min_points: 6 # min scan points to be considered as blob object diff --git a/include/laser_filters/scan_blob_filter.h b/include/laser_filters/scan_blob_filter.h new file mode 100644 index 00000000..149640e5 --- /dev/null +++ b/include/laser_filters/scan_blob_filter.h @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2013 Kei Okada + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id$ + * + */ + +/* + \author Kei OKada + + +*/ + +#ifndef LASER_SCAN_BLOB_FILTER_H +#define LASER_SCAN_BLOB_FILTER_H + +#include + +#include +#include +#include "angles/angles.h" + +namespace laser_filters{ + +/** @b ScanBlobFilter is a simple filter that filters shadow points in a laser scan line + */ + +class ScanBlobFilter : public filters::FilterBase +{ +public: + + double max_radius_; // Filter angle threshold + int min_points_; + + + //////////////////////////////////////////////////////////////////////////////// + ScanBlobFilter () + { + + + } + + /**@b Configure the filter from XML */ + bool configure() + { + max_radius_ = 0.1;//default value + if (!filters::FilterBase::getParam(std::string("max_radius"), max_radius_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: BlobFilter was not given min_radius.\n"); + return false; + } + + min_points_ = 5;//default value + if (!filters::FilterBase::getParam(std::string("min_points"), min_points_)) + { + RCLCPP_INFO(logging_interface_->get_logger(), "Error: BlobFilter was not given min_points.\n"); + return false; + } + return true; + } + + //////////////////////////////////////////////////////////////////////////////// + virtual ~ScanBlobFilter () { } + + //////////////////////////////////////////////////////////////////////////////// + /** \brief + * \param scan_in the input LaserScan message + * \param scan_out the output LaserScan message + */ + bool update(const sensor_msgs::msg::LaserScan& scan_in, sensor_msgs::msg::LaserScan& scan_out) + { + //copy across all data first + scan_out = scan_in; + + std::set indices_to_publish; + // assume that all points is pass thorugh shadow filter, so each blob is separeted by invalide scan data + std::vector > range_blobs; + std::vector range_blob; + for (unsigned int i = 0; i < scan_in.ranges.size (); i++) + { + scan_out.ranges[i] = -1.0 * fabs(scan_in.ranges[i]); // set all ranges to invalid (*) + if ( scan_in.ranges[i] < 0 || std::isnan(scan_in.ranges[i])) { + if ( range_blob.size() > min_points_ ) { + range_blobs.push_back(range_blob); + } + range_blob.clear(); + }else{ + range_blob.push_back(i); + } + } + if ( range_blob.size() > min_points_ ) { + range_blobs.push_back(range_blob); + } + // for each blob calculate center and radius + for (unsigned int i = 0; i < range_blobs.size(); i++) { + int size = range_blobs[i].size(); + // check center of blob + double center_x = 0, center_y = 0; + for (unsigned int j = 0; j < size; j++) { + double x = scan_in.ranges[range_blobs[i][j]]; + double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; + center_x += x; + center_y += y; + } + center_x /= size; + center_y /= size; + + // check range of blob + double radius = 0; + for (unsigned int j = 0; j < size; j++) { + double x = scan_in.ranges[range_blobs[i][j]]; + double y = scan_in.ranges[range_blobs[i][j]] * scan_in.angle_increment; + if ( radius < fabs(center_x - x) ) radius = fabs(center_x - x) ; + if ( radius < fabs(center_y - y) ) radius = fabs(center_y - y) ; + } + + RCLCPP_DEBUG_STREAM(logging_interface_->get_logger(), "blob center " << center_x << " " << center_y << ", radius " << radius << ", num of ponits " << size); + if ( radius < max_radius_ ) { + indices_to_publish.insert(range_blobs[i][0] + size/2); + } + } + RCLCPP_DEBUG(logging_interface_->get_logger(), "ScanBlobFilter %d Points from scan with min radius: %.2f, num of pints: %d", (int)indices_to_publish.size(), max_radius_, min_points_); + for ( std::set::iterator it = indices_to_publish.begin(); it != indices_to_publish.end(); ++it) + { + scan_out.ranges[*it] = fabs(scan_in.ranges[*it]); // valid only the ranges that passwd the test (*) + } + return true; + } + + //////////////////////////////////////////////////////////////////////////////// + +} ; +} + +#endif //LASER_SCAN_BLOB_FILTER_H diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 180ba06c..859b0fd2 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -78,6 +78,10 @@ DEPRECATED: This is a filter which filters points out of a laser scan which are inside the inscribed radius. + + This is a filter which extract blob object (human's foot, chair's foot) from a laser. + This is a filter that removes points in a laser scan inside of a polygon. diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 6069b30b..677c8c57 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -39,6 +39,7 @@ #include "laser_filters/angular_bounds_filter_in_place.h" #include "laser_filters/box_filter.h" #include "laser_filters/speckle_filter.h" +#include "laser_filters/scan_blob_filter.h" #include "laser_filters/polygon_filter.h" #include @@ -58,5 +59,6 @@ PLUGINLIB_EXPORT_CLASS(laser_filters::InterpolationFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMaskFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSpeckleFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::ScanBlobFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::StaticLaserScanPolygonFilter, filters::FilterBase) \ No newline at end of file From 7339c23f257a8131cd4c7d285d46ebffba614cf3 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 14:35:27 +0100 Subject: [PATCH 03/25] Port sector filter to ROS2 --- examples/sector_filter_example.launch.py | 20 ++++ examples/sector_filter_example.yaml | 12 ++ include/laser_filters/sector_filter.h | 142 +++++++++++++++++++++++ laser_filters_plugins.xml | 4 + src/laser_scan_filters.cpp | 2 + 5 files changed, 180 insertions(+) create mode 100644 examples/sector_filter_example.launch.py create mode 100644 examples/sector_filter_example.yaml create mode 100644 include/laser_filters/sector_filter.h diff --git a/examples/sector_filter_example.launch.py b/examples/sector_filter_example.launch.py new file mode 100644 index 00000000..f365705e --- /dev/null +++ b/examples/sector_filter_example.launch.py @@ -0,0 +1,20 @@ +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", "sector_filter_example.yaml"] + ) + ], + ) + ] + ) diff --git a/examples/sector_filter_example.yaml b/examples/sector_filter_example.yaml new file mode 100644 index 00000000..e278235e --- /dev/null +++ b/examples/sector_filter_example.yaml @@ -0,0 +1,12 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: scan_filter + type: laser_filters/LaserScanSectorFilter + params: + angle_min: 2.54 # if not specified defaults to 0.0 + angle_max: -2.54 # if not specified defaults to 0.0 + range_min: 0.2 # if not specified defaults to 0.0 + range_max: 2.0 # if not specified defaults to 100000.0 + clear_inside: true # if not specified defaults to true + invert: false # (!clear_inside) if not specified defaults to false diff --git a/include/laser_filters/sector_filter.h b/include/laser_filters/sector_filter.h new file mode 100644 index 00000000..e3ddda96 --- /dev/null +++ b/include/laser_filters/sector_filter.h @@ -0,0 +1,142 @@ +/********************************************************************* +* BSD 2-Clause License +* +* Copyright (c) 2021, Jimmy F. Klarke +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions are met: +* +* 1. Redistributions of source code must retain the above copyright notice, +* this list of conditions and the following disclaimer. +* +* 2. Redistributions in binary form must reproduce the above copyright notice, +* this list of conditions and the following disclaimer in the documentation +* and/or other materials provided with the distribution. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +* +* \author: Jimmy F. Klarke +*********************************************************************/ + +#ifndef LASER_SCAN_SECTOR_FILTER_IN_PLACE_H +#define LASER_SCAN_SECTOR_FILTER_IN_PLACE_H + +#include +#include + +namespace laser_filters +{ + +class LaserScanSectorFilter : public filters::FilterBase +{ +public: + LaserScanSectorFilter(){} + + bool configure() + { + if (!filters::FilterBase::getParam(std::string("angle_min"), angle_min_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given angle_min.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("angle_max"), angle_max_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given angle_max.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("range_min"), range_min_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given range_min.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("range_max"), range_max_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given range_max.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("clear_inside"), clear_inside_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given clear_inside.\n"); + return false; + }if (!filters::FilterBase::getParam(std::string("invert"), invert_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: LaserScanSectorFilter was not given invert.\n"); + return false; + } + + RCLCPP_DEBUG(logging_interface_->get_logger(), "clear_inside(!invert): %s", (isClearInside() ? "true" : "false")); + return true; + } + + bool isClearInside() + { + return invert_ ? false : clear_inside_; + } + + bool update(const sensor_msgs::msg::LaserScan& input_scan, sensor_msgs::msg::LaserScan& output_scan) + { + output_scan = input_scan; //copy entire message + bool clear_inside = isClearInside(); + + double angle_delta = angle_max_ - angle_min_; + if (angle_max_ < angle_min_) + { + angle_delta += M_PI * 2; + } + + double current_angle = input_scan.angle_min; + unsigned int count = 0; + //loop through the scan and remove ranges at angles between lower_angle_ and upper_angle_ + for (size_t i = 0; i < input_scan.ranges.size(); ++i) + { + current_angle = (i == 0) ? current_angle : (current_angle + input_scan.angle_increment); + + double current_range = input_scan.ranges[i]; + double current_angle_delta = current_angle - angle_min_; + if ((angle_max_ < angle_min_) && (current_angle_delta < 0)) + { + current_angle_delta += M_PI * 2; + } + + if (clear_inside != ((current_angle_delta > 0) + && (current_angle_delta < angle_delta) + && (current_range > range_min_) + && (current_range < range_max_))) + { + continue; + } + + output_scan.ranges[i] = input_scan.range_max + 1.0; + if (i < output_scan.intensities.size()) + { + output_scan.intensities[i] = 0.0; + } + count++; + } + + RCLCPP_DEBUG(logging_interface_->get_logger(), "Filtered out %u points from the laser scan.", count); + + return true; + } + + virtual ~LaserScanSectorFilter(){} + +private: + double angle_min_; + double angle_max_; + double range_min_; + double range_max_; + bool clear_inside_; + bool invert_; +}; + +} // end namespace laser_filters + +#endif // LASER_SCAN_SECTOR_FILTER_IN_PLACE_H diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 859b0fd2..82061248 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -82,6 +82,10 @@ base_class_type="filters::FilterBase<sensor_msgs::msg::LaserScan>"> This is a filter which extract blob object (human's foot, chair's foot) from a laser. + + This is a filter that removes points in a laser scan inside/outside of a circle sector. + This is a filter that removes points in a laser scan inside of a polygon. diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 677c8c57..c7f1a075 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -40,6 +40,7 @@ #include "laser_filters/box_filter.h" #include "laser_filters/speckle_filter.h" #include "laser_filters/scan_blob_filter.h" +#include "laser_filters/sector_filter.h" #include "laser_filters/polygon_filter.h" #include @@ -60,5 +61,6 @@ PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanBoxFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSpeckleFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::ScanBlobFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSectorFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::StaticLaserScanPolygonFilter, filters::FilterBase) \ No newline at end of file From 2e9bb30adb1f15b138550db678f17db6f7dd1fae Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 14:42:54 +0100 Subject: [PATCH 04/25] Removed commented print statements --- include/laser_filters/polygon_filter.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index 9a645c3c..d46dfd33 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -158,8 +158,6 @@ geometry_msgs::msg::Polygon makePolygonFromString(const std::string& polygon_str if (error != "") { - // RCLCPP_ERROR(logging_interface_->get_logger(), "Error parsing polygon parameter: '%s'", error.c_str()); - // RCLCPP_ERROR(logging_interface_->get_logger(), " Polygon string was '%s'.", polygon_string.c_str()); return last_polygon; } @@ -169,7 +167,6 @@ geometry_msgs::msg::Polygon makePolygonFromString(const std::string& polygon_str // convert vvf into points. if (vvf.size() < 3 && vvf.size() > 0) { - // RCLCPP_WARN(logging_interface_->get_logger(), "You must specify at least three points for the robot polygon"); return last_polygon; } @@ -184,8 +181,6 @@ geometry_msgs::msg::Polygon makePolygonFromString(const std::string& polygon_str } else { - // RCLCPP_ERROR(logging_interface_->get_logger(), "Points in the polygon specification must be pairs of numbers. Found a point with %d numbers.", - // int(vvf[ i ].size())); return last_polygon; } } From 53ae9b48efcae93cd50a3e5da4427632ae6494ac Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 14:43:55 +0100 Subject: [PATCH 05/25] Port speckle filter launch tests to ROS2 --- CMakeLists.txt | 1 + test/test_speckle_filter | 78 -------------- test/test_speckle_filter.launch | 13 --- test/test_speckle_filter.test.py | 137 ++++++++++++++++++++++++ test/test_speckle_filter.yaml | 21 ++++ test/test_speckle_filter_distance.yaml | 8 -- test/test_speckle_filter_euclidean.yaml | 8 -- 7 files changed, 159 insertions(+), 107 deletions(-) delete mode 100755 test/test_speckle_filter delete mode 100644 test/test_speckle_filter.launch create mode 100755 test/test_speckle_filter.test.py create mode 100644 test/test_speckle_filter.yaml delete mode 100644 test/test_speckle_filter_distance.yaml delete mode 100644 test/test_speckle_filter_euclidean.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index cfe132e8..7e5b4762 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,4 +65,5 @@ if(BUILD_TESTING) find_package(launch_testing_ament_cmake) add_launch_test(test/test_polygon_filter.test.py) + add_launch_test(test/test_speckle_filter.test.py) endif() diff --git a/test/test_speckle_filter b/test/test_speckle_filter deleted file mode 100755 index 641904ed..00000000 --- a/test/test_speckle_filter +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python -# -# Copyright (c) 2020, Eurotec, Netherlands -# All rights reserved. -# -# \author Rein Appeldoorn -# Nicolas Limpert - -import math -import unittest - -import rospy -import rostest -from sensor_msgs.msg import LaserScan -from std_msgs.msg import Header - - -class TestSpeckleFilter(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(TestSpeckleFilter, self).__init__(*args, **kwargs) - self.msg_euclid = 0 - self.msg_dist = 0 - - rospy.init_node('test_speckle_filter_distance') - num_beams = 11 - rospy.Publisher("scan", LaserScan, queue_size=1, latch=True).publish(LaserScan( - header=Header( - frame_id="laser", - stamp=rospy.Time.now() - ), - angle_min=-math.pi / 2, - angle_max=math.pi / 2, - angle_increment=math.pi / (num_beams - 1), - ranges=[1, 1, 1, 1, 0.5, 1, 1, 1, 1, 1, 1], - range_max=100 - )) - - def dist_cb(self, msg): - self.msg_dist = msg - print ("received dist") - print (self.msg_dist) - - def euclid_cb(self, msg): - self.msg_euclid = msg - print ("received euclid") - print (self.msg_euclid) - - def test_speckle_filter(self): - rospy.Subscriber("scan_filtered_distance", LaserScan, self.dist_cb) - rospy.Subscriber("scan_filtered_euclidean", LaserScan, self.euclid_cb) - - now = rospy.Time.now() - while (self.msg_euclid == 0 or self.msg_dist == 0): - rospy.sleep(0.1) - - if (rospy.Time.now() - now) > rospy.Duration(10): - print ("Error: did not receive messages within 10 seconds") - self.assert_(False, "Error: did not receive messages within 10 seconds") - - expected_scan_ranges = [1, 1, 1, 1, float('nan'), 1, 1, 1, 1, 1, 1] - for scan_range, expected_scan_range in zip(self.msg_dist.ranges, expected_scan_ranges): - if math.isnan(expected_scan_range) or math.isnan(scan_range): - self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range)) - else: - self.assertEqual(scan_range, expected_scan_range) - - expected_scan_ranges = [1, 1, 1, 1, float('nan'), 1, 1, 1, 1, 1, 1] - for scan_range, expected_scan_range in zip(self.msg_euclid.ranges, expected_scan_ranges): - if math.isnan(expected_scan_range) or math.isnan(scan_range): - self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range)) - else: - self.assertEqual(scan_range, expected_scan_range) - - -PKG = 'laser_filters' -NAME = 'test_speckle_filter' -if __name__ == '__main__': - rostest.unitrun(PKG, NAME, TestSpeckleFilter) diff --git a/test/test_speckle_filter.launch b/test/test_speckle_filter.launch deleted file mode 100644 index 4fb5bdfd..00000000 --- a/test/test_speckle_filter.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/test/test_speckle_filter.test.py b/test/test_speckle_filter.test.py new file mode 100755 index 00000000..e14c2e55 --- /dev/null +++ b/test/test_speckle_filter.test.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020, Eurotec, Netherlands +# All rights reserved. +# +# \author Rein Appeldoorn + +from threading import Thread, Event +import math +import unittest +import launch +import launch.actions +import launch.substitutions +import launch_testing +import launch_ros.actions +import os +from ament_index_python.packages import get_package_share_directory +import pytest +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan + + +@pytest.mark.launch_test +def generate_test_description(): + config = os.path.join(get_package_share_directory("laser_filters"), "test", "test_speckle_filter.yaml") + + dist_node = launch_ros.actions.Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + name="laser_filter_distance", + parameters=[config], + remappings=[("/scan_filtered", "/scan_filtered_distance")], + ) + + eucl_node = launch_ros.actions.Node( + package="laser_filters", + executable="scan_to_scan_filter_chain", + name="laser_filter_euclidean", + parameters=[config], + remappings=[("/scan_filtered", "/scan_filtered_euclidean")], + ) + return launch.LaunchDescription([dist_node, eucl_node, launch_testing.actions.ReadyToTest()]) + + +class TestSpeckleFilter(unittest.TestCase): + def test_speckle_filter(self): + rclpy.init() + node = TestFixture() + self.assertTrue(node.wait_for_subscribers(10)) + node.start_subscribers() + node.publish_laser_scan() + + dist_msgs_received_flag = node.dist_msg_event_object.wait(timeout=10.0) + eucl_msgs_received_flag = node.eucl_msg_event_object.wait(timeout=10.0) + assert dist_msgs_received_flag, "Did not receive distance msgs !" + assert eucl_msgs_received_flag, "Did not receive distance msgs !" + + expected_scan_ranges = [1, 1, 1, 1, float("nan"), 1, 1, 1, 1, 1, 1] + for scan_range, expected_scan_range in zip(node.msg_dist.ranges, expected_scan_ranges): + if math.isnan(expected_scan_range) or math.isnan(scan_range): + self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range)) + else: + self.assertEqual(scan_range, expected_scan_range) + + # NOTE: This is the actual behavior you would get in ROS1, but there was a bug because of which it would never go into euclidean mode + expected_scan_ranges = [ + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + float("nan"), + ] + for scan_range, expected_scan_range in zip(node.msg_euclid.ranges, expected_scan_ranges): + if math.isnan(expected_scan_range) or math.isnan(scan_range): + self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range)) + else: + self.assertEqual(scan_range, expected_scan_range) + + +class TestFixture(Node): + def __init__(self): + super().__init__("test_speckle_filter_distance") + self.dist_msg_event_object = Event() + self.eucl_msg_event_object = Event() + self.publisher = self.create_publisher(LaserScan, "scan", 10) + + def wait_for_subscribers(self, timeout): + timer_period = 0.1 + t = 0.0 + rate = self.create_rate(1 / timer_period, self.get_clock()) + while rclpy.ok() and t < timeout: + rclpy.spin_once(self) + if self.publisher.get_subscription_count() >= 2: + return True + rate.sleep() + t += timer_period + return False + + def publish_laser_scan(self): + num_beams = 11 + msg = LaserScan() + msg.header.frame_id = "laser" + msg.header.stamp = self.get_clock().now().to_msg() + msg.angle_min = -math.pi / 2 + msg.angle_max = math.pi / 2 + msg.angle_increment = math.pi / (num_beams - 1) + msg.ranges = [1.0, 1.0, 1.0, 1.0, 0.5, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + msg.range_max = 100.0 + self.publisher.publish(msg) + + def start_subscribers(self): + self.distance_subscriber = self.create_subscription(LaserScan, "scan_filtered_distance", self.dist_cb, 10) + + self.euclidean_subscriber = self.create_subscription(LaserScan, "scan_filtered_euclidean", self.euclid_cb, 10) + + # Add a spin thread + self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) + self.ros_spin_thread.start() + + def dist_cb(self, msg): + self.msg_dist = msg + self.dist_msg_event_object.set() + print("received dist") + print(self.msg_dist) + + def euclid_cb(self, msg): + self.msg_euclid = msg + self.eucl_msg_event_object.set() + print("received euclid") + print(self.msg_euclid) diff --git a/test/test_speckle_filter.yaml b/test/test_speckle_filter.yaml new file mode 100644 index 00000000..a14aed37 --- /dev/null +++ b/test/test_speckle_filter.yaml @@ -0,0 +1,21 @@ +laser_filter_distance: + ros__parameters: + filter1: + name: speckle_filter + type: laser_filters/LaserScanSpeckleFilter + params: + filter_type: 0 + max_range: 2.0 + max_range_difference: 0.1 + filter_window: 2 + +laser_filter_euclidean: + ros__parameters: + filter1: + name: speckle_filter + type: laser_filters/LaserScanSpeckleFilter + params: + filter_type: 1 + max_range: 2.0 + max_range_difference: 0.1 + filter_window: 2 diff --git a/test/test_speckle_filter_distance.yaml b/test/test_speckle_filter_distance.yaml deleted file mode 100644 index f1100c94..00000000 --- a/test/test_speckle_filter_distance.yaml +++ /dev/null @@ -1,8 +0,0 @@ -scan_filter_chain: -- name: speckle_filter - type: laser_filters/LaserScanSpeckleFilter - params: - filter_type: 0 - max_range: 2.0 - max_range_difference: 0.1 - filter_window: 2 diff --git a/test/test_speckle_filter_euclidean.yaml b/test/test_speckle_filter_euclidean.yaml deleted file mode 100644 index 714332ce..00000000 --- a/test/test_speckle_filter_euclidean.yaml +++ /dev/null @@ -1,8 +0,0 @@ -scan_filter_chain: -- name: speckle_filter - type: laser_filters/LaserScanSpeckleFilter - params: - filter_type: 1 - max_range: 2.0 - max_range_difference: 0.1 - filter_window: 2 From d919da86b95b16d702e059dd285dad6d3b1b70a5 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 15:04:37 +0100 Subject: [PATCH 06/25] Port speckle filter unit tests to ROS2 --- CMakeLists.txt | 14 + include/laser_filters/speckle_filter.h | 28 +- test/test_speckle_filter.cpp | 358 +++++++++++++++++++++++++ 3 files changed, 386 insertions(+), 14 deletions(-) create mode 100644 test/test_speckle_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7e5b4762..35518df3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,20 @@ if(BUILD_TESTING) RESULT_FILE ${RESULT_FILENAME} ) + set(TEST_NAME test_speckle_filter) + set(RESULT_FILENAME ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TEST_NAME}.gtest.xml) + ament_add_gtest_executable(${TEST_NAME} test/${TEST_NAME}.cpp) + ament_target_dependencies(${TEST_NAME} angles) + target_link_libraries(${TEST_NAME} laser_scan_filters) + 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/speckle_filter.h b/include/laser_filters/speckle_filter.h index 3a3ca0a1..526283e6 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -253,18 +253,6 @@ class LaserScanSpeckleFilter : public filters::FilterBase parameters) { auto result = rcl_interfaces::msg::SetParametersResult(); @@ -272,7 +260,8 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Unknown parameter"); + if(node_ != nullptr) RCLCPP_WARN(node_->get_logger(), "Unknown parameter"); } switch (filter_type) { @@ -310,6 +299,17 @@ class LaserScanSpeckleFilter : public filters::FilterBase +#include +#include +#include + +/* +Notes: +- The tests below only use/test the Distance window validator. There are no tests for the radius + outlier window validator yet. +*/ + +sensor_msgs::msg::LaserScan create_message( + float ranges[], int num_beams +) { + sensor_msgs::msg::LaserScan msg; + + std::vector v_range(ranges, ranges + num_beams); + + msg.header.frame_id = "laser"; + msg.angle_min = -.5; + msg.angle_max = .5; + msg.angle_increment = (msg.angle_max - msg.angle_min) / (num_beams - 1); + msg.time_increment = 0.1 / num_beams; + msg.scan_time = 0.1; + msg.range_min = 0.5; + msg.range_max = 1.5; + msg.ranges = v_range; + + return msg; +} + +/** + * Verifies that two vectors of range values are the same. Allows the case + * where corresponding values are both NaN. + */ +void expect_ranges_equal(const std::vector &actual, const float expected[], int expected_len) { + EXPECT_EQ(expected_len, actual.size()); + for (int i = 0; i < expected_len; i++) { + if (std::isnan(expected[i])) { + EXPECT_TRUE(std::isnan(actual[i])) << "Mismatch at index " << i << std::endl; + } + else { + EXPECT_NEAR(expected[i], actual[i], 1e-6) << "Mismatch at index " << i << std::endl; + } + } +} + +void expect_ranges_equal(const std::vector &actual, const std::vector expected) { + expect_ranges_equal(actual, &expected[0], expected.size()); +} + +TEST(SpeckleFilter_Distance, SingleSpeckle) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1, 1, 0.5, 1, 1, 1, 1, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, NAN, 1, 1, 1, 1, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoDistantSpeckles) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1, 1, 0.5, 1, 1, 1, 0.5, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, NAN, 1, 1, 1, NAN, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoNearSpeckles) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1, 1, 0.5, 1, 0.5, 1, 1, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, NAN, NAN, NAN, 1, 1, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoSpecklesAtEdge) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1.2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1.2}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {NAN, 1, 1, 1, 1, 1, 1, 1, 1, 1, NAN}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, SinglePeak) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1, 1, 3, 1, 1, 1, 1, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, 3, 1, 1, 1, 1, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoFarPeaks) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + + float ranges[] = {1, 1, 1, 1, 3, 1, 1, 1, 3, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, 3, 1, 1, 1, 3, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoNearPeaks) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1, 1, 3, 1, 3, 1, 1, 1, 1}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1, 1, 3, NAN, 3, 1, 1, 1, 1}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, TwoPeaksAtEdge) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {3, 1, 1, 1, 1, 1, 1, 1, 1, 1, 3}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, MultiplePlateausNoSpeckles) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 2)); + + filter.reconfigureCB(config); + + float ranges[] = {1, 1, 1.2, 1.2, 1.2, 0.7, 0.7, 0.9, 0.9, 0.9, 0.9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {1, 1, 1.2, 1.2, 1.2, 0.7, 0.7, 0.9, 0.9, 0.9, 0.9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(SpeckleFilter_Distance, MultiplePlateausBiggerWindow) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window",3)); + + filter.reconfigureCB(config); + + + float ranges[] = {1, 1, 1.2, 1.2, 1.2, 0.7, 0.7, 0.9, 0.9, 0.9, 0.9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + float expected[] = {NAN, NAN, 1.2, 1.2, 1.2, NAN, NAN, 0.9, 0.9, 0.9, 0.9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +#ifdef ENABLE_PERFORMANCE +TEST(SpeckleFilter_Distance, Performance) { + laser_filters::LaserScanSpeckleFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("filter_type", laser_filters::SpeckleFilterType::Distance)); + config.push_back(rclcpp::Parameter("max_range", 2.0)); + config.push_back(rclcpp::Parameter("max_range_difference", 0.1)); + config.push_back(rclcpp::Parameter("filter_window", 8)); + + filter.reconfigureCB(config); + + float ranges[] = {}; + sensor_msgs::msg::LaserScan input_scan[2]; + input_scan[0] = create_message(ranges, 0); + int num_samples = 1024; + int next_index = 0; + int pending = 0; + float range_val = 0; + while (input_scan[0].ranges.size() < num_samples) { + if (pending == 0) { + range_val = 0.5 + next_index / 10.0f; + pending = next_index + 1; + + if (next_index == 10) { + next_index = 0; + } else { + ++next_index; + } + } + + input_scan[0].ranges.push_back(range_val); + --pending; + } + + // Create second input set. A mirror of the first + input_scan[1] = input_scan[0]; + std::reverse(input_scan[1].ranges.begin(), input_scan[1].ranges.end()); + + sensor_msgs::msg::LaserScan output_scan; + sensor_msgs::msg::LaserScan expected_scan[2]; + float expected_data[2][num_samples]; + + // Create expected output by running filter once. + + // The main purpose is to check that when executing repeatedly, the expected output + // alternates. + for (int j = 0; j < 2; j++) { + filter.update(input_scan[j], expected_scan[j]); + } + + for (int i = 0; i < 10000; i++) { + int j = i % 2; + filter.update(input_scan[j], output_scan); + + expect_ranges_equal(output_scan.ranges, expected_scan[j].ranges); + } +} +#endif + +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} From e2c35debb5dbeaa9d7ba543905c48789d8e87e25 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Fri, 19 Jan 2024 15:51:24 +0100 Subject: [PATCH 07/25] Port scan shadows filter unit tests to ROS2 --- CMakeLists.txt | 16 +- include/laser_filters/scan_shadows_filter.h | 41 +- test/test_scan_shadows_filter.cpp | 416 ++++++++++++++++++++ 3 files changed, 470 insertions(+), 3 deletions(-) create mode 100644 test/test_scan_shadows_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 35518df3..0fa213b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,7 +76,21 @@ if(BUILD_TESTING) --gtest_output=xml:${RESULT_FILENAME} RESULT_FILE ${RESULT_FILENAME} ) - + + set(TEST_NAME test_scan_shadows_filter) + set(RESULT_FILENAME ${AMENT_TEST_RESULTS_DIR}/${PROJECT_NAME}/${TEST_NAME}.gtest.xml) + ament_add_gtest_executable(${TEST_NAME} test/${TEST_NAME}.cpp) + ament_target_dependencies(${TEST_NAME} filters rclcpp sensor_msgs) + target_link_libraries(${TEST_NAME} laser_scan_filters) + 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/scan_shadows_filter.h b/include/laser_filters/scan_shadows_filter.h index d9e14d88..f17ff6e4 100644 --- a/include/laser_filters/scan_shadows_filter.h +++ b/include/laser_filters/scan_shadows_filter.h @@ -61,6 +61,7 @@ class ScanShadowsFilter : public filters::FilterBaseget_logger(), "Error: ShadowsFilter was not given neighbors.\n"); } + remove_shadow_start_point_ = false; // default value + if (!filters::FilterBase::getParam(std::string("remove_shadow_start_point"), remove_shadow_start_point_)) + { + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: ShadowsFilter was not given remove_shadow_start_point.\n"); + } if (min_angle_ < 0) { @@ -162,12 +168,19 @@ class ScanShadowsFilter : public filters::FilterBase::quiet_NaN(); + } } } } - RCLCPP_DEBUG(logging_interface_->get_logger(), "ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d", - (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_); + if(logging_interface_!= nullptr) + { + RCLCPP_DEBUG(logging_interface_->get_logger(), "ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d", + (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_); + } for (std::set::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it) { scan_out.ranges[*it] = std::numeric_limits::quiet_NaN(); // Failed test to set the ranges to invalid value @@ -175,6 +188,30 @@ class ScanShadowsFilter : public filters::FilterBase parameters) + { + auto result = rcl_interfaces::msg::SetParametersResult(); + result.successful = true; + + for (auto parameter : parameters) + { + if(parameter.get_name() == "min_angle"&& parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + min_angle_ = parameter.as_double(); + else if(parameter.get_name() == "max_angle" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + max_angle_ = parameter.as_double(); + else if(parameter.get_name() == "neighbors" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + neighbors_ = parameter.as_int(); + else if(parameter.get_name() == "window" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + window_ = parameter.as_int(); + else if(parameter.get_name() == "remove_shadow_start_point" && parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) + remove_shadow_start_point_ = parameter.as_bool(); + } + shadow_detector_.configure( + angles::from_degrees(min_angle_), + angles::from_degrees(max_angle_)); + + return result; + } //////////////////////////////////////////////////////////////////////////////// }; } diff --git a/test/test_scan_shadows_filter.cpp b/test/test_scan_shadows_filter.cpp new file mode 100644 index 00000000..a4621674 --- /dev/null +++ b/test/test_scan_shadows_filter.cpp @@ -0,0 +1,416 @@ +#include +#include +#include +#include "laser_filters/scan_shadows_filter.h" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + +sensor_msgs::msg::LaserScan create_message( + float ranges[], int num_beams +) { + sensor_msgs::msg::LaserScan msg; + + std::vector v_range(ranges, ranges + num_beams); + + msg.header.frame_id = "laser"; + // Use a small beam so that angle_increment remains small (realistic) also with few points + msg.angle_min = -M_PI / 12; + msg.angle_max = M_PI / 12; + msg.angle_increment = (msg.angle_max - msg.angle_min) / (num_beams - 1); + msg.time_increment = 0.1 / num_beams; + msg.scan_time = 0.1; + msg.range_min = 0.1; + msg.range_max = 10; + msg.ranges = v_range; + + return msg; +} + +/** + * Verifies that two vectors of range values are the same. Allows the case + * where corresponding values are both NaN. + */ +void expect_ranges_equal(const std::vector &actual, const float expected[], int expected_len) { + EXPECT_EQ(expected_len, actual.size()); + for (int i = 0; i < expected_len; i++) { + if (std::isnan(expected[i])) { + EXPECT_TRUE(std::isnan(actual[i])) << "Mismatch at index " << i << std::endl; + } + else { + EXPECT_NEAR(expected[i], actual[i], 1e-6) << "Mismatch at index " << i << std::endl; + } + } +} + +void expect_ranges_equal(const std::vector &actual, const std::vector expected) { + expect_ranges_equal(actual, &expected[0], expected.size()); +} + +TEST(ScanShadowsFilter, NoShadows) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 1)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + filter.update(input_scan, output_scan); + + float expected[] = {9, 9, 9, 9, 9, 9, 9, 9, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, DistanceDeltaWithoutShadow) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + // This input data is very much simplified. The range-5 points represent a nearby object, and + // the range-9 points a wall. + float ranges[] = {5, 5, 5, 5, 5, 9, 9, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Below is what is expected given the filter's current logic. However, it shows that the + // filter is primitive and can filter out point that are not shadows. + float expected[] = {5, 5, 5, 5, NAN, NAN, 9, 9, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, DistanceDeltaWithoutShadow_Angles_8_172) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 8.0)); + config.push_back(rclcpp::Parameter("max_angle", 172.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 9, 9, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Increasing the angle range still results in the same filter behaviour. + float expected[] = {5, 5, 5, 5, NAN, NAN, 9, 9, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, DistanceDeltaWithoutShadow_Angles_5_175) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 5.0)); + config.push_back(rclcpp::Parameter("max_angle", 175.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 9, 9, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Increasing the angle range more, filters out fewer points + float expected[] = {5, 5, 5, 5, 5, NAN, 9, 9, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, DistanceDeltaWithoutShadowFlipped_Angles_5_175) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 5.0)); + config.push_back(rclcpp::Parameter("max_angle", 175.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {9, 9, 9, 9, 9, 5, 5, 5, 5, 5}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Reversing the input results in reversed output + float expected[] = {9, 9, 9, 9, NAN, 5, 5, 5, 5, 5}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, DistanceDeltaWithoutShadow_Angles_3_177) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 3.0)); + config.push_back(rclcpp::Parameter("max_angle", 177.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 9, 9, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float)); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Increasing the range even more, no more points are filtered out + float expected[] = {5, 5, 5, 5, 5, 9, 9, 9, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, SingleBackwardShadow_NoNeighbours) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + // This input data is very much simplified. The range-5 points represent a nearby object, and + // the range-9 points a wall. The range-7 point is a shadow. + float ranges[] = {5, 5, 5, 5, 5, 9, 7, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // The shadow is filtered out, as well as some other points + float expected[] = {5, 5, 5, 5, NAN, NAN, NAN, NAN, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, SingleBackwardShadow_OneNeighbour) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 1)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", false)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 9, 7, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Below is what is expected given the filter's current logic. Note, this configuration does + // not filter out the simulated shadow but some other points. + float expected[] = {5, 5, 5, 5, 5, NAN, 7, NAN, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, SingleForwardShadow_NoNeighbours) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 0)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + // This input data is very much simplified. The range-5 points represent a nearby object, and + // the range-9 points a wall. The range-3 point is a shadow. + float ranges[] = {5, 5, 5, 5, 5, 9, 3, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // The shadow is filtered out, as well as some other points + float expected[] = {5, 5, 5, 5, NAN, NAN, NAN, NAN, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, SingleForwardShadow_OneNeighbour) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 1)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", false)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 9, 3, 9, 9, 9}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Below is what is expected given the filter's current logic. Note, this configuration does + // not filter out the simulated shadow but some other points. + float expected[] = {5, 5, 5, 5, 5, NAN, 3, NAN, 9, 9}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); +} + +TEST(ScanShadowsFilter, SingleForwardShadow_AngleIncrementChanged) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 1)); + config.push_back(rclcpp::Parameter("window", 1)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", false)); + + filter.reconfigureCB(config); + + float ranges[] = {5, 5, 5, 5, 5, 4, 4, 4, 4, 4}; + sensor_msgs::msg::LaserScan input_scan = create_message( + ranges, sizeof(ranges) / sizeof(float) + ); + sensor_msgs::msg::LaserScan output_scan; + + filter.update(input_scan, output_scan); + + // Below is what is expected given the filter's current logic. Note, this configuration does + // not filter out the simulated shadow but some other points. + float expected[] = {5, 5, 5, 5, 5, 4, 4, 4, 4, 4}; + + expect_ranges_equal(output_scan.ranges, expected, sizeof(expected) / sizeof(float)); + + // Smaller angle_increment, results in steeper angles and shadow detection + input_scan.angle_increment /= 2; + filter.update(input_scan, output_scan); + + float new_expected[] = {5, 5, 5, 5, NAN, 4, 4, 4, 4, 4}; + + expect_ranges_equal(output_scan.ranges, new_expected, sizeof(new_expected) / sizeof(float)); +} + +#ifdef ENABLE_PERFORMANCE +TEST(ScanShadowsFilter, Performance) { + laser_filters::ScanShadowsFilter filter; + std::vector config; + + config.push_back(rclcpp::Parameter("min_angle", 15.0)); + config.push_back(rclcpp::Parameter("max_angle", 165.0)); + config.push_back(rclcpp::Parameter("neighbors", 2)); + config.push_back(rclcpp::Parameter("window", 2)); + config.push_back(rclcpp::Parameter("remove_shadow_start_point", true)); + + filter.reconfigureCB(config); + + float ranges[] = {}; + sensor_msgs::msg::LaserScan input_scan[2]; + input_scan[0] = create_message(ranges, 0); + int num_samples = 1024; + int next_index = 0; + int pending = 0; + float range_val = 5; + while (input_scan[0].ranges.size() < num_samples) { + if (pending == 0) { + range_val = 5 + next_index; + pending = next_index + 1; + + if (next_index == 10) { + range_val = range_val + 20; + next_index = 0; + } else { + ++next_index; + } + } + + input_scan[0].ranges.push_back(range_val); + --pending; + } + + // Create second input set. A mirror of the first + input_scan[1] = input_scan[0]; + std::reverse(input_scan[1].ranges.begin(), input_scan[1].ranges.end()); + + sensor_msgs::msg::LaserScan output_scan; + sensor_msgs::msg::LaserScan expected_scan[2]; + float expected_data[2][num_samples]; + + // Create expected output by running filter once. + + // The main purpose is to check that when executing repeatedly, the expected output + // alternates. + for (int j = 0; j < 2; j++) { + filter.update(input_scan[j], expected_scan[j]); + } + + for (int i = 0; i < 10000; i++) { + int j = i % 2; + filter.update(input_scan[j], output_scan); + + expect_ranges_equal(output_scan.ranges, expected_scan[j].ranges); + } +} +#endif + +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 5185c7be2a5fbb42eac2eb0dd7b9a5e64ff2ff46 Mon Sep 17 00:00:00 2001 From: berend-kupers <144335677+berend-kupers@users.noreply.github.com> Date: Tue, 16 Jul 2024 09:08:33 +0200 Subject: [PATCH 08/25] Update include/laser_filters/polygon_filter.h MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Błażej Sowa --- include/laser_filters/polygon_filter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index d46dfd33..2c0869e5 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include #include From 50237b282dc5bcb3b1d6710361b71f0c24a22c13 Mon Sep 17 00:00:00 2001 From: berend-kupers <144335677+berend-kupers@users.noreply.github.com> Date: Tue, 16 Jul 2024 09:09:18 +0200 Subject: [PATCH 09/25] Changed RCLCPP_WARN_THROTTLE MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Błażej Sowa --- include/laser_filters/polygon_filter.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index 2c0869e5..5b2aaab5 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -536,7 +536,6 @@ class StaticLaserScanPolygonFilter : public LaserScanPolygonFilterBase { { RCLCPP_WARN_THROTTLE(logging_interface_->get_logger(), *get_clock(), 1000, - "StaticLaserScanPolygonFilter", "Could not get transform, ignoring laser scan! %s", ex.what()); return false; } From 8058839c1970c5ddf75bd75dff2850276f4bf0be Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Tue, 16 Jul 2024 09:17:06 +0200 Subject: [PATCH 10/25] Added dependency --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 444ebec0..93df4e49 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,7 @@ rclcpp_lifecycle sensor_msgs tf2 + tf2_geometry_msgs tf2_ros tf2_kdl From 8b19c2d5226a6ec2cfc4ccd883c0122db801b8a9 Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Tue, 16 Jul 2024 09:39:03 +0200 Subject: [PATCH 11/25] Copy getPointCloud2FieldIndex function --- include/laser_filters/polygon_filter.h | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h index 5b2aaab5..3d4aa33b 100644 --- a/include/laser_filters/polygon_filter.h +++ b/include/laser_filters/polygon_filter.h @@ -46,7 +46,6 @@ #include #include -#include #include #include #include @@ -203,6 +202,19 @@ std::string polygonToString(geometry_msgs::msg::Polygon polygon) return polygon_string; } +static inline int getPointCloud2FieldIndex( + const sensor_msgs::msg::PointCloud2 & cloud, + const std::string & field_name) +{ + // Get the index we need + for (size_t d = 0; d < cloud.fields.size(); ++d) { + if (cloud.fields[d].name == field_name) { + return static_cast(d); + } + } + return -1; +} + using namespace std::literals; namespace laser_filters { @@ -391,10 +403,10 @@ class LaserScanPolygonFilter : public LaserScanPolygonFilterBase { return false; } - const int i_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "index"); - const int x_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "x"); - const int y_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "y"); - const int z_idx_c = sensor_msgs::getPointCloud2FieldIndex(laser_cloud, "z"); + const int i_idx_c = getPointCloud2FieldIndex(laser_cloud, "index"); + const int x_idx_c = getPointCloud2FieldIndex(laser_cloud, "x"); + const int y_idx_c = getPointCloud2FieldIndex(laser_cloud, "y"); + const int z_idx_c = getPointCloud2FieldIndex(laser_cloud, "z"); if (i_idx_c == -1 || x_idx_c == -1 || y_idx_c == -1 || z_idx_c == -1) { From 16118ea4a86e6794a9b3f7ec118c1dc2140e1307 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 25 Jul 2024 12:45:49 +0200 Subject: [PATCH 12/25] Initial version for distance moving window filter. --- include/laser_filters/speckle_filter.h | 72 +++++++++++++++++++++++++- 1 file changed, 71 insertions(+), 1 deletion(-) diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index 3a3ca0a1..d35c25bc 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -40,6 +40,9 @@ #ifndef SPECKLE_FILTER_H #define SPECKLE_FILTER_H +#include +#include + #include #include @@ -49,7 +52,8 @@ namespace laser_filters enum SpeckleFilterType //Enum to select the filtering method { Distance = 0, // Range based filtering (distance between consecutive points - RadiusOutlier = 1 // Euclidean filtering based on radius outlier search + RadiusOutlier = 1, // Euclidean filtering based on radius outlier search + DistanceMovingWindow = 2 // Range based filtering based on number of in- and outliers }; class WindowValidator @@ -58,6 +62,63 @@ class WindowValidator virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0; }; +class DistanceMovingWindowValidator : public WindowValidator +{ + virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) + { + const size_t half_window = std::floor(window/2.0); + + const float& range = scan.ranges[idx]; + if (std::isnan(range)) { + return false; + } + + size_t nr_inliers = 0; + size_t nr_outliers = 0; + + auto check_in_out_lier = [&](const size_t neighbor_idx) + { + const float& neighbor_range = scan.ranges[neighbor_idx]; + if (std::isnan(neighbor_range) || fabs(neighbor_range - range) > max_range_difference) + { + nr_outliers++; + } + else + { + nr_inliers++; + } + return; + }; + + for (size_t neighbor_idx_nr = 1; neighbor_idx_nr <= half_window; ++neighbor_idx_nr) + { + // check points before index if possible side of the + if (idx >= neighbor_idx_nr) // out of bounds check + { + const size_t neighbor_idx = idx - neighbor_idx_nr; + check_in_out_lier(neighbor_idx); + } + + // check points after the index + const size_t neighbor_idx = idx + neighbor_idx_nr; + if (neighbor_idx < scan.ranges.size()) // Out of bound check + { + check_in_out_lier(neighbor_idx); + } + } + + // more aggressive removal if the number is the same + // ignore filter if both values are equal 0 + if (!(nr_outliers != 0 && nr_inliers != 0) && nr_outliers >= nr_inliers) + { + std::cout << "Filtered out: " << idx << " inliner: " << nr_inliers << "; outliers: " << nr_outliers << std::endl; + return false; + } + + return true; + } +}; + class DistanceWindowValidator : public WindowValidator { virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) @@ -215,6 +276,15 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), filter_window < 2, "SpackleFilter needs window of at least size 2 to work properly. Please update the parameter and restart the filter!"); + break; + default: break; } From 07ae1cfdc4d3c71d6d29c0cb0d75898aeb8f300e Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Mon, 5 Aug 2024 16:24:06 +0200 Subject: [PATCH 13/25] Added LaserScanMedianFilter --- include/laser_filters/median_spatial_filter.h | 122 ++++++++++++++++++ laser_filters_plugins.xml | 6 + src/laser_scan_filters.cpp | 2 + 3 files changed, 130 insertions(+) create mode 100644 include/laser_filters/median_spatial_filter.h diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h new file mode 100644 index 00000000..15b1bc1b --- /dev/null +++ b/include/laser_filters/median_spatial_filter.h @@ -0,0 +1,122 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2017, laser_filters authors +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +/* +\author Yannic Bachmann +*/ + +#ifndef LASER_SCAN_MEDIAN_SPATIAL_FILTER_H +#define LASER_SCAN_MEDIAN_SPATIAL_FILTER_H + +#include "filters/filter_base.hpp" +#include +#include +#include +#include + +namespace laser_filters +{ + +class LaserScanMedianSpatialFilter : public filters::FilterBase +{ +public: + int window_size_; + + bool configure() + { + // Default window size + window_size_ = 3; + getParam("window_size", window_size_); + + // Ensure window size is positive + if (window_size_ <= 0) + { + throw std::runtime_error("Window size must be positive"); + } + + // Ensure window size is odd + if (window_size_ % 2 == 0) + { + window_size_ += 1; + } + + return true; + } + + virtual ~LaserScanMedianSpatialFilter() {} + + bool update(const sensor_msgs::msg::LaserScan &input_scan, sensor_msgs::msg::LaserScan &filtered_scan) + { + filtered_scan = input_scan; + + int half_window = window_size_ / 2; + std::vector window; + + for (size_t i = 0; i < input_scan.ranges.size(); ++i) + { + window.clear(); + + // Collect points within the window + for (int j = -half_window; j <= half_window; ++j) + { + int index = i + j; + + if (index >= 0 && index < input_scan.ranges.size()) + { + if (!std::isnan(input_scan.ranges[index])) + { + window.push_back(input_scan.ranges[index]); + } + } + } + + if (!window.empty()) + { + // Calculate median + std::sort(window.begin(), window.end()); + filtered_scan.ranges[i] = window[window.size() / 2]; + } + else + { + filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); + } + } + + return true; + } +}; + +} // namespace laser_filters + +#endif // LASER_SCAN_MEDIAN_SPATIAL_FILTER_H diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 71f0be58..a437a48e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -60,6 +60,12 @@ This is a filter that removes points on directions defined in a mask from a laser scan. + + + This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. + + diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 485d3c14..c1791cf6 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -28,6 +28,7 @@ */ #include "laser_filters/median_filter.h" +#include "laser_filters/median_spatial_filter.h" #include "laser_filters/array_filter.h" #include "laser_filters/intensity_filter.h" #include "laser_filters/range_filter.h" @@ -46,6 +47,7 @@ #include // NOLINT PLUGINLIB_EXPORT_CLASS(laser_filters::LaserMedianFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMedianSpatialFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserArrayFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanIntensityFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanRangeFilter, filters::FilterBase) From 35a00143c9338c475cbab2ed11dfec28f6659840 Mon Sep 17 00:00:00 2001 From: MAHA Maia Date: Tue, 6 Aug 2024 09:14:32 +0200 Subject: [PATCH 14/25] Revert "Initial version for distance moving window filter." This reverts commit 16118ea4a86e6794a9b3f7ec118c1dc2140e1307. --- include/laser_filters/speckle_filter.h | 72 +------------------------- 1 file changed, 1 insertion(+), 71 deletions(-) diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index d35c25bc..3a3ca0a1 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -40,9 +40,6 @@ #ifndef SPECKLE_FILTER_H #define SPECKLE_FILTER_H -#include -#include - #include #include @@ -52,8 +49,7 @@ namespace laser_filters enum SpeckleFilterType //Enum to select the filtering method { Distance = 0, // Range based filtering (distance between consecutive points - RadiusOutlier = 1, // Euclidean filtering based on radius outlier search - DistanceMovingWindow = 2 // Range based filtering based on number of in- and outliers + RadiusOutlier = 1 // Euclidean filtering based on radius outlier search }; class WindowValidator @@ -62,63 +58,6 @@ class WindowValidator virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0; }; -class DistanceMovingWindowValidator : public WindowValidator -{ - virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) - { - const size_t half_window = std::floor(window/2.0); - - const float& range = scan.ranges[idx]; - if (std::isnan(range)) { - return false; - } - - size_t nr_inliers = 0; - size_t nr_outliers = 0; - - auto check_in_out_lier = [&](const size_t neighbor_idx) - { - const float& neighbor_range = scan.ranges[neighbor_idx]; - if (std::isnan(neighbor_range) || fabs(neighbor_range - range) > max_range_difference) - { - nr_outliers++; - } - else - { - nr_inliers++; - } - return; - }; - - for (size_t neighbor_idx_nr = 1; neighbor_idx_nr <= half_window; ++neighbor_idx_nr) - { - // check points before index if possible side of the - if (idx >= neighbor_idx_nr) // out of bounds check - { - const size_t neighbor_idx = idx - neighbor_idx_nr; - check_in_out_lier(neighbor_idx); - } - - // check points after the index - const size_t neighbor_idx = idx + neighbor_idx_nr; - if (neighbor_idx < scan.ranges.size()) // Out of bound check - { - check_in_out_lier(neighbor_idx); - } - } - - // more aggressive removal if the number is the same - // ignore filter if both values are equal 0 - if (!(nr_outliers != 0 && nr_inliers != 0) && nr_outliers >= nr_inliers) - { - std::cout << "Filtered out: " << idx << " inliner: " << nr_inliers << "; outliers: " << nr_outliers << std::endl; - return false; - } - - return true; - } -}; - class DistanceWindowValidator : public WindowValidator { virtual bool checkWindowValid(const sensor_msgs::msg::LaserScan& scan, size_t idx, size_t window, double max_range_difference) @@ -276,15 +215,6 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), filter_window < 2, "SpackleFilter needs window of at least size 2 to work properly. Please update the parameter and restart the filter!"); - break; - default: break; } From 742e76ae2e62ee2768e97237ee51e0bea64aa47f Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Tue, 6 Aug 2024 14:31:11 +0200 Subject: [PATCH 15/25] Fixed test --- test/test_speckle_filter.test.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/test/test_speckle_filter.test.py b/test/test_speckle_filter.test.py index e14c2e55..32b5fbcd 100755 --- a/test/test_speckle_filter.test.py +++ b/test/test_speckle_filter.test.py @@ -54,7 +54,7 @@ def test_speckle_filter(self): dist_msgs_received_flag = node.dist_msg_event_object.wait(timeout=10.0) eucl_msgs_received_flag = node.eucl_msg_event_object.wait(timeout=10.0) assert dist_msgs_received_flag, "Did not receive distance msgs !" - assert eucl_msgs_received_flag, "Did not receive distance msgs !" + assert eucl_msgs_received_flag, "Did not receive euclidean msgs !" expected_scan_ranges = [1, 1, 1, 1, float("nan"), 1, 1, 1, 1, 1, 1] for scan_range, expected_scan_range in zip(node.msg_dist.ranges, expected_scan_ranges): @@ -82,6 +82,8 @@ def test_speckle_filter(self): self.assertEqual(math.isnan(expected_scan_range), math.isnan(scan_range)) else: self.assertEqual(scan_range, expected_scan_range) + rclpy.shutdown() + node.join_thread() class TestFixture(Node): @@ -124,6 +126,9 @@ def start_subscribers(self): self.ros_spin_thread = Thread(target=lambda node: rclpy.spin(node), args=(self,)) self.ros_spin_thread.start() + def join_thread(self): + self.ros_spin_thread.join() + def dist_cb(self, msg): self.msg_dist = msg self.dist_msg_event_object.set() From ffddce3f9985d12800b1af9789edebc14ef31f6a Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Tue, 6 Aug 2024 14:31:20 +0200 Subject: [PATCH 16/25] Removed node interface --- include/laser_filters/speckle_filter.h | 21 +++++++-------------- 1 file changed, 7 insertions(+), 14 deletions(-) diff --git a/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index 526283e6..f071afa8 100644 --- a/include/laser_filters/speckle_filter.h +++ b/include/laser_filters/speckle_filter.h @@ -174,27 +174,22 @@ class LaserScanSpeckleFilter : public filters::FilterBase(getName()); - // dynamic reconfigure parameters callback: - on_set_parameters_callback_handle_ = node_->add_on_set_parameters_callback( - std::bind(&LaserScanSpeckleFilter::reconfigureCB, this, std::placeholders::_1)); - // get params if (!filters::FilterBase::getParam(std::string("filter_type"), filter_type)) { - RCLCPP_ERROR(node_->get_logger(), "Error: SpeckleFilter was not given filter_type.\n"); + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: SpeckleFilter was not given filter_type.\n"); return false; }if (!filters::FilterBase::getParam(std::string("max_range"), max_range)) { - RCLCPP_ERROR(node_->get_logger(), "Error: SpeckleFilter was not given max_range.\n"); + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: SpeckleFilter was not given max_range.\n"); return false; }if (!filters::FilterBase::getParam(std::string("max_range_difference"), max_range_difference)) { - RCLCPP_ERROR(node_->get_logger(), "Error: SpeckleFilter was not given max_range_difference.\n"); + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: SpeckleFilter was not given max_range_difference.\n"); return false; }if (!filters::FilterBase::getParam(std::string("filter_window"), filter_window)) { - RCLCPP_ERROR(node_->get_logger(), "Error: SpeckleFilter was not given filter_window.\n"); + RCLCPP_ERROR(logging_interface_->get_logger(), "Error: SpeckleFilter was not given filter_window.\n"); return false; } @@ -260,8 +255,8 @@ class LaserScanSpeckleFilter : public filters::FilterBaseget_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Update parameter " << parameter.get_name().c_str()<< " to "<get_logger(), "Unknown parameter"); + if(logging_interface_ != nullptr) RCLCPP_WARN(logging_interface_->get_logger(), "Unknown parameter"); } switch (filter_type) { @@ -308,8 +303,6 @@ class LaserScanSpeckleFilter : public filters::FilterBase Date: Tue, 6 Aug 2024 14:31:36 +0200 Subject: [PATCH 17/25] Removed unused file --- src/speckle_filter.cpp | 132 ----------------------------------------- 1 file changed, 132 deletions(-) delete mode 100644 src/speckle_filter.cpp diff --git a/src/speckle_filter.cpp b/src/speckle_filter.cpp deleted file mode 100644 index 7f6b12e1..00000000 --- a/src/speckle_filter.cpp +++ /dev/null @@ -1,132 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Robot Operating System code by Eurotec B.V. - * Copyright (c) 2020, Eurotec B.V. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above - * copyright notice, this list of conditions and the following - * disclaimer. - * - * 2. Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED - * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * speckle_filter.cpp - */ - -#include -#include - -namespace laser_filters -{ -LaserScanSpeckleFilter::LaserScanSpeckleFilter() -{ - validator_ = 0; -} - -LaserScanSpeckleFilter::~LaserScanSpeckleFilter() -{ - if (!validator_) - { - delete validator_; - } -} - -bool LaserScanSpeckleFilter::configure() -{ - ros::NodeHandle private_nh("~" + getName()); - dyn_server_.reset(new dynamic_reconfigure::Server(own_mutex_, private_nh)); - dynamic_reconfigure::Server::CallbackType f; - f = boost::bind(&laser_filters::LaserScanSpeckleFilter::reconfigureCB, this, _1, _2); - dyn_server_->setCallback(f); - - getParam("filter_type", config_.filter_type); - getParam("max_range", config_.max_range); - getParam("max_range_difference", config_.max_range_difference); - getParam("filter_window", config_.filter_window); - dyn_server_->updateConfig(config_); - return true; -} - -bool LaserScanSpeckleFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan) -{ - output_scan = input_scan; - std::vector valid_ranges(output_scan.ranges.size(), false); - for (size_t idx = 0; idx < output_scan.ranges.size() - config_.filter_window + 1; ++idx) - { - bool window_valid = validator_->checkWindowValid( - output_scan, idx, config_.filter_window, config_.max_range_difference); - - // Actually set the valid ranges (do not set to false if it was already valid or out of range) - for (size_t neighbor_idx_or_self_nr = 0; neighbor_idx_or_self_nr < config_.filter_window; ++neighbor_idx_or_self_nr) - { - size_t neighbor_idx_or_self = idx + neighbor_idx_or_self_nr; - if (neighbor_idx_or_self < output_scan.ranges.size()) // Out of bound check - { - bool out_of_range = output_scan.ranges[neighbor_idx_or_self] > config_.max_range; - valid_ranges[neighbor_idx_or_self] = valid_ranges[neighbor_idx_or_self] || window_valid || out_of_range; - } - } - } - - for (size_t idx = 0; idx < valid_ranges.size(); ++idx) - { - if (!valid_ranges[idx]) - { - output_scan.ranges[idx] = std::numeric_limits::quiet_NaN(); - } - } - - return true; -} - -void LaserScanSpeckleFilter::reconfigureCB(laser_filters::SpeckleFilterConfig& config, uint32_t level) -{ - config_ = config; - - switch (config_.filter_type) { - case laser_filters::SpeckleFilter_RadiusOutlier: - if (validator_) - { - delete validator_; - } - validator_ = new laser_filters::RadiusOutlierWindowValidator(); - break; - - case laser_filters::SpeckleFilter_Distance: - if (validator_) - { - delete validator_; - } - validator_ = new laser_filters::DistanceWindowValidator(); - break; - - default: - break; - } - -} -} From affc0eff86c20057987fcdd1cf03c173e8b6017d Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:36:01 +0200 Subject: [PATCH 18/25] Use nan and infinity values for the median if they make up the majority of the window --- include/laser_filters/median_spatial_filter.h | 84 +++++++++++++------ 1 file changed, 59 insertions(+), 25 deletions(-) diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 15b1bc1b..7a09bc0a 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -44,15 +44,19 @@ #include #include #include +#include +#include namespace laser_filters { class LaserScanMedianSpatialFilter : public filters::FilterBase { -public: +private: int window_size_; +public: + bool configure() { // Default window size @@ -81,36 +85,66 @@ class LaserScanMedianSpatialFilter : public filters::FilterBase window; + std::vector valid_values; + int nan_count = 0; + int neg_inf_count = 0; + int pos_inf_count = 0; - for (size_t i = 0; i < input_scan.ranges.size(); ++i) + for (size_t current_beam_index = 0; current_beam_index < input_scan.ranges.size(); ++current_beam_index) { - window.clear(); + valid_values.clear(); + nan_count = 0; + neg_inf_count = 0; + pos_inf_count = 0; - // Collect points within the window - for (int j = -half_window; j <= half_window; ++j) - { - int index = i + j; + // Collect points within the window + for (int window_offset = -half_window; window_offset <= half_window; ++window_offset) + { + int index = current_beam_index + window_offset; + + if (index >= 0 && index < input_scan.ranges.size()) + { + float value = input_scan.ranges[index]; + + if (std::isnan(value)) + { + nan_count++; + } + else if (value == -std::numeric_limits::infinity()) + { + neg_inf_count++; + } + else if (value == std::numeric_limits::infinity()) + { + pos_inf_count++; + } + else + { + valid_values.push_back(value); + } + } + } - if (index >= 0 && index < input_scan.ranges.size()) + // Determine which set is the largest + // In case of a tie, prioritize valid-values over nan-values over neg-inf-values over pos-inf-values + if (valid_values.size() >= nan_count && valid_values.size() >= neg_inf_count && valid_values.size() >= pos_inf_count) + { + // Sort the valid values and return the median + std::sort(valid_values.begin(), valid_values.end()); + filtered_scan.ranges[current_beam_index] = valid_values[valid_values.size() / 2]; + } + else if (nan_count >= valid_values.size() && nan_count >= neg_inf_count && nan_count >= pos_inf_count) + { + filtered_scan.ranges[current_beam_index] = std::numeric_limits::quiet_NaN(); + } + else if (neg_inf_count >= valid_values.size() && neg_inf_count >= nan_count && neg_inf_count >= pos_inf_count) + { + filtered_scan.ranges[current_beam_index] = -std::numeric_limits::infinity(); + } + else if (pos_inf_count >= valid_values.size() && pos_inf_count >= nan_count && pos_inf_count >= neg_inf_count) { - if (!std::isnan(input_scan.ranges[index])) - { - window.push_back(input_scan.ranges[index]); - } + filtered_scan.ranges[current_beam_index] = std::numeric_limits::infinity(); } - } - - if (!window.empty()) - { - // Calculate median - std::sort(window.begin(), window.end()); - filtered_scan.ranges[i] = window[window.size() / 2]; - } - else - { - filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); - } } return true; From 210e1b16c2c36ef4e819b7fcfc9238aa75042c99 Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:37:58 +0200 Subject: [PATCH 19/25] =?UTF-8?q?Don=C2=B4t=20declare+initialize=20window?= =?UTF-8?q?=5Fsize=20before=20getParam=20call?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/laser_filters/median_spatial_filter.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 7a09bc0a..5ea97532 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -59,14 +59,13 @@ class LaserScanMedianSpatialFilter : public filters::FilterBaseget_logger(), "Window size must be positive.\n"); + return false; } // Ensure window size is odd From c58a312a871eb46762569e76cb257a4d7506a7cc Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Tue, 13 Aug 2024 17:38:59 +0200 Subject: [PATCH 20/25] Expand description of filter in laser_filters_plugins.xml --- laser_filters_plugins.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index a437a48e..421d819e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -1,4 +1,4 @@ - +d @@ -63,7 +63,7 @@ - This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. + This is a spatial 1D median filter which filters sensor_msgs::msg::LaserScan messages. It smoothes continuous obstacles/walls and removes isolated/scattered noise. Date: Tue, 13 Aug 2024 17:51:31 +0200 Subject: [PATCH 21/25] Fix typo --- laser_filters_plugins.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 421d819e..624beb6e 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -1,4 +1,4 @@ -d + From e02d296ded13853072bb70f8569344e7c9f5803d Mon Sep 17 00:00:00 2001 From: Berend Kupers Date: Mon, 19 Aug 2024 09:49:54 +0200 Subject: [PATCH 22/25] Durability policy for publisher in speckle filter test --- test/test_speckle_filter.test.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/test_speckle_filter.test.py b/test/test_speckle_filter.test.py index 32b5fbcd..fcd951ca 100755 --- a/test/test_speckle_filter.test.py +++ b/test/test_speckle_filter.test.py @@ -18,6 +18,7 @@ import pytest import rclpy from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSDurabilityPolicy from sensor_msgs.msg import LaserScan @@ -91,7 +92,8 @@ def __init__(self): super().__init__("test_speckle_filter_distance") self.dist_msg_event_object = Event() self.eucl_msg_event_object = Event() - self.publisher = self.create_publisher(LaserScan, "scan", 10) + qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.publisher = self.create_publisher(LaserScan, "scan", qos_profile=qos) def wait_for_subscribers(self, timeout): timer_period = 0.1 From 5ea368f7c1ed166dcbb0b377d5cbe82a84d2ed3e Mon Sep 17 00:00:00 2001 From: Jon Binney Date: Tue, 20 Aug 2024 19:48:05 -0400 Subject: [PATCH 23/25] Fix race condition in tests We need to publish scans repeatedly in case the filter chain runs and processes the output scan before we finish subscribing to that topic. --- test/test_polygon_filter.test.py | 10 ++++++++-- test/test_speckle_filter.test.py | 18 ++++++++++-------- 2 files changed, 18 insertions(+), 10 deletions(-) diff --git a/test/test_polygon_filter.test.py b/test/test_polygon_filter.test.py index 838c6eab..a066e544 100755 --- a/test/test_polygon_filter.test.py +++ b/test/test_polygon_filter.test.py @@ -37,8 +37,14 @@ def test_polygon_filter(self): node.start_subscriber() node.publish_laser_scan() - msgs_received_flag = node._msg_event_object.wait(timeout=10.0) - assert msgs_received_flag, "Did not receive msgs !" + publish_rate = node.create_rate(5) + for _ in range(10): + if node._msg_event_object.isSet(): + break + node.publish_laser_scan() + publish_rate.sleep() + + assert node._msg_event_object.isSet(), "Did not receive msgs !" expected_scan_ranges = [1.0, 1.0, 1.0, 1.0, float("nan"), float("nan"), float("nan"), 1, 1, 1, 1] for scan_range, expected_scan_range in zip(node._received_message.ranges, expected_scan_ranges): if math.isnan(expected_scan_range) or math.isnan(scan_range): diff --git a/test/test_speckle_filter.test.py b/test/test_speckle_filter.test.py index fcd951ca..883f8ec3 100755 --- a/test/test_speckle_filter.test.py +++ b/test/test_speckle_filter.test.py @@ -18,7 +18,6 @@ import pytest import rclpy from rclpy.node import Node -from rclpy.qos import QoSProfile, QoSDurabilityPolicy from sensor_msgs.msg import LaserScan @@ -50,12 +49,16 @@ def test_speckle_filter(self): node = TestFixture() self.assertTrue(node.wait_for_subscribers(10)) node.start_subscribers() - node.publish_laser_scan() - dist_msgs_received_flag = node.dist_msg_event_object.wait(timeout=10.0) - eucl_msgs_received_flag = node.eucl_msg_event_object.wait(timeout=10.0) - assert dist_msgs_received_flag, "Did not receive distance msgs !" - assert eucl_msgs_received_flag, "Did not receive euclidean msgs !" + publish_rate = node.create_rate(5) + for _ in range(10): + if node.dist_msg_event_object.isSet() and node.eucl_msg_event_object.isSet(): + break + node.publish_laser_scan() + publish_rate.sleep() + + assert node.dist_msg_event_object.isSet(), "Did not receive distance msgs !" + assert node.eucl_msg_event_object.isSet(), "Did not receive euclidean msgs !" expected_scan_ranges = [1, 1, 1, 1, float("nan"), 1, 1, 1, 1, 1, 1] for scan_range, expected_scan_range in zip(node.msg_dist.ranges, expected_scan_ranges): @@ -92,8 +95,7 @@ def __init__(self): super().__init__("test_speckle_filter_distance") self.dist_msg_event_object = Event() self.eucl_msg_event_object = Event() - qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.publisher = self.create_publisher(LaserScan, "scan", qos_profile=qos) + self.publisher = self.create_publisher(LaserScan, "scan", 10) def wait_for_subscribers(self, timeout): timer_period = 0.1 From 044f45d2aab12952230e775749459ef9688f0bbe Mon Sep 17 00:00:00 2001 From: Yannic Bachmann Date: Fri, 23 Aug 2024 15:08:13 +0200 Subject: [PATCH 24/25] Added example launchfiles (xml+py) and example parameter yaml file. Added comment and warning when ensuring window_size_ is odd --- examples/median_spatial_filter.launch.py | 18 ++++++++++++++++++ examples/median_spatial_filter.launch.xml | 5 +++++ examples/median_spatial_filter_example.yaml | 17 +++++++++++++++++ include/laser_filters/median_spatial_filter.h | 2 ++ 4 files changed, 42 insertions(+) create mode 100644 examples/median_spatial_filter.launch.py create mode 100644 examples/median_spatial_filter.launch.xml create mode 100644 examples/median_spatial_filter_example.yaml diff --git a/examples/median_spatial_filter.launch.py b/examples/median_spatial_filter.launch.py new file mode 100644 index 00000000..ad00bcad --- /dev/null +++ b/examples/median_spatial_filter.launch.py @@ -0,0 +1,18 @@ +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", "median_spatial_filter_example.yaml", + ])], + ) + ]) diff --git a/examples/median_spatial_filter.launch.xml b/examples/median_spatial_filter.launch.xml new file mode 100644 index 00000000..a7e1ce29 --- /dev/null +++ b/examples/median_spatial_filter.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/examples/median_spatial_filter_example.yaml b/examples/median_spatial_filter_example.yaml new file mode 100644 index 00000000..bbe95e05 --- /dev/null +++ b/examples/median_spatial_filter_example.yaml @@ -0,0 +1,17 @@ +scan_to_scan_filter_chain: + ros__parameters: + filter1: + name: median_spatial + type: laser_filters/LaserScanMedianSpatialFilter + params: + window_size: 31 + filter2: + name: median_filter + type: laser_filters/LaserArrayFilter + params: + range_filter_chain: + filter1: + name: median + type: filters/MultiChannelMedianFilterFloat + params: + number_of_observations: 3 \ No newline at end of file diff --git a/include/laser_filters/median_spatial_filter.h b/include/laser_filters/median_spatial_filter.h index 5ea97532..3d3aae72 100644 --- a/include/laser_filters/median_spatial_filter.h +++ b/include/laser_filters/median_spatial_filter.h @@ -69,8 +69,10 @@ class LaserScanMedianSpatialFilter : public filters::FilterBaseget_logger(), "Window size must be odd. Automatically setting window_size to %d instead of %d.\n", window_size_+1, window_size_); window_size_ += 1; } From cd10537ef539112753f5e592b812a3f30357f086 Mon Sep 17 00:00:00 2001 From: Jon Binney Date: Sat, 24 Aug 2024 11:20:31 -0400 Subject: [PATCH 25/25] Add missing dependency on ament launch testing --- package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/package.xml b/package.xml index 512070ea..36b2edc2 100644 --- a/package.xml +++ b/package.xml @@ -28,6 +28,7 @@ tf2_kdl ament_cmake_gtest + launch_testing_ament_cmake ament_cmake