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