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 #ifndef LASER_SCAN_FOOTPRINT_FILTER_H
00036 #define LASER_SCAN_FOOTPRINT_FILTER_H
00037
00045 #include "filters/filter_base.h"
00046 #include "sensor_msgs/LaserScan.h"
00047 #include "tf/transform_listener.h"
00048 #include "sensor_msgs/PointCloud.h"
00049 #include "ros/ros.h"
00050 #include "laser_geometry/laser_geometry.h"
00051
00052 namespace laser_filters
00053 {
00054
00055 class LaserScanFootprintFilter : public filters::FilterBase<sensor_msgs::LaserScan>
00056 {
00057 public:
00058 LaserScanFootprintFilter()
00059 {
00060 ROS_WARN("LaserScanFootprintFilter has been deprecated. Please use PR2LaserScanFootprintFilter instead.\n");
00061 }
00062
00063 bool configure()
00064 {
00065 if(!getParam("inscribed_radius", inscribed_radius_))
00066 {
00067 ROS_ERROR("LaserScanFootprintFilter needs inscribed_radius to be set");
00068 return false;
00069 }
00070 return true;
00071 }
00072
00073 virtual ~LaserScanFootprintFilter()
00074 {
00075
00076 }
00077
00078 bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan)
00079 {
00080 filtered_scan = input_scan ;
00081 sensor_msgs::PointCloud laser_cloud;
00082
00083 try{
00084 projector_.transformLaserScanToPointCloud("base_link", input_scan, laser_cloud, tf_);
00085 }
00086 catch(tf::TransformException& ex){
00087 ROS_ERROR("Transform unavailable %s", ex.what());
00088 return false;
00089 }
00090
00091 int c_idx = indexChannel(laser_cloud);
00092
00093 if (c_idx == -1 || laser_cloud.channels[c_idx].values.size () == 0){
00094 ROS_ERROR("We need an index channel to be able to filter out the footprint");
00095 return false;
00096 }
00097
00098 for (unsigned int i=0; i < laser_cloud.points.size(); i++)
00099 {
00100 if (inFootprint(laser_cloud.points[i])){
00101 int index = laser_cloud.channels[c_idx].values[i];
00102 filtered_scan.ranges[index] = filtered_scan.range_max + 1.0 ;
00103 }
00104 }
00105 return true;
00106 }
00107
00108 int indexChannel(const sensor_msgs::PointCloud& scan_cloud){
00109 int c_idx = -1;
00110 for (unsigned int d = 0; d < scan_cloud.channels.size (); d++)
00111 {
00112 if (scan_cloud.channels[d].name == "index")
00113 {
00114 c_idx = d;
00115 break;
00116 }
00117 }
00118 return c_idx;
00119 }
00120
00121 bool inFootprint(const geometry_msgs::Point32& scan_pt){
00122 if(scan_pt.x < -1.0 * inscribed_radius_ || scan_pt.x > inscribed_radius_ || scan_pt.y < -1.0 * inscribed_radius_ || scan_pt.y > inscribed_radius_)
00123 return false;
00124 return true;
00125 }
00126
00127 private:
00128 tf::TransformListener tf_;
00129 laser_geometry::LaserProjection projector_;
00130 double inscribed_radius_;
00131 } ;
00132
00133 }
00134
00135 #endif // LASER_SCAN_FOOTPRINT_FILTER_H