$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011-2012, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 #include <math.h> 00036 #include <ros/ros.h> 00037 #include <sensor_msgs/LaserScan.h> 00038 #include <tf/transform_listener.h> 00039 00040 class LaserFootprintFilter 00041 { 00042 public: 00043 LaserFootprintFilter() 00044 : nh_("~"), listener_(ros::Duration(10)) 00045 { 00046 scan_filtered_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan_filtered", 1); 00047 scan_sub_ = nh_.subscribe("/scan", 1000, &LaserFootprintFilter::update, this); 00048 00049 nh_.param<double>("footprint_inscribed_radius", inscribed_radius_, 0.16495*1.1); 00050 nh_.param<std::string>("base_frame", base_frame_, "/base_link"); 00051 } 00052 00053 void update(const sensor_msgs::LaserScan& input_scan) 00054 { 00055 sensor_msgs::LaserScan filtered_scan; 00056 filtered_scan = input_scan; 00057 00058 double angle = filtered_scan.angle_min - filtered_scan.angle_increment; 00059 geometry_msgs::PointStamped p_input; 00060 p_input.header = input_scan.header; 00061 00062 for(size_t i=0; i < filtered_scan.ranges.size(); i++) 00063 { 00064 angle += filtered_scan.angle_increment; 00065 if(filtered_scan.ranges[i] >= filtered_scan.range_max) continue; 00066 00067 p_input.point.x = cos(angle) * filtered_scan.ranges[i]; 00068 p_input.point.y = sin(angle) * filtered_scan.ranges[i]; 00069 00070 geometry_msgs::PointStamped p_transformed; 00071 try{ 00072 listener_.transformPoint(base_frame_, p_input, p_transformed); 00073 }catch(tf::TransformException &ex){ 00074 ROS_ERROR("Received an exception trying to transform a point: %s", ex.what()); 00075 return; 00076 } 00077 00078 if( inFootprint(p_transformed) ) 00079 { 00080 filtered_scan.ranges[i] = filtered_scan.range_max + 1.0; 00081 } 00082 00083 } 00084 00085 scan_filtered_pub_.publish(filtered_scan); 00086 } 00087 00088 // Filter out circular area 00089 bool inFootprint(const geometry_msgs::PointStamped& scan_pt) 00090 { 00091 // Do a radius instead of a box. 00092 if (sqrt(scan_pt.point.x*scan_pt.point.x + scan_pt.point.y*scan_pt.point.y) > inscribed_radius_) 00093 return false; 00094 return true; 00095 } 00096 00097 private: 00098 ros::NodeHandle nh_; 00099 tf::TransformListener listener_; 00100 double inscribed_radius_; 00101 std::string base_frame_; 00102 ros::Publisher scan_filtered_pub_; 00103 ros::Publisher debug_pub_; 00104 ros::Subscriber scan_sub_; 00105 }; 00106 00107 int main(int argc, char** argv) 00108 { 00109 ros::init(argc, argv, "laser_footprint_filter"); 00110 00111 LaserFootprintFilter filter; 00112 ros::spin(); 00113 } 00114