Merge pull request ros-perception#105 from isjfk/laser_filter_sector

Add circle sector sharp filter

CMakeLists.txt

@@ -25,6 +25,7 @@ generate_dynamic_reconfigure_options(
   cfg/RangeFilter.cfg
   cfg/ScanShadowsFilter.cfg
   cfg/SpeckleFilter.cfg
+  cfg/SectorFilter.cfg
 )
 
 catkin_package(
@@ -49,6 +50,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})
 

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"))

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

examples/sector_filter_example.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
+    <rosparam command="load" file="$(find laser_filters)/examples/sector_filter.yaml" />
+  </node>
+</launch>

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 <dynamic_reconfigure/server.h>
+#include <laser_filters/SectorFilterConfig.h>
+
+#include <filters/filter_base.h>
+#include <sensor_msgs/LaserScan.h>
+
+namespace laser_filters
+{
+
+class LaserScanSectorFilter : public filters::FilterBase<sensor_msgs::LaserScan>
+{
+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<dynamic_reconfigure::Server<SectorFilterConfig>> 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

laser_filters_plugins.xml

@@ -41,6 +41,12 @@
       <description>
 	 This is a filter that removes points in a laser scan inside of certain angular bounds.
       </description>
+    </class>
+    <class name="laser_filters/LaserScanSectorFilter" type="laser_filters::LaserScanSectorFilter" 
+	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">
+      <description>
+        This is a filter that removes points in a laser scan inside/outside of a circle sector.
+      </description>
     </class>
      <class name="laser_filters/LaserScanBoxFilter" type="laser_filters::LaserScanBoxFilter" 
 	    base_class_type="filters::FilterBase<sensor_msgs::LaserScan>">

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<sensor_msgs::LaserScan>)
 PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanMaskFilter, filters::FilterBase<sensor_msgs::LaserScan>)
 PLUGINLIB_EXPORT_CLASS(laser_filters::ScanBlobFilter, filters::FilterBase<sensor_msgs::LaserScan>)
-
+PLUGINLIB_EXPORT_CLASS(laser_filters::LaserScanSectorFilter, filters::FilterBase<sensor_msgs::LaserScan>)

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 <math.h>
+
+#include <laser_filters/sector_filter.h>
+#include <ros/node_handle.h>
+
+namespace laser_filters
+{
+
+LaserScanSectorFilter::LaserScanSectorFilter()
+{
+}
+
+bool LaserScanSectorFilter::configure()
+{
+  ros::NodeHandle private_nh("~" + getName());
+  dyn_server_.reset(new dynamic_reconfigure::Server<SectorFilterConfig>(own_mutex_, private_nh));
+  dynamic_reconfigure::Server<SectorFilterConfig>::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