35 #ifndef LASER_SCAN_INTENSITY_FILTER_H 36 #define LASER_SCAN_INTENSITY_FILTER_H 45 #include "sensor_msgs/LaserScan.h" 61 lower_threshold_ = 8000.0;
62 upper_threshold_ = 100000.0;
64 getParam(
"lower_threshold", lower_threshold_);
65 getParam(
"upper_threshold", upper_threshold_) ;
66 getParam(
"disp_histogram", disp_hist_) ;
68 disp_hist_enabled_ = (disp_hist_ == 0) ?
false :
true;
75 bool update(
const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
77 const double hist_max = 4*12000.0 ;
78 const int num_buckets = 24 ;
79 int histogram[num_buckets] ;
80 for (
int i=0; i < num_buckets; i++)
83 filtered_scan = input_scan;
86 for (
unsigned int i=0;
87 i < input_scan.ranges.size() && i < input_scan.intensities.size();
92 if (filtered_scan.intensities[i] <= lower_threshold_ ||
93 filtered_scan.intensities[i] >= upper_threshold_)
96 filtered_scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
100 if (disp_hist_enabled_){
102 if( std::isinf((
double)filtered_scan.intensities[i]) ||
103 std::isnan((
double)filtered_scan.intensities[i]) )
108 int cur_bucket = (int)(filtered_scan.intensities[i] / hist_max * num_buckets);
109 if (cur_bucket > num_buckets-1)
110 cur_bucket = num_buckets-1;
111 else if (cur_bucket < 0) cur_bucket = 0;
112 histogram[cur_bucket]++;
117 if (disp_hist_enabled_)
119 printf(
"********** SCAN **********\n") ;
120 for (
int i=0; i < num_buckets; i++)
122 printf(
"%u - %u: %u\n", (
unsigned int) hist_max/num_buckets*i,
123 (
unsigned int) hist_max/num_buckets*(i+1),
132 #endif // LASER_SCAN_INTENSITY_FILTER_H LaserScanMaskFilter removes points on directions defined in a mask from a laser scan.
bool getParam(const std::string &name, std::string &value)
bool update(const sensor_msgs::LaserScan &input_scan, sensor_msgs::LaserScan &filtered_scan)
virtual ~LaserScanIntensityFilter()