Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef INTERPOLATION_FILTER_H
00036 #define INTERPOLATION_FILTER_H
00037
00044 #include "filters/filter_base.h"
00045 #include "sensor_msgs/LaserScan.h"
00046
00047 namespace laser_filters
00048 {
00049
00050 class InterpolationFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00051 {
00052 public:
00053
00054 bool configure()
00055 {
00056 return true;
00057 }
00058
00059 virtual ~InterpolationFilter()
00060 {
00061 }
00062
00063 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
00064 {
00065 double previous_valid_range = input_scan.range_max - .01;
00066 double next_valid_range = input_scan.range_max - .01;
00067 filtered_scan= input_scan;
00068
00069 unsigned int i = 0;
00070 while(i < input_scan.ranges.size())
00071 {
00072
00073 if (filtered_scan.ranges[i] <= input_scan.range_min ||
00074 filtered_scan.ranges[i] >= input_scan.range_max){
00075
00076
00077 unsigned int j = i + 1;
00078 unsigned int start_index = i;
00079 unsigned int end_index = i;
00080 while(j < input_scan.ranges.size()){
00081 if (filtered_scan.ranges[j] <= input_scan.range_min ||
00082 filtered_scan.ranges[j] >= input_scan.range_max){
00083 end_index = j;
00084 }
00085 else{
00086 next_valid_range = filtered_scan.ranges[j];
00087 break;
00088 }
00089 ++j;
00090 }
00091
00092
00093 double average_range = (previous_valid_range + next_valid_range) / 2.0;
00094
00095 for(unsigned int k = start_index; k <= end_index; k++){
00096 filtered_scan.ranges[k] = average_range;
00097 }
00098
00099
00100 previous_valid_range = next_valid_range;
00101 i = j + 1;
00102 }
00103 else{
00104 previous_valid_range = filtered_scan.ranges[i];
00105 ++i;
00106 }
00107
00108 }
00109 return true;
00110 }
00111 };
00112
00113 }
00114
00115 #endif // LASER_SCAN_INTENSITY_FILTER_H