$search
00001 /* 00002 * Copyright (c) 2008 Radu Bogdan Rusu <rusu@cs.tum.edu> 00003 * 00004 * All rights reserved. 00005 * 00006 * Redistribution and use in source and binary forms, with or without 00007 * modification, are permitted provided that the following conditions are met: 00008 * 00009 * * Redistributions of source code must retain the above copyright 00010 * notice, this list of conditions and the following disclaimer. 00011 * * Redistributions in binary form must reproduce the above copyright 00012 * notice, this list of conditions and the following disclaimer in the 00013 * documentation and/or other materials provided with the distribution. 00014 * 00015 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00016 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00017 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00018 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00019 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00020 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00021 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00022 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00023 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00024 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00025 * POSSIBILITY OF SUCH DAMAGE. 00026 * 00027 * $Id: scan_shadows_filter.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $ 00028 * 00029 */ 00030 00031 /* 00032 \author Radu Bogdan Rusu <rusu@cs.tum.edu> Tully Foote <tfoote@willowgarage.com> 00033 00034 00035 */ 00036 00037 #ifndef LASER_SCAN_SHADOWS_FILTER_H 00038 #define LASER_SCAN_SHADOWS_FILTER_H 00039 00040 #include <set> 00041 00042 #include "filters/filter_base.h" 00043 #include <sensor_msgs/LaserScan.h> 00044 #include "angles/angles.h" 00045 00046 namespace laser_filters{ 00047 00051 class ScanShadowsFilter : public filters::FilterBase<sensor_msgs::LaserScan> 00052 { 00053 public: 00054 00055 double laser_max_range_; // Used in laser scan projection 00056 double min_angle_, max_angle_; // Filter angle threshold 00057 int window_, neighbors_; 00058 00059 00061 ScanShadowsFilter () 00062 { 00063 00064 00065 } 00066 00068 bool configure() 00069 { 00070 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("min_angle"), min_angle_)) 00071 { 00072 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n"); 00073 return false; 00074 } 00075 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("max_angle"), max_angle_)) 00076 { 00077 ROS_ERROR("Error: ShadowsFilter was not given min_angle.\n"); 00078 return false; 00079 } 00080 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("window"), window_)) 00081 { 00082 ROS_ERROR("Error: ShadowsFilter was not given window.\n"); 00083 return false; 00084 } 00085 neighbors_ = 0;//default value 00086 if (!filters::FilterBase<sensor_msgs::LaserScan>::getParam(std::string("neighbors"), neighbors_)) 00087 { 00088 ROS_INFO("Error: ShadowsFilter was not given neighbors.\n"); 00089 } 00090 00091 return true; 00092 } 00093 00095 virtual ~ScanShadowsFilter () { } 00096 00099 inline double getAngleWithViewpoint(float r1, float r2, float included_angle) 00100 { 00101 return atan2(r2 * sin(included_angle), r1 - r2 * cos(included_angle)); 00102 } 00103 00104 00106 00113 bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out) 00114 { 00115 //copy across all data first 00116 scan_out = scan_in; 00117 00118 std::set<int> indices_to_delete; 00119 // For each point in the current line scan 00120 for (unsigned int i = 0; i < scan_in.ranges.size (); i++) 00121 { 00122 for (int y = -window_; y < window_ + 1; y++) 00123 { 00124 int j = i + y; 00125 if ( j < 0 || j >= (int)scan_in.ranges.size () || (int)i == j ){ // Out of scan bounds or itself 00126 continue; 00127 } 00128 00129 double angle = abs(angles::to_degrees(getAngleWithViewpoint (scan_in.ranges[i],scan_in.ranges[j], y * scan_in.angle_increment))); 00130 if (angle < min_angle_ || angle > max_angle_) 00131 { 00132 for (int index = std::max<int>(i - neighbors_, 0); index <= std::min<int>(i+neighbors_, (int)scan_in.ranges.size()-1); index++) 00133 { 00134 if (scan_in.ranges[i] < scan_in.ranges[index]) // delete neighbor if they are farther away (note not self) 00135 indices_to_delete.insert(index); 00136 } 00137 } 00138 00139 } 00140 } 00141 00142 ROS_DEBUG("ScanShadowsFilter removing %d Points from scan with min angle: %.2f, max angle: %.2f, neighbors: %d, and window: %d", (int)indices_to_delete.size(), min_angle_, max_angle_, neighbors_, window_); 00143 for ( std::set<int>::iterator it = indices_to_delete.begin(); it != indices_to_delete.end(); ++it) 00144 { 00145 scan_out.ranges[*it] = -1.0 * fabs(scan_in.ranges[*it]); //Failed test so set the ranges to invalid value 00146 } 00147 return true; 00148 } 00149 00151 00152 } ; 00153 } 00154 00155 #endif //LASER_SCAN_SHADOWS_FILTER_H