diff --git a/CMakeLists.txt b/CMakeLists.txt index da378ee6..ee6017a7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) generate_dynamic_reconfigure_options( cfg/IntensityFilter.cfg cfg/PolygonFilter.cfg + cfg/RangeFilter.cfg cfg/ScanShadowsFilter.cfg cfg/SpeckleFilter.cfg ) 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/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; + } } ; }