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 PR2_POINT_CLOUD_FOOTPRINT_FILTER_H
00036 #define PR2_POINT_CLOUD_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 "ros/ros.h"
00048 #include <pcl_ros/point_cloud.h>
00049 #include <pcl/point_types.h>
00050 #include <pcl_ros/transforms.h>
00051 #include <sensor_msgs/PointCloud2.h>
00052
00053 namespace pr2_laser_filters
00054 {
00055
00056 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00057
00058 class PR2PointCloudFootprintFilterNew : public filters::FilterBase<sensor_msgs::PointCloud2>
00059 {
00060 public:
00061 PR2PointCloudFootprintFilterNew() {}
00062
00063 bool configure()
00064 {
00065 if(!getParam("inscribed_radius", inscribed_radius_))
00066 {
00067 ROS_ERROR("PR2PointCloudPointCloudFootprintFilter needs inscribed_radius to be set");
00068 return false;
00069 }
00070 return true;
00071 }
00072
00073 virtual ~PR2PointCloudFootprintFilterNew()
00074 {
00075
00076 }
00077
00078 bool update(const sensor_msgs::PointCloud2& input_scan2, sensor_msgs::PointCloud2& filtered_scan2)
00079 {
00080 if(&input_scan2 == &filtered_scan2){
00081 ROS_ERROR("This filter does not currently support in place copying");
00082 return false;
00083 }
00084
00085 PointCloud input_scan, filtered_scan, laser_cloud;
00086 pcl::fromROSMsg(input_scan2, input_scan);
00087
00088 try{
00089 tf_.waitForTransform(input_scan.header.frame_id, "base_link", input_scan.header.stamp, ros::Duration(0.2));
00090 pcl_ros::transformPointCloud("base_link", input_scan, laser_cloud, tf_);
00091 }
00092 catch(tf::TransformException& ex){
00093 ROS_ERROR("Transform unavailable %s", ex.what());
00094 return false;
00095 }
00096
00097 filtered_scan.header = input_scan.header;
00098 filtered_scan.points.resize (input_scan.points.size());
00099
00100 int num_pts = 0;
00101 for (unsigned int i=0; i < laser_cloud.points.size(); i++)
00102 {
00103 if (!inFootprint(laser_cloud.points[i])){
00104 filtered_scan.points[num_pts] = input_scan.points[i];
00105 num_pts++;
00106 }
00107 }
00108
00109
00110 filtered_scan.points.resize (num_pts);
00111 pcl::toROSMsg(filtered_scan, filtered_scan2);
00112
00113 return true;
00114 }
00115
00116
00117 bool inFootprint(const pcl::PointXYZ& scan_pt){
00118 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_)
00119 return false;
00120 return true;
00121 }
00122
00123 private:
00124 tf::TransformListener tf_;
00125 laser_geometry::LaserProjection projector_;
00126 double inscribed_radius_;
00127 } ;
00128
00129 }
00130
00131 #endif // PR2_POINT_CLOUD_FOOTPRINT_FILTER_H