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 filtered_scan = input_scan ;
00078
00079 for (unsigned int i=0;
00080 i < input_scan.ranges.size() && i < input_scan.intensities.size();
00081 i++)
00082 {
00083 if (filtered_scan.intensities[i] <= lower_threshold_ ||
00084 filtered_scan.intensities[i] >= upper_threshold_)
00085 filtered_scan.ranges[i] = input_scan.range_max + 1.0 ;
00086 }
00087
00088 if (disp_hist_ > 0)
00089 {
00090 const double hist_max = 4*12000.0 ;
00091 const int num_buckets = 24 ;
00092 std::vector<int> histogram(num_buckets, 0);
00093
00094 int num_negative = 0;
00095 for (unsigned int i=0;
00096 i < input_scan.ranges.size() && i < input_scan.intensities.size();
00097 i++)
00098 {
00099 int cur_bucket((filtered_scan.intensities[i]/hist_max)*num_buckets) ;
00100 if (cur_bucket >= num_buckets-1)
00101 histogram[num_buckets-1]++;
00102 else if (cur_bucket < 0)
00103 ++num_negative;
00104 else
00105 histogram[cur_bucket]++;
00106 }
00107
00108 std::cout << "********** SCAN **********" << std::endl;
00109 std::cout << "negative - 0: " << num_negative << std::endl;
00110 for (int i=0; i < num_buckets; i++)
00111 {
00112 std::cout << (unsigned int) hist_max/num_buckets*i << " - ";
00113 std::cout << (unsigned int) hist_max/num_buckets*(i+1) << ": ";
00114 std::cout << histogram[i] << std::endl;
00115 }
00116 }
00117 return true;
00118 }
00119 } ;
00120
00121 }
00122
00123 #endif // LASER_SCAN_INTENSITY_FILTER_H