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
00036
00037 #ifndef LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H
00038 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_IN_PLACE_H
00039
00040 #include <filters/filter_base.h>
00041 #include <sensor_msgs/LaserScan.h>
00042
00043 namespace laser_filters
00044 {
00045 class LaserScanAngularBoundsFilterInPlace : public filters::FilterBase<sensor_msgs::LaserScan>
00046 {
00047 public:
00048 double lower_angle_;
00049 double upper_angle_;
00050
00051 bool configure()
00052 {
00053 lower_angle_ = 0;
00054 upper_angle_ = 0;
00055
00056 if(!getParam("lower_angle", lower_angle_) || !getParam("upper_angle", upper_angle_)){
00057 ROS_ERROR("Both the lower_angle and upper_angle parameters must be set to use this filter.");
00058 return false;
00059 }
00060
00061 return true;
00062 }
00063
00064 virtual ~LaserScanAngularBoundsFilterInPlace(){}
00065
00066 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
00067 filtered_scan = input_scan;
00068
00069 double current_angle = input_scan.angle_min;
00070 unsigned int count = 0;
00071
00072 for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
00073 if((current_angle > lower_angle_) && (current_angle < upper_angle_)){
00074 filtered_scan.ranges[i] = input_scan.range_max + 1.0;
00075 if(i < filtered_scan.intensities.size()){
00076 filtered_scan.intensities[i] = 0.0;
00077 }
00078 count++;
00079 }
00080 current_angle += input_scan.angle_increment;
00081 }
00082
00083 ROS_DEBUG("Filtered out %u points from the laser scan.", count);
00084
00085 return true;
00086
00087 }
00088 };
00089 };
00090 #endif