$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2009, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage, Inc. nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Eitan Marder-Eppstein 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 //loop through the scan and truncate the beginning and the end of the scan as necessary 00075 for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){ 00076 //wait until we get to our desired starting angle 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 //make sure that we don't update intensity data if its not available 00086 if(input_scan.intensities.size() > i) 00087 filtered_scan.intensities[count] = input_scan.intensities[i]; 00088 00089 count++; 00090 00091 //check if we need to break out of the loop, basically if the next increment will put us over the threshold 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 //make sure to set all the needed fields on the filtered scan 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