nan_to_inf_filter.h
Go to the documentation of this file.
00001 /*
00002  * LICENSE: https://github.com/utexas-bwi/segbot/blob/devel/LICENSE
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           // TODO: remove once navigation handles inf_is_valid again
00035           // if (filtered_scan.ranges[count] == std::numeric_limits<float>::infinity() ||
00036             //   filtered_scan.ranges[count] >= input_scan.range_max) {
00037             // filtered_scan.ranges[count] = input_scan.range_max - 0.0001;
00038           // }
00039         }
00040 
00041         //make sure to set all the needed fields on the filtered scan
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 


segbot_sensors
Author(s): Piyush Khandelwal
autogenerated on Thu Jun 6 2019 21:37:13