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 LASER_SCAN_INTENSITY_FILTER_H
00036 #define LASER_SCAN_INTENSITY_FILTER_H
00037
00044 #include "filters/filter_base.h"
00045 #include "sensor_msgs/LaserScan.h"
00046
00047 namespace laser_filters
00048 {
00049
00050 class LaserScanIntensityFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00051 {
00052 public:
00053
00054 double lower_threshold_ ;
00055 double upper_threshold_ ;
00056 int disp_hist_ ;
00057 bool disp_hist_enabled_;
00058
00059 bool configure()
00060 {
00061 lower_threshold_ = 8000.0;
00062 upper_threshold_ = 100000.0;
00063 disp_hist_ = 1;
00064 getParam("lower_threshold", lower_threshold_);
00065 getParam("upper_threshold", upper_threshold_) ;
00066 getParam("disp_histogram", disp_hist_) ;
00067
00068 disp_hist_enabled_ = (disp_hist_ == 0) ? false : true;
00069
00070 return true;
00071 }
00072
00073 virtual ~LaserScanIntensityFilter(){}
00074
00075 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
00076 {
00077 const double hist_max = 4*12000.0 ;
00078 const int num_buckets = 24 ;
00079 int histogram[num_buckets] ;
00080 for (int i=0; i < num_buckets; i++)
00081 histogram[i] = 0 ;
00082
00083 filtered_scan = input_scan;
00084
00085
00086 for (unsigned int i=0;
00087 i < input_scan.ranges.size() && i < input_scan.intensities.size();
00088 i++)
00089 {
00090
00091
00092 if (filtered_scan.intensities[i] <= lower_threshold_ ||
00093 filtered_scan.intensities[i] >= upper_threshold_)
00094 {
00095
00096 filtered_scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
00097 }
00098
00099
00100 if (disp_hist_enabled_){
00101
00102 if( std::isinf((double)filtered_scan.intensities[i]) ||
00103 std::isnan((double)filtered_scan.intensities[i]) )
00104 continue;
00105
00106
00107
00108 int cur_bucket = (int)(filtered_scan.intensities[i] / hist_max * num_buckets);
00109 if (cur_bucket > num_buckets-1)
00110 cur_bucket = num_buckets-1;
00111 else if (cur_bucket < 0) cur_bucket = 0;
00112 histogram[cur_bucket]++;
00113 }
00114 }
00115
00116
00117 if (disp_hist_enabled_)
00118 {
00119 printf("********** SCAN **********\n") ;
00120 for (int i=0; i < num_buckets; i++)
00121 {
00122 printf("%u - %u: %u\n", (unsigned int) hist_max/num_buckets*i,
00123 (unsigned int) hist_max/num_buckets*(i+1),
00124 histogram[i]) ;
00125 }
00126 }
00127 return true;
00128 }
00129 };
00130 }
00131
00132 #endif // LASER_SCAN_INTENSITY_FILTER_H