Go to the documentation of this file.00001
00002
00003
00004
00005 #ifndef LASER_SCAN_NAN_TO_INF_FILTER_H
00006 #define LASER_SCAN_NAN_TO_INF_FILTER_H
00007
00008 #include <filters/filter_base.h>
00009 #include <sensor_msgs/LaserScan.h>
00010
00011 namespace segbot_sensors
00012 {
00013 class NanToInfFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00014 {
00015 public:
00016
00017 bool configure() {
00018 return true;
00019 }
00020
00021 virtual ~NanToInfFilter(){}
00022
00023 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
00024
00025 filtered_scan.ranges.resize(input_scan.ranges.size());
00026
00027 for(unsigned int count = 0; count < input_scan.ranges.size(); ++count){
00028 filtered_scan.ranges[count] =
00029 (isnan(input_scan.ranges[count]) ||
00030 input_scan.ranges[count] < input_scan.range_min) ?
00031 std::numeric_limits<float>::infinity() : input_scan.ranges[count];
00032
00033
00034 if (filtered_scan.ranges[count] == std::numeric_limits<float>::infinity() ||
00035 filtered_scan.ranges[count] >= input_scan.range_max) {
00036 filtered_scan.ranges[count] = input_scan.range_max - 0.0001;
00037 }
00038 }
00039
00040
00041 filtered_scan.header.frame_id = input_scan.header.frame_id;
00042 filtered_scan.header.stamp = input_scan.header.stamp;
00043 filtered_scan.angle_min = input_scan.angle_min;
00044 filtered_scan.angle_max = input_scan.angle_max;
00045 filtered_scan.angle_increment = input_scan.angle_increment;
00046 filtered_scan.time_increment = input_scan.time_increment;
00047 filtered_scan.scan_time = input_scan.scan_time;
00048 filtered_scan.range_min = input_scan.range_min;
00049 filtered_scan.range_max = input_scan.range_max;
00050 filtered_scan.intensities = input_scan.intensities;
00051
00052 return true;
00053
00054 }
00055 };
00056 }
00057 #endif
00058