diff --git a/CMakeLists.txt b/CMakeLists.txt index 02eb5896..e7638cd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,7 +43,7 @@ ament_auto_add_executable(generic_laser_filter_node src/generic_laser_filter_nod ############################################################################## 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 @@ -77,4 +77,36 @@ if(BUILD_TESTING) --gtest_output=xml:${RESULT_FILENAME} 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} + ) + + 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) 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/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/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/polygon_filter.h b/include/laser_filters/polygon_filter.h new file mode 100644 index 00000000..3d4aa33b --- /dev/null +++ b/include/laser_filters/polygon_filter.h @@ -0,0 +1,619 @@ +/* + * 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 + + +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 != "") + { + return last_polygon; + } + + geometry_msgs::msg::Polygon polygon; + geometry_msgs::msg::Point32 point; + + // convert vvf into points. + if (vvf.size() < 3 && vvf.size() > 0) + { + 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 + { + 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; +} + +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 +{ +/** + * @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 = 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) + { + 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, + "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/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/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/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/include/laser_filters/speckle_filter.h b/include/laser_filters/speckle_filter.h index 3a3ca0a1..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; } @@ -253,18 +248,6 @@ class LaserScanSpeckleFilter : public filters::FilterBase parameters) { auto result = rcl_interfaces::msg::SetParametersResult(); @@ -272,7 +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) { @@ -310,6 +294,15 @@ class LaserScanSpeckleFilter : public filters::FilterBase + + 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. + + + + 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 6e095fc1..512070ea 100644 --- a/package.xml +++ b/package.xml @@ -23,7 +23,9 @@ rclcpp_components sensor_msgs tf2 + tf2_geometry_msgs tf2_ros + tf2_kdl ament_cmake_gtest diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index c1791cf6..82141fce 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -40,6 +40,9 @@ #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/sector_filter.h" +#include "laser_filters/polygon_filter.h" #include @@ -59,3 +62,7 @@ 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::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 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; - } - -} -} diff --git a/test/test_polygon_filter.test.py b/test/test_polygon_filter.test.py new file mode 100755 index 00000000..a066e544 --- /dev/null +++ b/test/test_polygon_filter.test.py @@ -0,0 +1,103 @@ +#!/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() + + 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): + 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 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 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.cpp b/test/test_speckle_filter.cpp new file mode 100644 index 00000000..75980d39 --- /dev/null +++ b/test/test_speckle_filter.cpp @@ -0,0 +1,358 @@ +#include +#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(); +} 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..883f8ec3 --- /dev/null +++ b/test/test_speckle_filter.test.py @@ -0,0 +1,146 @@ +#!/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() + + 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): + 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) + rclpy.shutdown() + node.join_thread() + + +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 join_thread(self): + self.ros_spin_thread.join() + + 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