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
00058 bool configure()
00059 {
00060 lower_threshold_ = 8000.0;
00061 upper_threshold_ = 100000.0;
00062 disp_hist_ = 1;
00063 getParam("lower_threshold", lower_threshold_);
00064 getParam("upper_threshold", upper_threshold_) ;
00065 getParam("disp_histogram", disp_hist_) ;
00066
00067 return true;
00068 }
00069
00070 virtual ~LaserScanIntensityFilter()
00071 {
00072
00073 }
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 for (unsigned int i=0;
00086 i < input_scan.ranges.size() && i < input_scan.intensities.size();
00087 i++)
00088 {
00089 if (filtered_scan.intensities[i] <= lower_threshold_ ||
00090 filtered_scan.intensities[i] >= upper_threshold_)
00091 {
00092
00093 filtered_scan.ranges[i] = input_scan.range_max + 1.0 ;
00094 }
00095
00096 int cur_bucket = (int) ((filtered_scan.intensities[i]/hist_max)*num_buckets) ;
00097 if (cur_bucket >= num_buckets-1)
00098 cur_bucket = num_buckets-1 ;
00099 histogram[cur_bucket]++ ;
00100 }
00101
00102 if (disp_hist_ > 0)
00103 {
00104 printf("********** SCAN **********\n") ;
00105 for (int i=0; i < num_buckets; i++)
00106 {
00107 printf("%u - %u: %u\n", (unsigned int) hist_max/num_buckets*i,
00108 (unsigned int) hist_max/num_buckets*(i+1),
00109 histogram[i]) ;
00110 }
00111 }
00112 return true;
00113 }
00114 } ;
00115
00116 }
00117
00118 #endif // LASER_SCAN_INTENSITY_FILTER_H