Merge pull request ros-perception#111 from EricWiener/feat/range-filt…

…er-dynamic-reconfigure

Added DynamicReconfigure for RangeFilter

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
 )

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 <lower_threshold. Default: NaN", 0, 0.0, 100000.0)
+
+gen.add("upper_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"))

include/laser_filters/range_filter.h

@@ -40,6 +40,8 @@
 **/
 
 
+#include <dynamic_reconfigure/server.h>
+#include <laser_filters/RangeFilterConfig.h>
 #include "filters/filter_base.h"
 #include "sensor_msgs/LaserScan.h"
 
@@ -48,34 +50,27 @@ namespace laser_filters
 
 class LaserScanRangeFilter : public filters::FilterBase<sensor_msgs::LaserScan>
 {
-public:
+  std::shared_ptr<dynamic_reconfigure::Server<RangeFilterConfig>> 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<double>::quiet_NaN();
-    getParam("lower_replacement_value", temp_replacement_value);
-    lower_replacement_value_ = static_cast<float>(temp_replacement_value);
-
-    // work around the not implemented getParam(std::string name, float& value) method
-    temp_replacement_value = std::numeric_limits<double>::quiet_NaN();
-    getParam("upper_replacement_value", temp_replacement_value);
-    upper_replacement_value_ = static_cast<float>(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<RangeFilterConfig>(own_mutex_, private_nh));
+    dynamic_reconfigure::Server<RangeFilterConfig>::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,30 +81,38 @@ class LaserScanRangeFilter : public filters::FilterBase<sensor_msgs::LaserScan>
 
   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;
          i < input_scan.ranges.size();
          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;
+  }
 } ;
 
 }