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_H
00038 #define LASER_SCAN_ANGULAR_BOUNDS_FILTER_H
00039
00040 #include <filters/filter_base.h>
00041 #include <sensor_msgs/LaserScan.h>
00042
00043 namespace laser_filters
00044 {
00045 class LaserScanAngularBoundsFilter : 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 ~LaserScanAngularBoundsFilter(){}
00065
00066 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
00067 filtered_scan.ranges.resize(input_scan.ranges.size());
00068 filtered_scan.intensities.resize(input_scan.intensities.size());
00069
00070 double start_angle = input_scan.angle_min;
00071 double current_angle = input_scan.angle_min;
00072 ros::Time start_time = input_scan.header.stamp;
00073 unsigned int count = 0;
00074
00075 for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
00076
00077 if(start_angle < lower_angle_){
00078 start_angle += input_scan.angle_increment;
00079 current_angle += input_scan.angle_increment;
00080 start_time += ros::Duration(input_scan.time_increment);
00081 }
00082 else{
00083 filtered_scan.ranges[count] = input_scan.ranges[i];
00084
00085
00086 if(input_scan.intensities.size() > i)
00087 filtered_scan.intensities[count] = input_scan.intensities[i];
00088
00089 count++;
00090
00091
00092 if(current_angle + input_scan.angle_increment > upper_angle_){
00093 break;
00094 }
00095
00096 current_angle += input_scan.angle_increment;
00097
00098 }
00099 }
00100
00101
00102 filtered_scan.header.frame_id = input_scan.header.frame_id;
00103 filtered_scan.header.stamp = start_time;
00104 filtered_scan.angle_min = start_angle;
00105 filtered_scan.angle_max = current_angle;
00106 filtered_scan.angle_increment = input_scan.angle_increment;
00107 filtered_scan.time_increment = input_scan.time_increment;
00108 filtered_scan.scan_time = input_scan.scan_time;
00109 filtered_scan.range_min = input_scan.range_min;
00110 filtered_scan.range_max = input_scan.range_max;
00111
00112 filtered_scan.ranges.resize(count);
00113
00114 if(input_scan.intensities.size() >= count)
00115 filtered_scan.intensities.resize(count);
00116
00117 ROS_DEBUG("Filtered out %d points from the laser scan.", (int)input_scan.ranges.size() - (int)count);
00118
00119 return true;
00120
00121 }
00122 };
00123 };
00124 #endif