diff --git a/.github/workflows/ros1_ci.yml b/.github/workflows/ros1_ci.yml new file mode 100644 index 00000000..2b34fab8 --- /dev/null +++ b/.github/workflows/ros1_ci.yml @@ -0,0 +1,22 @@ +name: Build and run ROS tests + +on: + push: + branches: [ kinetic-devel ] + pull_request: + branches: [ kinetic-devel ] + +jobs: + build: + runs-on: ubuntu-latest + + container: + # The perception docker images includes laser_geometry, which we need. + image: ros:noetic-perception + + steps: + - uses: actions/checkout@v2 + + - name: Build and run tests + run: . /opt/ros/noetic/setup.sh && ./ci.sh + diff --git a/.travis.yml b/.travis.yml index 1ba25b90..df37fe82 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,7 +1,7 @@ # This config file for Travis CI utilizes ros-industrial/industrial_ci package. # For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst sudo: required -dist: trusty +dist: xenial language: generic compiler: - gcc @@ -11,10 +11,10 @@ notifications: on_failure: always env: matrix: - - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu - - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - USE_DEB=true ROS_DISTRO="jade" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - USE_DEB=true ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu + - USE_DEB=true ROS_DISTRO="lunar" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu + - USE_DEB=true ROS_DISTRO="lunar" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu install: - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config script: diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 6fba59e6..5898180d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,73 @@ Changelog for package laser_filters ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.8.11 (2020-06-03) +------------------- +* Merge pull request `#97 `_ from eurogroep/feat/speckle-filter-for-noise-removal +* Merge pull request `#96 `_ from eurogroep/feat/intensity-filter-dynamic-reconfigure-and-optionally-override-intensity-values + feat(IntensityFilter): Dynamic reconfigure and optionally override intensity +* Merge pull request `#3 `_ from nlimpert/nlimpert/speckle-filter-radius-outlier-merge + Merge distance based speckle filter with RadiusOutlier removal +* Contributors: Jonathan Binney, Nicolas Limpert, Rein Appeldoorn + +1.8.10 (2020-04-07) +------------------- +* radius_outlier_filter: new filter for radius based outlier removal + Add a new filter to remove measurements that do not have a number of + neighbors within a certain range. +* Contributors: Jonathan Binney, Nicolas Limpert + +1.8.9 (2020-04-05) +------------------ +* Bump CMake version to avoid CMP0048 warning +* Polygon filter +* Add dynamic reconfigure for scan shadows filter +* Parameter to remove shadow start point in scan shadows filter +* Contributors: Jonathan Binney, Rein Appeldoorn, Yannick_de_Hoop, ahcorde + +1.8.8 (2019-11-07) +------------------ +* Merge pull request `#83 `_ from remco-r/indigo-devel + [fix] when high fidelity true added laser_max_range\_ to projection +* [fix] when high fidelity true added laser_max_range\_ to projection +* Merge pull request `#79 `_ from Jailander/indigo-devel + Adding invert filter parameter to BOX filter +* Merge pull request `#80 `_ from k-okada/indigo-devel + Add scan blob filters +* add scan blob filters +* Merge pull request `#72 `_ from ms-iot/windows_port_tests_fix + [Windows][indigo] Use ${GTEST_LIBRARIES} for more portable gtest library linkage. +* Adding invert filter parameter to BOX filter +* Remove extra changes. +* windows bring up +* Contributors: Jonathan Binney, Kei Okada, Remco, Sean Yen, jailander + +1.8.7 (2019-06-13) +------------------ +* Merge pull request `#77 `_ from bionade24/indigo-devel + Removed boost signals from CMakeLists.txt +* Removed boost signals from CMakeLists.txt + With boost=>1.69 there `signals` isn't available anymore. As it's not necessary, it should be removed to be compatible to all boost versions. +* Merge pull request `#76 `_ from peci1/fix_travis + Fix Travis and move on to Kinetic and Lunar. +* Fix Travis and move on to Kinetic and Lunar. +* Merge pull request `#73 `_ from peci1/patch-1 + Added error message when the filter chain failed. +* Added error message when the filter chain failed. +* Merge pull request `#62 `_ from at-wat/optimize-shadows-filter + Reduce computation cost of ScanShadowsFilter +* Merge pull request `#63 `_ from procopiostein/indigo-devel + set values for variables that could be used uninitialized +* Add some comments to ScanShadowDetector +* set values for variables that could be used uninitialized +* Reduce computation cost of ScanShadowsFilter + ScanShadowsFilter required a lot of CPU power mainly due to atan2. + This commit reduces computation cost of the filter. + * Remove atan2 and directly compare tangent values + * Add a test to check geometric calculation +* Apply ROS recommended indent style to ScanShadowsFilter +* Contributors: Atsushi Watanabe, Jonathan Binney, Martin Pecka, Oskar Roesler, Procópio Stein + 1.8.6 (2018-04-11) ------------------ * Updated deprecated pluginlib macros to avoid warning messages diff --git a/CMakeLists.txt b/CMakeLists.txt index abb72fac..8917ced7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,21 +1,33 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(laser_filters) +set(CMAKE_CXX_STANDARD 11) + ############################################################################## # Find dependencies ############################################################################## set(THIS_PACKAGE_ROS_DEPS sensor_msgs roscpp tf filters message_filters - laser_geometry pluginlib angles nodelet) + laser_geometry pluginlib angles dynamic_reconfigure nodelet) find_package(catkin REQUIRED COMPONENTS ${THIS_PACKAGE_ROS_DEPS}) -find_package(Boost REQUIRED COMPONENTS system signals) +find_package(Boost REQUIRED COMPONENTS system) include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) ############################################################################## # Define package ############################################################################## +# dynamic reconfigure +generate_dynamic_reconfigure_options( + cfg/IntensityFilter.cfg + cfg/PolygonFilter.cfg + cfg/RangeFilter.cfg + cfg/ScanShadowsFilter.cfg + cfg/SpeckleFilter.cfg + cfg/SectorFilter.cfg +) + catkin_package( INCLUDE_DIRS include LIBRARIES pointcloud_filters laser_scan_filters @@ -30,7 +42,16 @@ catkin_package( add_library(pointcloud_filters src/pointcloud_filters.cpp) target_link_libraries(pointcloud_filters ${catkin_LIBRARIES}) -add_library(laser_scan_filters src/laser_scan_filters.cpp src/median_filter.cpp src/array_filter.cpp src/box_filter.cpp) +add_library(laser_scan_filters + src/laser_scan_filters.cpp + src/median_filter.cpp + src/array_filter.cpp + src/box_filter.cpp + src/polygon_filter.cpp + src/speckle_filter.cpp + src/intensity_filter.cpp + src/sector_filter.cpp +) target_link_libraries(laser_scan_filters ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(scan_to_cloud_filter_chain src/scan_to_cloud_filter_chain.cpp) @@ -46,13 +67,17 @@ target_link_libraries(scan_to_scan_filter_chain ${catkin_LIBRARIES} ${Boost_LIBR add_executable(generic_laser_filter_node src/generic_laser_filter_node.cpp) target_link_libraries(generic_laser_filter_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(laser_scan_filters ${PROJECT_NAME}_gencfg) + if (CATKIN_ENABLE_TESTING) find_package(rostest) add_executable(test_scan_filter_chain test/test_scan_filter_chain.cpp) - target_link_libraries(test_scan_filter_chain laser_scan_filters ${rostest_LIBRARIES} gtest) + target_link_libraries(test_scan_filter_chain laser_scan_filters ${rostest_LIBRARIES} ${GTEST_LIBRARIES}) add_dependencies(test_scan_filter_chain gtest) add_rostest(test/test_scan_filter_chain.launch) + add_rostest(test/test_polygon_filter.launch) + add_rostest(test/test_speckle_filter.launch) catkin_add_gtest(test_shadow_detector test/test_shadow_detector.cpp) target_link_libraries(test_shadow_detector ${catkin_LIBRARIES} ${rostest_LIBRARIES}) diff --git a/cfg/IntensityFilter.cfg b/cfg/IntensityFilter.cfg new file mode 100755 index 00000000..b0811b07 --- /dev/null +++ b/cfg/IntensityFilter.cfg @@ -0,0 +1,54 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Rein Appeldoorn + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +gen.add("lower_threshold", double_t, 0, + "Intensity values lower than this value will be filtered", 8000.0, 0, 100000.0) +gen.add("upper_threshold", double_t, 0, + "Intensity values greater than this value will be filtered", 100000.0, 0, 100000.0) +gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False) + +gen.add("filter_override_range", bool_t, 0, + "Whether to set the filtered laser beam ranges to NaN", True) +gen.add("filter_override_intensity", bool_t, 0, + "Whether to set the filtered and non-filtered laser beam intensities to 0.0 and 1.0 respectively", False) + +exit(gen.generate(PACKAGE, "laser_filters", "IntensityFilter")) diff --git a/cfg/PolygonFilter.cfg b/cfg/PolygonFilter.cfg new file mode 100755 index 00000000..dc142b7c --- /dev/null +++ b/cfg/PolygonFilter.cfg @@ -0,0 +1,48 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Yannick de Hoop, Rein Appeldoorn + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +gen.add("polygon", str_t, 0, + "A list of coordinates that represents a polygon in the format [ [x1, y1], [x2, y2], ..., [xn, yn] ]", "[]") +gen.add("polygon_padding", double_t, 0, "Polygon padding in meters", 0, 0, 100.) +gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False) + +exit(gen.generate(PACKAGE, "laser_filters", "PolygonFilter")) diff --git a/cfg/RangeFilter.cfg b/cfg/RangeFilter.cfg new file mode 100755 index 00000000..883e4c64 --- /dev/null +++ b/cfg/RangeFilter.cfg @@ -0,0 +1,55 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Rein Appeldoorn + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +gen.add("lower_threshold", double_t, 0, + "Range values lower than this value will be filtered", 0.0, 0.0, 100000.0) +gen.add("upper_threshold", double_t, 0, + "Range values greater than this value will be filtered", 100000.0, 0.0, 100000.0) +gen.add("use_message_range_limits", bool_t, 0, "Use the range_min and range_max values from the laserscan message as threshold. Defaults to false", False) + +gen.add("lower_replacement_value", double_t, 0, + "Use this value for all measurements upper_threshold. Default: NaN", 100000.0, 0.0, 100000.0) + +exit(gen.generate(PACKAGE, "laser_filters", "RangeFilter")) diff --git a/cfg/ScanShadowsFilter.cfg b/cfg/ScanShadowsFilter.cfg new file mode 100755 index 00000000..8842b89f --- /dev/null +++ b/cfg/ScanShadowsFilter.cfg @@ -0,0 +1,49 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Yannick de Hoop + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +gen.add("min_angle", double_t, 0, "Minimum perpendicular angle", 10, 0, 360) +gen.add("max_angle", double_t, 0, "Maximum perpendicular angle", 170, 0, 360) +gen.add("window", int_t, 0, "Number of consecutive measurements to consider angles inside of", 1, 0, 100) +gen.add("neighbors", int_t, 0, "Number of further-away neighbors to remove", 20, 0, 100) +gen.add("remove_shadow_start_point", bool_t, 0, "Whether to remove the shadow start point", False) + +exit(gen.generate(PACKAGE, "laser_filters", "ScanShadowsFilter")) diff --git a/cfg/SectorFilter.cfg b/cfg/SectorFilter.cfg new file mode 100755 index 00000000..fca58453 --- /dev/null +++ b/cfg/SectorFilter.cfg @@ -0,0 +1,56 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Rein Appeldoorn + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +gen.add("angle_min", double_t, 0, + "Min angle of the circle sector (radians)", 0.0, -3.14159265358979323846, 3.14159265358979323846) +gen.add("angle_max", double_t, 0, + "Max angle of the circle sector (radians)", 0.0, -3.14159265358979323846, 3.14159265358979323846) + +gen.add("range_min", double_t, 0, + "Min radius of the circle sector (meter)", 0.0, 0.0, 100000.0) +gen.add("range_max", double_t, 0, + "Max radius of the circle sector (meter)", 100000.0, 0.0, 100000.0) + +gen.add("clear_inside", bool_t, 0, "True to clear inside of the circle sector, False to clear outside. Default: true", True) +gen.add("invert", bool_t, 0, "A Boolean to invert the filter", False) + +exit(gen.generate(PACKAGE, "laser_filters", "SectorFilter")) diff --git a/cfg/SpeckleFilter.cfg b/cfg/SpeckleFilter.cfg new file mode 100755 index 00000000..74b43bd1 --- /dev/null +++ b/cfg/SpeckleFilter.cfg @@ -0,0 +1,54 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2020, Eurotec, Netherlands +# 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 TNO IVS 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 Rein Appeldoorn +# Nicolas Limpert + + +from dynamic_reconfigure.parameter_generator_catkin import * + +PACKAGE = "laser_filters" + +gen = ParameterGenerator() + +filter_type_enum = gen.enum([ gen.const("Distance", int_t, 0, "Range based filtering (distance between consecutive points)"), + gen.const("RadiusOutlier", int_t, 1, "Euclidean filtering based on radius outlier search")], + "Enum to select the filtering method") + +gen.add("filter_type", int_t, 0, "Filtering method selection", 0, 0, 1, edit_method=filter_type_enum) +gen.add("max_range", double_t, 0, "Only ranges smaller than this range are taken into account", 2.0, 0, 100) +gen.add("max_range_difference", double_t, 0, "Distance: max distance between consecutive points - RadiusOutlier: max distance between points", 0.4, 0, 100) +gen.add("filter_window", int_t, 0, "Minimum number of neighbors", 4, 0, 100) + +exit(gen.generate(PACKAGE, "laser_filters", "SpeckleFilter")) diff --git a/ci.sh b/ci.sh new file mode 100755 index 00000000..34231702 --- /dev/null +++ b/ci.sh @@ -0,0 +1,22 @@ +#!/bin/bash + +# Exit with any error. +set -e + +# Should be run from the root directory of the repo. +BUILD_DIR=build + +cmake -B ${BUILD_DIR} -DCATKIN_ENABLE_TESTING=1 + +# Build. +make -C ${BUILD_DIR} + +# Build the tests. +make -C ${BUILD_DIR} tests + +# Run the tests. +make -C ${BUILD_DIR} test + +# Summarize test results (also sets the exit status for the script) +catkin_test_results + diff --git a/examples/box_filter.yaml b/examples/box_filter.yaml index 72f26562..9dccb64f 100644 --- a/examples/box_filter.yaml +++ b/examples/box_filter.yaml @@ -9,3 +9,4 @@ scan_filter_chain: min_x: -0.45 min_y: -0.34 min_z: -0.28 + invert: false diff --git a/examples/footprint_filter_example.yaml b/examples/footprint_filter_example.yaml index c56c856a..f134e9f1 100644 --- a/examples/footprint_filter_example.yaml +++ b/examples/footprint_filter_example.yaml @@ -1,5 +1,5 @@ scan_filter_chain: - name: footprint_filter - type: LaserScanFootprintFilter + type: laser_filters/LaserScanFootprintFilter params: inscribed_radius: 0.325 diff --git a/examples/intensity_filter_example.yaml b/examples/intensity_filter_example.yaml index 77c4ddd5..3ecead5b 100644 --- a/examples/intensity_filter_example.yaml +++ b/examples/intensity_filter_example.yaml @@ -1,7 +1,9 @@ scan_filter_chain: - name: intensity - type: LaserScanIntensityFilter + type: laser_filters/LaserScanIntensityFilter params: lower_threshold: 8000 upper_threshold: 100000 - disp_histogram: 0 + invert: false + filter_override_range: true + filter_override_intensity: false diff --git a/examples/median_filter_5_example.yaml b/examples/median_filter_5_example.yaml index db91b301..a8f6a837 100644 --- a/examples/median_filter_5_example.yaml +++ b/examples/median_filter_5_example.yaml @@ -1,16 +1,16 @@ scan_filter_chain: -- type: LaserArrayFilter +- type: laser_filters/LaserArrayFilter name: laser_median_filter params: range_filter_chain: - name: median_5 - type: MultiChannelMedianFilterFloat + type: laser_filters/MultiChannelMedianFilterFloat params: number_of_observations: 5 unused: 10 intensity_filter_chain: - name: median_5 - type: MultiChannelMedianFilterFloat + type: laser_filters/MultiChannelMedianFilterFloat params: number_of_observations: 5 unused: 10 diff --git a/examples/multiple_filters_example.yaml b/examples/multiple_filters_example.yaml index b8fe7ac4..363cf125 100644 --- a/examples/multiple_filters_example.yaml +++ b/examples/multiple_filters_example.yaml @@ -1,34 +1,34 @@ scan_filter_chain: -- type: LaserArrayFilter +- type: laser_filters/LaserArrayFilter name: laser_median_5 params: range_filter_chain: - name: median_5 - type: MultiChannelMedianFilterFloat + type: laser_filters/MultiChannelMedianFilterFloat params: number_of_observations: 5 unused: 10 intensity_filter_chain: - name: median_5 - type: MultiChannelMedianFilterFloat + type: laser_filters/MultiChannelMedianFilterFloat params: number_of_observations: 5 unused: 10 - name: intensity - type: LaserScanIntensityFilter + type: laser_filters/LaserScanIntensityFilter params: lower_threshold: 8000 upper_threshold: 100000 disp_histogram: 0 - name: shadows - type: ScanShadowsFilter + type: laser_filters/ScanShadowsFilter params: min_angle: 10 max_angle: 170 neighbors: 20 window: 0 - name: dark_shadows - type: LaserScanIntensityFilter + type: laser_filters/LaserScanIntensityFilter params: lower_threshold: 100 upper_threshold: 10000 diff --git a/examples/point_cloud_footprint_filter_example.yaml b/examples/point_cloud_footprint_filter_example.yaml index 9576513d..84d739ef 100644 --- a/examples/point_cloud_footprint_filter_example.yaml +++ b/examples/point_cloud_footprint_filter_example.yaml @@ -1,5 +1,5 @@ cloud_filter_chain: -- type: PointCloudFootprintFilter +- type: laser_filters/PointCloudFootprintFilter name: footprint_filter params: inscribed_radius: 0.325 diff --git a/examples/polygon_filter.yaml b/examples/polygon_filter.yaml new file mode 100644 index 00000000..193682e5 --- /dev/null +++ b/examples/polygon_filter.yaml @@ -0,0 +1,7 @@ +scan_filter_chain: +- 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 diff --git a/examples/polygon_filter_example.launch b/examples/polygon_filter_example.launch new file mode 100644 index 00000000..2f984efc --- /dev/null +++ b/examples/polygon_filter_example.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/examples/sector_filter.yaml b/examples/sector_filter.yaml new file mode 100644 index 00000000..24945465 --- /dev/null +++ b/examples/sector_filter.yaml @@ -0,0 +1,10 @@ +scan_filter_chain: +- 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/examples/sector_filter_example.launch b/examples/sector_filter_example.launch new file mode 100644 index 00000000..49bca4f3 --- /dev/null +++ b/examples/sector_filter_example.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/examples/shadow_filter_example.yaml b/examples/shadow_filter_example.yaml index 1091f0a8..5f736357 100644 --- a/examples/shadow_filter_example.yaml +++ b/examples/shadow_filter_example.yaml @@ -1,13 +1,13 @@ scan_filter_chain: - name: shadows - type: ScanShadowsFilter + type: laser_filters/ScanShadowsFilter params: min_angle: 10 max_angle: 170 neighbors: 20 window: 1 - name: dark_shadows - type: LaserScanIntensityFilter + type: laser_filters/LaserScanIntensityFilter params: lower_threshold: 100 upper_threshold: 10000 diff --git a/examples/speckle_filter_example.launch b/examples/speckle_filter_example.launch new file mode 100644 index 00000000..71303409 --- /dev/null +++ b/examples/speckle_filter_example.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/examples/speckle_filter_example.yaml b/examples/speckle_filter_example.yaml new file mode 100644 index 00000000..6f8c6a74 --- /dev/null +++ b/examples/speckle_filter_example.yaml @@ -0,0 +1,19 @@ +scan_filter_chain: +- name: speckle_filter + type: laser_filters/LaserScanSpeckleFilter + params: + # Select which filter type to use. + # 0: Range based filtering (distance between consecutive points) + # 1: Euclidean filtering based on radius outlier search + filter_type: 0 + + # Only ranges smaller than this range are taken into account + max_range: 2.0 + + # filter_type[0] (Distance): max distance between consecutive points + # filter_type[1] (RadiusOutlier): max distance between points + max_range_difference: 0.1 + + # filter_type[0] (Distance): Number of consecutive ranges that will be tested for max_distance + # filter_type[1] (RadiusOutlier): Minimum number of neighbors + filter_window: 2 diff --git a/include/laser_filters/angular_bounds_filter.h b/include/laser_filters/angular_bounds_filter.h index e9f88b4d..903d653a 100644 --- a/include/laser_filters/angular_bounds_filter.h +++ b/include/laser_filters/angular_bounds_filter.h @@ -73,28 +73,53 @@ namespace laser_filters unsigned int count = 0; //loop through the scan and truncate the beginning and the end of the scan as necessary for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){ - //wait until we get to our desired starting angle - if(start_angle < lower_angle_){ - start_angle += input_scan.angle_increment; - current_angle += input_scan.angle_increment; - start_time += ros::Duration(input_scan.time_increment); - } - else{ - filtered_scan.ranges[count] = input_scan.ranges[i]; + if(input_scan.angle_increment > 0){ //if the laserscanner turns counterclockwise + //wait until we get to our desired starting angle + if(start_angle < lower_angle_){ + start_angle += input_scan.angle_increment; + current_angle += input_scan.angle_increment; + start_time += ros::Duration(input_scan.time_increment); + } + else{ + filtered_scan.ranges[count] = input_scan.ranges[i]; - //make sure that we don't update intensity data if its not available - if(input_scan.intensities.size() > i) - filtered_scan.intensities[count] = input_scan.intensities[i]; + //make sure that we don't update intensity data if its not available + if(input_scan.intensities.size() > i) + filtered_scan.intensities[count] = input_scan.intensities[i]; - count++; + count++; - //check if we need to break out of the loop, basically if the next increment will put us over the threshold - if(current_angle + input_scan.angle_increment > upper_angle_){ - break; + //check if we need to break out of the loop, basically if the next increment will put us over the threshold + if(current_angle + input_scan.angle_increment > upper_angle_){ + break; + } + current_angle += input_scan.angle_increment; + } + } + else{ //the laserscanner turns clockwise + //wait until we get to our desired starting angle + if(start_angle > upper_angle_){ + start_angle += input_scan.angle_increment; + current_angle += input_scan.angle_increment; + start_time += ros::Duration(input_scan.time_increment); } + else{ + filtered_scan.ranges[count] = input_scan.ranges[i]; - current_angle += input_scan.angle_increment; + //make sure that we don't update intensity data if its not available + if(input_scan.intensities.size() > i) + filtered_scan.intensities[count] = input_scan.intensities[i]; + count++; + + //check if we need to break out of the loop, basically if the next increment will put us over the threshold + if(current_angle + input_scan.angle_increment < lower_angle_){ + break; + } + + current_angle += input_scan.angle_increment; + + } } } diff --git a/include/laser_filters/box_filter.h b/include/laser_filters/box_filter.h index 7b3c1514..82cd74e3 100644 --- a/include/laser_filters/box_filter.h +++ b/include/laser_filters/box_filter.h @@ -82,6 +82,7 @@ class LaserScanBoxFilter : public filters::FilterBase // defines two opposite corners of the box tf::Point min_, max_; + bool invert_filter; bool up_and_running_; }; diff --git a/include/laser_filters/intensity_filter.h b/include/laser_filters/intensity_filter.h index 4c9cfbc9..93189ab6 100644 --- a/include/laser_filters/intensity_filter.h +++ b/include/laser_filters/intensity_filter.h @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. +* Copyright (c) 2020, Eurotec B.V. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -30,103 +31,32 @@ * 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 Vijay Pradeep, Rein Appeldoorn +* *********************************************************************/ -#ifndef LASER_SCAN_INTENSITY_FILTER_H -#define LASER_SCAN_INTENSITY_FILTER_H -/** -\author Vijay Pradeep -@b ScanIntensityFilter takes input scans and fiters out that are not within the specified range. The filtered out readings are set at >max_range in order to invalidate them. +#pragma once -**/ - - -#include "filters/filter_base.h" -#include "sensor_msgs/LaserScan.h" +#include +#include +#include +#include namespace laser_filters { - class LaserScanIntensityFilter : public filters::FilterBase { public: + LaserScanIntensityFilter(); + bool configure(); + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan); - double lower_threshold_ ; - double upper_threshold_ ; - int disp_hist_ ; - bool disp_hist_enabled_; - - bool configure() - { - lower_threshold_ = 8000.0; - upper_threshold_ = 100000.0; - disp_hist_ = 1; - getParam("lower_threshold", lower_threshold_); - getParam("upper_threshold", upper_threshold_) ; - getParam("disp_histogram", disp_hist_) ; - - disp_hist_enabled_ = (disp_hist_ == 0) ? false : true; - - return true; - } - - virtual ~LaserScanIntensityFilter(){} +private: + std::shared_ptr> dyn_server_; + void reconfigureCB(IntensityFilterConfig& config, uint32_t level); + boost::recursive_mutex own_mutex_; - bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) - { - const double hist_max = 4*12000.0 ; - const int num_buckets = 24 ; - int histogram[num_buckets] ; - for (int i=0; i < num_buckets; i++) - histogram[i] = 0 ; - - filtered_scan = input_scan; - - // Need to check ever reading in the current scan - for (unsigned int i=0; - i < input_scan.ranges.size() && i < input_scan.intensities.size(); - i++) - { - // Is this reading below our lower threshold? - // Is this reading above our upper threshold? - if (filtered_scan.intensities[i] <= lower_threshold_ || - filtered_scan.intensities[i] >= upper_threshold_) - { - // If so, then make it an invalid value (NaN) - filtered_scan.ranges[i] = std::numeric_limits::quiet_NaN(); - } - - // Calculate histogram - if (disp_hist_enabled_){ - // If intensity value is inf or NaN, skip voting histogram - if( std::isinf((double)filtered_scan.intensities[i]) || - std::isnan((double)filtered_scan.intensities[i]) ) - continue; - - // Choose bucket to vote on histogram, - // and check the index of bucket is in the histogram array - int cur_bucket = (int)(filtered_scan.intensities[i] / hist_max * num_buckets); - if (cur_bucket > num_buckets-1) - cur_bucket = num_buckets-1; - else if (cur_bucket < 0) cur_bucket = 0; - histogram[cur_bucket]++; - } - } - - // Display Histogram - if (disp_hist_enabled_) - { - printf("********** SCAN **********\n") ; - for (int i=0; i < num_buckets; i++) - { - printf("%u - %u: %u\n", (unsigned int) hist_max/num_buckets*i, - (unsigned int) hist_max/num_buckets*(i+1), - histogram[i]) ; - } - } - return true; - } + IntensityFilterConfig config_ = IntensityFilterConfig::__getDefault__(); }; } - -#endif // LASER_SCAN_INTENSITY_FILTER_H diff --git a/include/laser_filters/polygon_filter.h b/include/laser_filters/polygon_filter.h new file mode 100644 index 00000000..bc53cef1 --- /dev/null +++ b/include/laser_filters/polygon_filter.h @@ -0,0 +1,92 @@ +/* + * 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 + +namespace laser_filters +{ +/** + * @brief This is a filter that removes points in a laser scan inside of a polygon. + */ +class LaserScanPolygonFilter : public filters::FilterBase +{ +public: + LaserScanPolygonFilter(); + bool configure(); + + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan); + +private: + // configuration + ros::Publisher polygon_pub_; + std::shared_ptr> dyn_server_; + void reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level); + boost::recursive_mutex own_mutex_; + std::string polygon_frame_; + geometry_msgs::Polygon polygon_; + double polygon_padding_; + bool invert_filter_; + + // checks if points in polygon + bool inPolygon(tf::Point& point) const; + + laser_geometry::LaserProjection projector_; + + // tf listener to transform scans into the polygon_frame + tf::TransformListener tf_; +}; +} +#endif /* polygon_filter.h */ diff --git a/include/laser_filters/range_filter.h b/include/laser_filters/range_filter.h index 310cf455..e5b1b5ec 100644 --- a/include/laser_filters/range_filter.h +++ b/include/laser_filters/range_filter.h @@ -40,6 +40,8 @@ **/ +#include +#include #include "filters/filter_base.h" #include "sensor_msgs/LaserScan.h" @@ -48,34 +50,27 @@ namespace laser_filters class LaserScanRangeFilter : public filters::FilterBase { -public: + std::shared_ptr> dyn_server_; + boost::recursive_mutex own_mutex_; - double lower_threshold_ ; - double upper_threshold_ ; - bool use_message_range_limits_ ; - float lower_replacement_value_ ; - float upper_replacement_value_ ; + RangeFilterConfig config_ = RangeFilterConfig::__getDefault__(); +public: bool configure() { - use_message_range_limits_ = false; - getParam("use_message_range_limits", use_message_range_limits_); - - // work around the not implemented getParam(std::string name, float& value) method - double temp_replacement_value = std::numeric_limits::quiet_NaN(); - getParam("lower_replacement_value", temp_replacement_value); - lower_replacement_value_ = static_cast(temp_replacement_value); - - // work around the not implemented getParam(std::string name, float& value) method - temp_replacement_value = std::numeric_limits::quiet_NaN(); - getParam("upper_replacement_value", temp_replacement_value); - upper_replacement_value_ = static_cast(temp_replacement_value); - - - lower_threshold_ = 0.0; - upper_threshold_ = 100000.0; - getParam("lower_threshold", lower_threshold_); - getParam("upper_threshold", upper_threshold_) ; + ros::NodeHandle private_nh("~" + getName()); + dyn_server_.reset(new dynamic_reconfigure::Server(own_mutex_, private_nh)); + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&LaserScanRangeFilter::reconfigureCB, this, _1, _2); + dyn_server_->setCallback(f); + + getParam("lower_threshold", config_.lower_threshold); + getParam("upper_threshold", config_.upper_threshold); + getParam("use_message_range_limits", config_.use_message_range_limits); + getParam("lower_replacement_value", config_.lower_replacement_value); + getParam("upper_replacement_value", config_.upper_replacement_value); + + dyn_server_->updateConfig(config_); return true; } @@ -86,10 +81,13 @@ class LaserScanRangeFilter : public filters::FilterBase bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) { - if (use_message_range_limits_) + double lower_threshold = config_.lower_threshold; + double upper_threshold = config_.upper_threshold; + + if (config_.use_message_range_limits) { - lower_threshold_ = input_scan.range_min; - upper_threshold_ = input_scan.range_max; + lower_threshold = input_scan.range_min; + upper_threshold = input_scan.range_max; } filtered_scan = input_scan; for (unsigned int i=0; @@ -97,19 +95,24 @@ class LaserScanRangeFilter : public filters::FilterBase i++) // Need to check ever reading in the current scan { - if (filtered_scan.ranges[i] <= lower_threshold_) + if (filtered_scan.ranges[i] <= lower_threshold) { - filtered_scan.ranges[i] = lower_replacement_value_; + filtered_scan.ranges[i] = config_.lower_replacement_value; } - else if (filtered_scan.ranges[i] >= upper_threshold_) + else if (filtered_scan.ranges[i] >= upper_threshold) { - filtered_scan.ranges[i] = upper_replacement_value_; + filtered_scan.ranges[i] = config_.upper_replacement_value; } } return true; } + + void reconfigureCB(RangeFilterConfig& config, uint32_t level) + { + config_ = config; + } } ; } diff --git a/include/laser_filters/scan_blob_filter.h b/include/laser_filters/scan_blob_filter.h new file mode 100644 index 00000000..7acd5c0d --- /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 "filters/filter_base.h" +#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_)) + { + ROS_ERROR("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_)) + { + ROS_INFO("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::LaserScan& scan_in, sensor_msgs::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) ; + } + + ROS_DEBUG_STREAM("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); + } + } + ROS_DEBUG("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 7573bd7f..c821f016 100644 --- a/include/laser_filters/scan_shadows_filter.h +++ b/include/laser_filters/scan_shadows_filter.h @@ -43,6 +43,9 @@ #include "laser_filters/scan_shadow_detector.h" #include #include +#include +#include +#include namespace laser_filters { @@ -55,9 +58,14 @@ class ScanShadowsFilter : public filters::FilterBase double laser_max_range_; // Used in laser scan projection double min_angle_, max_angle_; // Filter angle threshold int window_, neighbors_; + bool remove_shadow_start_point_; // Whether to also remove the start point of the shadow ScanShadowDetector shadow_detector_; + std::shared_ptr> dyn_server_; + boost::recursive_mutex own_mutex_; + ScanShadowsFilterConfig param_config; + //////////////////////////////////////////////////////////////////////////////// ScanShadowsFilter() { @@ -66,6 +74,12 @@ class ScanShadowsFilter : public filters::FilterBase /**@b Configure the filter from XML */ bool 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::ScanShadowsFilter::reconfigureCB, this, _1, _2); + dyn_server_->setCallback(f); + if (!filters::FilterBase::getParam(std::string("min_angle"), min_angle_)) { ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n"); @@ -86,6 +100,9 @@ class ScanShadowsFilter : public filters::FilterBase { ROS_INFO("Error: ShadowsFilter was not given neighbors.\n"); } + remove_shadow_start_point_ = false; // default value + filters::FilterBase::getParam(std::string("remove_shadow_start_point"), remove_shadow_start_point_); + ROS_INFO("Remove shadow start point: %s", remove_shadow_start_point_ ? "true" : "false"); if (min_angle_ < 0) { @@ -107,13 +124,33 @@ class ScanShadowsFilter : public filters::FilterBase ROS_ERROR("max_angle must be max_angle <= 180. Forcing max_angle = 180.\n"); max_angle_ = 180.0; } + shadow_detector_.configure( angles::from_degrees(min_angle_), angles::from_degrees(max_angle_)); + param_config.min_angle = min_angle_; + param_config.max_angle = max_angle_; + param_config.window = window_; + param_config.neighbors = neighbors_; + param_config.remove_shadow_start_point = remove_shadow_start_point_; + dyn_server_->updateConfig(param_config); + return true; } + void reconfigureCB(ScanShadowsFilterConfig& config, uint32_t level) + { + min_angle_ = config.min_angle; + max_angle_ = config.max_angle; + shadow_detector_.configure( + angles::from_degrees(min_angle_), + angles::from_degrees(max_angle_)); + neighbors_ = config.neighbors; + window_ = config.window; + remove_shadow_start_point_ = config.remove_shadow_start_point; + } + //////////////////////////////////////////////////////////////////////////////// virtual ~ScanShadowsFilter() { @@ -129,6 +166,8 @@ class ScanShadowsFilter : public filters::FilterBase */ bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out) { + boost::recursive_mutex::scoped_lock lock(own_mutex_); + // copy across all data first scan_out = scan_in; @@ -154,6 +193,10 @@ class ScanShadowsFilter : public filters::FilterBase indices_to_delete.insert(index); } } + if (remove_shadow_start_point_) + { + indices_to_delete.insert(i); + } } } } diff --git a/include/laser_filters/sector_filter.h b/include/laser_filters/sector_filter.h new file mode 100644 index 00000000..808a049b --- /dev/null +++ b/include/laser_filters/sector_filter.h @@ -0,0 +1,64 @@ +/********************************************************************* +* 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 + +#include +#include + +namespace laser_filters +{ + +class LaserScanSectorFilter : public filters::FilterBase +{ +public: + LaserScanSectorFilter(); + bool configure(); + bool isClearInside(); + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan); + + virtual ~LaserScanSectorFilter(){} + +private: + std::shared_ptr> dyn_server_; + void reconfigureCB(SectorFilterConfig& config, uint32_t level); + boost::recursive_mutex own_mutex_; + + SectorFilterConfig config_ = SectorFilterConfig::__getDefault__(); +}; + +} // 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 new file mode 100644 index 00000000..30876049 --- /dev/null +++ b/include/laser_filters/speckle_filter.h @@ -0,0 +1,172 @@ +/* + * 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.h + */ + +#ifndef SPECKLE_FILTER_H +#define SPECKLE_FILTER_H + +#include +#include +#include +#include + +namespace laser_filters +{ + +class WindowValidator +{ +public: + virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_range_difference) = 0; +}; + +class DistanceWindowValidator : public WindowValidator +{ + virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_range_difference) + { + const float& range = scan.ranges[idx]; + if (range != range) { + return false; + } + + for (size_t neighbor_idx_nr = 1; neighbor_idx_nr < window; ++neighbor_idx_nr) + { + size_t neighbor_idx = idx + neighbor_idx_nr; + if (neighbor_idx < scan.ranges.size()) // Out of bound check + { + const float& neighbor_range = scan.ranges[neighbor_idx]; + if (neighbor_range != neighbor_range || fabs(neighbor_range - range) > max_range_difference) + { + return false; + } + } + } + return true; + } +}; + +class RadiusOutlierWindowValidator : public WindowValidator +{ + virtual bool checkWindowValid(const sensor_msgs::LaserScan& scan, size_t idx, size_t window, double max_distance) + { + int num_neighbors = 0; + const float& r1 = scan.ranges[idx]; + float r2 = 0.; + + // Look around the current point until either the window is exceeded + // or the number of neighbors was found. + for (int y = -(int)window; y < (int)window + 1 && num_neighbors < (int)window; y++) + { + int j = idx + y; + r2 = scan.ranges[j]; + + if (j < 0 || j >= static_cast(scan.ranges.size()) || idx == j || std::isnan(r2)) + { // Out of scan bounds or itself or infinity + continue; + } + + // Explanation: + // + // Distance between two points: + // d² = (x2 - x1)² + (y2 - y1)² + // + // Substitute x with r * cos(phi) and y with r * sin(phi): + // d² = (r2 * cos(phi2) - r1 * cos(phi1))² + (r2 * sin(phi2) - r1 * sin(phi1))² + // + // Apply binomial theorem: + // d² = ((r2² * cos(phi2)² + r1² * cos(phi1)² - 2 * r1 * r2 * cos(phi1) * cos(phi2)) + + // ((r2² * sin(phi2)² + r1² * sin(phi1)² - 2 * r1 * r2 * sin(phi1) * sin(phi2)) + // + // Merge sums: + // d² = r2² * (cos(phi2)² + sin(phi2)²) + r1² * (cos(phi1)² + sin(phi1)² - + // 2 * r1 * r2 * (cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2)) + // + // Apply cos² + sin² = 1: + // d² = r2² + r1² - 2 * r1 * r2 * (cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2)) + // + // Note the following: + // cos(phi1) * cos(phi2) = 1/2 * (cos(phi1 - phi2) + cos(phi1 + phi2)) + // sin(phi1) * sin(phi2) = 1/2 * (cos(phi1 - phi2) - cos(phi1 + phi2)) + // + // cos(phi1) * cos(phi2) + sin(phi1) * sin(phi2) = cos(phi1 - phi2) + // + // Finally, phi1 - phi2 is our included_angle. + + const float d = sqrt( + pow(r1,2) + pow(r2,2) - + (2 * r1 * r2 * cosf(y * scan.angle_increment))); + + + if (d <= max_distance) + { + num_neighbors++; + } + } + + // consider the window to be the number of neighbors we need + if (num_neighbors < window) + { + return false; + } + else + { + return true; + } + } +}; + +/** + * @brief This is a filter that removes speckle points in a laser scan based on consecutive ranges + */ +class LaserScanSpeckleFilter : public filters::FilterBase +{ +public: + LaserScanSpeckleFilter(); + ~LaserScanSpeckleFilter(); + bool configure(); + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan); + +private: + std::shared_ptr> dyn_server_; + void reconfigureCB(laser_filters::SpeckleFilterConfig& config, uint32_t level); + boost::recursive_mutex own_mutex_; + + SpeckleFilterConfig config_ = SpeckleFilterConfig::__getDefault__(); + WindowValidator* validator_; +}; +} +#endif /* speckle_filter.h */ diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index f112da24..ab726132 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -41,12 +41,30 @@ This is a filter that removes points in a laser scan inside of certain angular bounds. + + + + 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 cartesian box. + + + + This is a filter that removes points in a laser scan inside of a polygon. + + + + + This is a filter that removes speckle points in a laser scan by looking at neighbor points. + @@ -66,6 +84,13 @@ 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. + + - 1.8.6 + 1.8.11 Jon Binney Tully Foote BSD @@ -13,6 +13,7 @@ catkin + dynamic_reconfigure sensor_msgs roscpp tf @@ -24,6 +25,7 @@ angles nodelet + dynamic_reconfigure sensor_msgs roscpp tf diff --git a/src/box_filter.cpp b/src/box_filter.cpp index eba24a78..bf6bb534 100644 --- a/src/box_filter.cpp +++ b/src/box_filter.cpp @@ -59,7 +59,10 @@ bool laser_filters::LaserScanBoxFilter::configure(){ bool x_min_set = getParam("min_x", min_x); bool y_min_set = getParam("min_y", min_y); bool z_min_set = getParam("min_z", min_z); + bool invert_set = getParam("invert", invert_filter); + ROS_INFO("BOX filter started"); + max_.setX(max_x); max_.setY(max_y); max_.setZ(max_z); @@ -88,6 +91,11 @@ bool laser_filters::LaserScanBoxFilter::configure(){ if(!z_min_set){ ROS_ERROR("min_z is not set!"); } + if(!invert_set){ + ROS_INFO("invert filter not set, assuming false"); + invert_filter=false; + } + return box_frame_set && x_max_set && y_max_set && z_max_set && x_min_set && y_min_set && z_min_set; @@ -174,9 +182,17 @@ bool laser_filters::LaserScanBoxFilter::update( tf::Point point(x, y, z); - if(inBox(point)){ - output_scan.ranges[index] = std::numeric_limits::quiet_NaN(); + if(!invert_filter){ + if(inBox(point)){ + output_scan.ranges[index] = std::numeric_limits::quiet_NaN(); + } } + else{ + if(!inBox(point)){ + output_scan.ranges[index] = std::numeric_limits::quiet_NaN(); + } + } + } up_and_running_ = true; return true; diff --git a/src/intensity_filter.cpp b/src/intensity_filter.cpp new file mode 100644 index 00000000..b35f1fe7 --- /dev/null +++ b/src/intensity_filter.cpp @@ -0,0 +1,112 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* 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: +* +* * 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 Vijay Pradeep, Rein Appeldoorn +* +*********************************************************************/ + +#include +#include + +namespace laser_filters +{ +LaserScanIntensityFilter::LaserScanIntensityFilter() +{ +} + +bool LaserScanIntensityFilter::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(&LaserScanIntensityFilter::reconfigureCB, this, _1, _2); + dyn_server_->setCallback(f); + + getParam("lower_threshold", config_.lower_threshold); + getParam("upper_threshold", config_.upper_threshold); + getParam("invert", config_.invert); + + getParam("filter_override_range", config_.filter_override_range); + getParam("filter_override_intensity", config_.filter_override_intensity); + dyn_server_->updateConfig(config_); + return true; +} + +bool LaserScanIntensityFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) +{ + filtered_scan = input_scan; + + // Need to check ever reading in the current scan + for (unsigned int i=0; i < input_scan.ranges.size() && i < input_scan.intensities.size(); i++) + { + float& range = filtered_scan.ranges[i]; + float& intensity = filtered_scan.intensities[i]; + + // Is this reading below our lower threshold? + // Is this reading above our upper threshold? + bool filter = intensity <= config_.lower_threshold || intensity >= config_.upper_threshold; + if (config_.invert) + { + filter = !filter; + } + + if (filter) + { + if (config_.filter_override_range) + { + // If so, then make it an invalid value (NaN) + range = std::numeric_limits::quiet_NaN(); + } + if (config_.filter_override_intensity) + { + intensity = 0.0; // Not intense + } + } + else + { + if (config_.filter_override_intensity) + { + intensity = 1.0; // Intense + } + } + } + + return true; +} + +void LaserScanIntensityFilter::reconfigureCB(IntensityFilterConfig& config, uint32_t level) +{ + config_ = config; +} +} diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index 8c2f23d7..0c5a8c71 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -38,6 +38,10 @@ #include "laser_filters/angular_bounds_filter.h" #include "laser_filters/angular_bounds_filter_in_place.h" #include "laser_filters/box_filter.h" +#include "laser_filters/polygon_filter.h" +#include "laser_filters/speckle_filter.h" +#include "laser_filters/scan_blob_filter.h" +#include "laser_filters/sector_filter.h" #include "sensor_msgs/LaserScan.h" #include "filters/filter_base.h" @@ -54,4 +58,8 @@ PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanFootprintFilter, filters::FilterB PLUGINLIB_EXPORT_CLASS(laser_filters::ScanShadowsFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::InterpolationFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanBoxFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSpeckleFilter, filters::FilterBase) PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMaskFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::ScanBlobFilter, filters::FilterBase) +PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSectorFilter, filters::FilterBase) diff --git a/src/polygon_filter.cpp b/src/polygon_filter.cpp new file mode 100644 index 00000000..125ea8f0 --- /dev/null +++ b/src/polygon_filter.cpp @@ -0,0 +1,413 @@ +/* + * 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.cpp + */ + +#include "laser_filters/polygon_filter.h" +#include +#include +#include +#include +#include // for EOF +#include +#include +#include + +/** @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::Polygon& polygon, double padding) +{ + // pad polygon in place + for (unsigned int i = 0; i < polygon.points.size(); i++) + { + geometry_msgs::Point32& pt = polygon.points[ i ]; + pt.x += sign0(pt.x) * padding; + pt.y += sign0(pt.y) * padding; + } +} + +double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +{ + // Make sure that the value we're looking at is either a double or an int. + if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && value.getType() != XmlRpc::XmlRpcValue::TypeDouble) + { + std::string& value_string = value; + ROS_FATAL("Values in the polygon specification (param %s) must be numbers. Found value %s.", + full_param_name.c_str(), value_string.c_str()); + throw std::runtime_error("Values in the polygon specification must be numbers"); + } + return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); +} + +geometry_msgs::Polygon makePolygonFromXMLRPC(const XmlRpc::XmlRpcValue& polygon_xmlrpc, + const std::string& full_param_name) +{ + // Make sure we have an array of at least 3 elements. + if (polygon_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || + polygon_xmlrpc.size() > 0 && polygon_xmlrpc.size() < 3) + { + ROS_FATAL("The polygon (parameter %s) must be specified as nested list on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]", + full_param_name.c_str()); + + throw std::runtime_error("The polygon must be specified as nested list on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); + } + geometry_msgs::Polygon polygon; + geometry_msgs::Point32 pt; + + for (int i = 0; i < polygon_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue point = polygon_xmlrpc[i]; + if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2) + { + ROS_FATAL("The polygon (parameter %s) must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", + full_param_name.c_str()); + throw std::runtime_error("The polygon must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); + } + + pt.x = getNumberFromXMLRPC(point[0], full_param_name); + pt.y = getNumberFromXMLRPC(point[1], full_param_name); + + polygon.points.push_back(pt); + } + return polygon; +} + +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::Polygon makePolygonFromString(const std::string& polygon_string, const geometry_msgs::Polygon& last_polygon) +{ + std::string error; + std::vector > vvf = parseVVF(polygon_string, error); + + if (error != "") + { + ROS_ERROR("Error parsing polygon parameter: '%s'", error.c_str()); + ROS_ERROR(" Polygon string was '%s'.", polygon_string.c_str()); + return last_polygon; + } + + geometry_msgs::Polygon polygon; + geometry_msgs::Point32 point; + + // convert vvf into points. + if (vvf.size() < 3 && vvf.size() > 0) + { + ROS_WARN("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 + { + ROS_ERROR("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::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; +} + +namespace laser_filters{ +LaserScanPolygonFilter::LaserScanPolygonFilter() +{ +} + +bool LaserScanPolygonFilter::configure() +{ + XmlRpc::XmlRpcValue polygon_xmlrpc; + std::string polygon_string; + PolygonFilterConfig param_config; + + 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::LaserScanPolygonFilter::reconfigureCB, this, _1, _2); + dyn_server_->setCallback(f); + + bool polygon_set = getParam("polygon", polygon_xmlrpc); + bool polygon_frame_set = getParam("polygon_frame", polygon_frame_); + bool invert_set = getParam("invert", invert_filter_); + polygon_ = makePolygonFromXMLRPC(polygon_xmlrpc, "polygon"); + + double polygon_padding = 0; + getParam("polygon_padding", polygon_padding); + + polygon_string = polygonToString(polygon_); + param_config.polygon = polygon_string; + param_config.polygon_padding = polygon_padding; + param_config.invert = invert_filter_; + dyn_server_->updateConfig(param_config); + + polygon_pub_ = private_nh.advertise("polygon", 1); + + if (!polygon_frame_set) + { + ROS_ERROR("polygon_frame is not set!"); + } + if (!polygon_set) + { + ROS_ERROR("polygon is not set!"); + } + if (!invert_set) + { + ROS_INFO("invert filter not set, assuming false"); + invert_filter_ = false; + } + + return polygon_frame_set && polygon_set; +} + +bool LaserScanPolygonFilter::update(const sensor_msgs::LaserScan& input_scan, + sensor_msgs::LaserScan& output_scan) +{ + boost::recursive_mutex::scoped_lock lock(own_mutex_); + + geometry_msgs::PolygonStamped polygon_stamped; + polygon_stamped.header.frame_id = polygon_frame_; + polygon_stamped.header.stamp = ros::Time::now(); + polygon_stamped.polygon = polygon_; + polygon_pub_.publish(polygon_stamped); + + output_scan = input_scan; + + sensor_msgs::PointCloud2 laser_cloud; + + std::string error_msg; + + bool success = tf_.waitForTransform( + polygon_frame_, input_scan.header.frame_id, + input_scan.header.stamp + ros::Duration().fromSec(input_scan.ranges.size() * input_scan.time_increment), + ros::Duration(1.0), ros::Duration(0.01), &error_msg); + if (!success) + { + ROS_WARN("Could not get transform, ignoring laser scan! %s", error_msg.c_str()); + return false; + } + + try + { + projector_.transformLaserScanToPointCloud(polygon_frame_, input_scan, laser_cloud, tf_); + } + catch (tf::TransformException& ex) + { + ROS_INFO_THROTTLE(.3, "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) + { + ROS_INFO_THROTTLE(.3, "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])); + + tf::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(); + } + } + } + + return true; +} + + +bool LaserScanPolygonFilter::inPolygon(tf::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 LaserScanPolygonFilter::reconfigureCB(laser_filters::PolygonFilterConfig& config, uint32_t level) +{ + invert_filter_ = config.invert; + polygon_ = makePolygonFromString(config.polygon, polygon_); + padPolygon(polygon_, config.polygon_padding); +} +} diff --git a/src/scan_to_cloud_filter_chain.cpp b/src/scan_to_cloud_filter_chain.cpp index f2ef8042..b07a6d71 100644 --- a/src/scan_to_cloud_filter_chain.cpp +++ b/src/scan_to_cloud_filter_chain.cpp @@ -62,7 +62,8 @@ #define LASER_WARN ROS_WARN #endif -/** @b ScanShadowsFilter is a simple node that filters shadow points in a laser scan line and publishes the results in a cloud. + +/** @b ScanToCloudFilterChain combines scan filtering, scan->cloud conversion and cloud filtering. */ class ScanToCloudFilterChain { @@ -92,6 +93,7 @@ class ScanToCloudFilterChain filters::FilterChain cloud_filter_chain_; filters::FilterChain scan_filter_chain_; ros::Publisher cloud_pub_; + unsigned int channel_options_; // Timer for displaying deprecation warnings ros::Timer deprecation_timer_; @@ -135,6 +137,24 @@ class ScanToCloudFilterChain private_nh.param("cloud_topic", cloud_topic_, std::string("tilt_laser_cloud_filtered")); private_nh.param("incident_angle_correction", incident_angle_correction_, true); + channel_options_ = laser_geometry::channel_option::None; + bool hasChannel; + private_nh.param("cloud_channel_intensity", hasChannel, true); + if (hasChannel) + channel_options_ |= laser_geometry::channel_option::Intensity; + private_nh.param("cloud_channel_index", hasChannel, true); + if (hasChannel) + channel_options_ |= laser_geometry::channel_option::Index; + private_nh.param("cloud_channel_distance", hasChannel, true); + if (hasChannel) + channel_options_ |= laser_geometry::channel_option::Distance; + private_nh.param("cloud_channel_timestamp", hasChannel, true); + if (hasChannel) + channel_options_ |= laser_geometry::channel_option::Timestamp; + private_nh.param("cloud_channel_viewpoint", hasChannel, false); + if (hasChannel) + channel_options_ |= laser_geometry::channel_option::Viewpoint; + filter_.setTargetFrame(target_frame_); filter_.registerCallback(boost::bind(&ScanToCloudFilterChain::scanCallback, this, _1)); filter_.setTolerance(ros::Duration(tf_tolerance_)); @@ -233,16 +253,12 @@ class ScanToCloudFilterChain } // Transform into a PointCloud message - int mask = laser_geometry::channel_option::Intensity | - laser_geometry::channel_option::Distance | - laser_geometry::channel_option::Index | - laser_geometry::channel_option::Timestamp; if (high_fidelity_) { try { - projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, mask); + projector_.transformLaserScanToPointCloud (target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, channel_options_); } catch (tf::TransformException &ex) { @@ -253,7 +269,7 @@ class ScanToCloudFilterChain } else { - projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, mask); + projector_.transformLaserScanToPointCloud(target_frame_, filtered_scan, scan_cloud, tf_, laser_max_range_, channel_options_); } sensor_msgs::PointCloud2 filtered_cloud; diff --git a/src/sector_filter.cpp b/src/sector_filter.cpp new file mode 100644 index 00000000..a47df68b --- /dev/null +++ b/src/sector_filter.cpp @@ -0,0 +1,131 @@ +/********************************************************************* +* 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 +*********************************************************************/ + +#include + +#include +#include + +namespace laser_filters +{ + +LaserScanSectorFilter::LaserScanSectorFilter() +{ +} + +bool LaserScanSectorFilter::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(&LaserScanSectorFilter::reconfigureCB, this, _1, _2); + dyn_server_->setCallback(f); + + getParam("angle_min", config_.angle_min); + getParam("angle_max", config_.angle_max); + getParam("range_min", config_.range_min); + getParam("range_max", config_.range_max); + getParam("clear_inside", config_.clear_inside); + getParam("invert", config_.invert); + + ROS_INFO("clear_inside(!invert): %s", (isClearInside() ? "true" : "false")); + + dyn_server_->updateConfig(config_); + return true; +} + +void LaserScanSectorFilter::reconfigureCB(SectorFilterConfig& config, uint32_t level) +{ + config_ = config; +} + +bool LaserScanSectorFilter::isClearInside() +{ + bool clear_inside = config_.clear_inside; + bool invert = config_.invert; + + clear_inside = invert ? false : clear_inside; + + return clear_inside; +} + +bool LaserScanSectorFilter::update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) +{ + filtered_scan = input_scan; //copy entire message + + double angle_min = config_.angle_min; + double angle_max = config_.angle_max; + double range_min = config_.range_min; + double range_max = config_.range_max; + 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; + } + + filtered_scan.ranges[i] = input_scan.range_max + 1.0; + if (i < filtered_scan.intensities.size()) + { + filtered_scan.intensities[i] = 0.0; + } + count++; + } + + ROS_DEBUG("Filtered out %u points from the laser scan.", count); + + return true; +} + +} // end namespace laser_filters diff --git a/src/speckle_filter.cpp b/src/speckle_filter.cpp new file mode 100644 index 00000000..7f6b12e1 --- /dev/null +++ b/src/speckle_filter.cpp @@ -0,0 +1,132 @@ +/* + * 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 b/test/test_polygon_filter new file mode 100755 index 00000000..143009a5 --- /dev/null +++ b/test/test_polygon_filter @@ -0,0 +1,45 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020, Eurotec, Netherlands +# All rights reserved. +# +# \author Rein Appeldoorn + +import math +import unittest + +import rospy +import rostest +from sensor_msgs.msg import LaserScan +from std_msgs.msg import Header + + +class TestPolygonFilter(unittest.TestCase): + rospy.init_node('test_polygon_filter') + 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] * num_beams, + range_max=100 + )) + + def test_polygon_filter(self): + msg = rospy.wait_for_message("scan_filtered", LaserScan, 10.) # type: LaserScan + 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(msg.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_polygon_filter' +if __name__ == '__main__': + rostest.unitrun(PKG, NAME, TestPolygonFilter) diff --git a/test/test_polygon_filter.launch b/test/test_polygon_filter.launch new file mode 100644 index 00000000..47a0da15 --- /dev/null +++ b/test/test_polygon_filter.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/test/test_polygon_filter.yaml b/test/test_polygon_filter.yaml new file mode 100644 index 00000000..aa8757ed --- /dev/null +++ b/test/test_polygon_filter.yaml @@ -0,0 +1,7 @@ +scan_filter_chain: +- name: polygon_filter + type: laser_filters/LaserScanPolygonFilter + params: + polygon_frame: laser + polygon: [[0, 0.5], [2, 0.5], [2, -0.5], [0, -0.5]] + invert: false diff --git a/test/test_speckle_filter b/test/test_speckle_filter new file mode 100755 index 00000000..641904ed --- /dev/null +++ b/test/test_speckle_filter @@ -0,0 +1,78 @@ +#!/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 new file mode 100644 index 00000000..4fb5bdfd --- /dev/null +++ b/test/test_speckle_filter.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/test/test_speckle_filter_distance.yaml b/test/test_speckle_filter_distance.yaml new file mode 100644 index 00000000..f1100c94 --- /dev/null +++ b/test/test_speckle_filter_distance.yaml @@ -0,0 +1,8 @@ +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 new file mode 100644 index 00000000..714332ce --- /dev/null +++ b/test/test_speckle_filter_euclidean.yaml @@ -0,0 +1,8 @@ +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