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