From 9d51f0d70ff94b557f2017bc7dbdc2006beba578 Mon Sep 17 00:00:00 2001 From: "Jimmy F. Klarke" Date: Tue, 2 Feb 2021 21:53:12 +0800 Subject: [PATCH] Add circle sector filter. --- CMakeLists.txt | 2 + cfg/SectorFilter.cfg | 56 +++++++++++ examples/sector_filter.yaml | 10 ++ examples/sector_filter_example.launch | 5 + include/laser_filters/sector_filter.h | 64 +++++++++++++ laser_filters_plugins.xml | 6 ++ src/laser_scan_filters.cpp | 3 +- src/sector_filter.cpp | 131 ++++++++++++++++++++++++++ 8 files changed, 276 insertions(+), 1 deletion(-) create mode 100644 cfg/SectorFilter.cfg create mode 100644 examples/sector_filter.yaml create mode 100644 examples/sector_filter_example.launch create mode 100644 include/laser_filters/sector_filter.h create mode 100644 src/sector_filter.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index da378ee6..0c2662dd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,7 @@ generate_dynamic_reconfigure_options( cfg/PolygonFilter.cfg cfg/ScanShadowsFilter.cfg cfg/SpeckleFilter.cfg + cfg/SectorFilter.cfg ) catkin_package( @@ -48,6 +49,7 @@ add_library(laser_scan_filters 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}) diff --git a/cfg/SectorFilter.cfg b/cfg/SectorFilter.cfg new file mode 100644 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/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/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/laser_filters_plugins.xml b/laser_filters_plugins.xml index 280a5ed1..ab726132 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -41,6 +41,12 @@ 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. + diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index a024f52b..0c5a8c71 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -41,6 +41,7 @@ #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" @@ -61,4 +62,4 @@ PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanPolygonFilter, filters::FilterBas 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/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