laser_footprint_filter.cpp
Go to the documentation of this file.
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 


turtlebot_navigation
Author(s): Tully Foote
autogenerated on Wed Aug 26 2015 16:34:52