35 #ifndef LASER_SCAN_RANGE_FILTER_H
36 #define LASER_SCAN_RANGE_FILTER_H
43 #include <dynamic_reconfigure/server.h>
44 #include <laser_filters/RangeFilterConfig.h>
46 #include "sensor_msgs/LaserScan.h"
53 std::shared_ptr<dynamic_reconfigure::Server<RangeFilterConfig>>
dyn_server_;
56 RangeFilterConfig
config_ = RangeFilterConfig::__getDefault__();
63 dynamic_reconfigure::Server<RangeFilterConfig>::CallbackType
f;
64 f = [
this](
auto& config,
auto level){
reconfigureCB(config, level); };
82 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
84 double lower_threshold =
config_.lower_threshold;
85 double upper_threshold =
config_.upper_threshold;
87 if (
config_.use_message_range_limits)
89 lower_threshold = input_scan.range_min;
90 upper_threshold = input_scan.range_max;
92 filtered_scan = input_scan;
93 for (
unsigned int i=0;
94 i < input_scan.ranges.size();
98 if (filtered_scan.ranges[i] <= lower_threshold)
100 filtered_scan.ranges[i] =
config_.lower_replacement_value;
103 else if (filtered_scan.ranges[i] >= upper_threshold)
105 filtered_scan.ranges[i] =
config_.upper_replacement_value;
112 void reconfigureCB(RangeFilterConfig& config, uint32_t level)
120 #endif // LASER_SCAN_RANGE_FILTER_H