35 #ifndef INTERPOLATION_FILTER_H 36 #define INTERPOLATION_FILTER_H 45 #include "sensor_msgs/LaserScan.h" 63 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
65 double previous_valid_range = input_scan.range_max - .01;
66 double next_valid_range = input_scan.range_max - .01;
67 filtered_scan= input_scan;
70 while(i < input_scan.ranges.size())
73 if (filtered_scan.ranges[i] <= input_scan.range_min ||
74 filtered_scan.ranges[i] >= input_scan.range_max){
77 unsigned int j = i + 1;
78 unsigned int start_index = i;
79 unsigned int end_index = i;
80 while(j < input_scan.ranges.size()){
81 if (filtered_scan.ranges[j] <= input_scan.range_min ||
82 filtered_scan.ranges[j] >= input_scan.range_max){
86 next_valid_range = filtered_scan.ranges[j];
93 double average_range = (previous_valid_range + next_valid_range) / 2.0;
95 for(
unsigned int k = start_index; k <= end_index; k++){
96 filtered_scan.ranges[k] = average_range;
100 previous_valid_range = next_valid_range;
104 previous_valid_range = filtered_scan.ranges[i];
115 #endif // LASER_SCAN_INTENSITY_FILTER_H virtual ~InterpolationFilter()
LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)