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