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 input_scan.ranges[count] >= input_scan.range_max) ?
00032 std::numeric_limits<float>::infinity() : input_scan.ranges[count];
00033
00034
00035
00036
00037
00038
00039 }
00040
00041
00042 filtered_scan.header.frame_id = input_scan.header.frame_id;
00043 filtered_scan.header.stamp = input_scan.header.stamp;
00044 filtered_scan.angle_min = input_scan.angle_min;
00045 filtered_scan.angle_max = input_scan.angle_max;
00046 filtered_scan.angle_increment = input_scan.angle_increment;
00047 filtered_scan.time_increment = input_scan.time_increment;
00048 filtered_scan.scan_time = input_scan.scan_time;
00049 filtered_scan.range_min = input_scan.range_min;
00050 filtered_scan.range_max = input_scan.range_max;
00051 filtered_scan.intensities = input_scan.intensities;
00052
00053 return true;
00054
00055 }
00056 };
00057 }
00058 #endif
00059