diff --git a/include/laser_filters/range_filter.h b/include/laser_filters/range_filter.h new file mode 100644 index 00000000..fb709da2 --- /dev/null +++ b/include/laser_filters/range_filter.h @@ -0,0 +1,94 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2013, JSK (The University of Tokyo). +* 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. +*********************************************************************/ + +#ifndef LASER_SCAN_RANGE_FILTER_H +#define LASER_SCAN_RANGE_FILTER_H +/** +\author Yohei Kakiuchi +@b ScanRangeFilter takes input scans and filters within indicated range. +**/ + + +#include "filters/filter_base.h" +#include "sensor_msgs/LaserScan.h" + +namespace laser_filters +{ + +class LaserScanRangeFilter : public filters::FilterBase +{ +public: + + double lower_threshold_ ; + double upper_threshold_ ; + int disp_hist_ ; + + bool configure() + { + lower_threshold_ = 0.0; + upper_threshold_ = 100000.0; + disp_hist_ = 1; + getParam("lower_threshold", lower_threshold_); + getParam("upper_threshold", upper_threshold_) ; + + return true; + } + + virtual ~LaserScanRangeFilter() + { + + } + + bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan) + { + filtered_scan = input_scan; + for (unsigned int i=0; + i < input_scan.ranges.size() && i < input_scan.intensities.size(); + i++) // Need to check ever reading in the current scan + { + { + if (filtered_scan.ranges[i] <= lower_threshold_ || filtered_scan.ranges[i] >= upper_threshold_) + { + filtered_scan.ranges[i] = input_scan.range_max + 1.0 ; + } + } + } + + return true; + } +} ; + +} + +#endif // LASER_SCAN_RANGE_FILTER_H diff --git a/laser_filters_plugins.xml b/laser_filters_plugins.xml index 7d7d041b..c767af1b 100644 --- a/laser_filters_plugins.xml +++ b/laser_filters_plugins.xml @@ -12,6 +12,12 @@ This is a filter which filters sensor_msgs::LaserScan messages based on intensity + + + This is a filter which filters sensor_msgs::LaserScan messages based on intensity + + diff --git a/src/laser_scan_filters.cpp b/src/laser_scan_filters.cpp index b10d60b1..b41b173f 100644 --- a/src/laser_scan_filters.cpp +++ b/src/laser_scan_filters.cpp @@ -30,6 +30,7 @@ #include "laser_filters/median_filter.h" #include "laser_filters/array_filter.h" #include "laser_filters/intensity_filter.h" +#include "laser_filters/range_filter.h" #include "laser_filters/scan_shadows_filter.h" #include "laser_filters/footprint_filter.h" #include "laser_filters/interpolation_filter.h" @@ -43,6 +44,7 @@ PLUGINLIB_DECLARE_CLASS(laser_filters, LaserMedianFilter, laser_filters::LaserMedianFilter, filters::FilterBase) PLUGINLIB_DECLARE_CLASS(laser_filters, LaserArrayFilter, laser_filters::LaserArrayFilter, filters::FilterBase) PLUGINLIB_DECLARE_CLASS(laser_filters, LaserScanIntensityFilter, laser_filters::LaserScanIntensityFilter, filters::FilterBase) +PLUGINLIB_DECLARE_CLASS(laser_filters, LaserScanRangeFilter, laser_filters::LaserScanRangeFilter, filters::FilterBase) PLUGINLIB_DECLARE_CLASS(laser_filters, LaserScanAngularBoundsFilter, laser_filters::LaserScanAngularBoundsFilter, filters::FilterBase) PLUGINLIB_DECLARE_CLASS(laser_filters, LaserScanFootprintFilter, laser_filters::LaserScanFootprintFilter, filters::FilterBase) PLUGINLIB_DECLARE_CLASS(laser_filters, ScanShadowsFilter, laser_filters::ScanShadowsFilter, filters::FilterBase) diff --git a/stack.xml b/stack.xml index d7289221..3c3167ce 100644 --- a/stack.xml +++ b/stack.xml @@ -16,5 +16,5 @@ - +