35 #ifndef LASER_SCAN_RANGE_FILTER_H 36 #define LASER_SCAN_RANGE_FILTER_H 44 #include "sensor_msgs/LaserScan.h" 61 use_message_range_limits_ =
false;
62 getParam(
"use_message_range_limits", use_message_range_limits_);
65 double temp_replacement_value = std::numeric_limits<double>::quiet_NaN();
66 getParam(
"lower_replacement_value", temp_replacement_value);
67 lower_replacement_value_ =
static_cast<float>(temp_replacement_value);
70 temp_replacement_value = std::numeric_limits<double>::quiet_NaN();
71 getParam(
"upper_replacement_value", temp_replacement_value);
72 upper_replacement_value_ =
static_cast<float>(temp_replacement_value);
75 lower_threshold_ = 0.0;
76 upper_threshold_ = 100000.0;
77 getParam(
"lower_threshold", lower_threshold_);
78 getParam(
"upper_threshold", upper_threshold_) ;
87 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
89 if (use_message_range_limits_)
91 lower_threshold_ = input_scan.range_min;
92 upper_threshold_ = input_scan.range_max;
94 filtered_scan = input_scan;
95 for (
unsigned int i=0;
96 i < input_scan.ranges.size();
100 if (filtered_scan.ranges[i] <= lower_threshold_)
105 else if (filtered_scan.ranges[i] >= upper_threshold_)
117 #endif // LASER_SCAN_RANGE_FILTER_H
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
float lower_replacement_value_
bool getParam(const std::string &name, std::string &value)
virtual ~LaserScanRangeFilter()
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)
float upper_replacement_value_
bool use_message_range_limits_