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 std_msgs::Header header;
00090 header = pcl_conversions::fromPCL(input_scan.header);
00091 tf_.waitForTransform(input_scan.header.frame_id, "base_link", header.stamp, ros::Duration(0.2));
00092 pcl_ros::transformPointCloud("base_link", input_scan, laser_cloud, tf_);
00093 }
00094 catch(tf::TransformException& ex){
00095 ROS_ERROR("Transform unavailable %s", ex.what());
00096 return false;
00097 }
00098
00099 filtered_scan.header = input_scan.header;
00100 filtered_scan.points.resize (input_scan.points.size());
00101
00102 int num_pts = 0;
00103 for (unsigned int i=0; i < laser_cloud.points.size(); i++)
00104 {
00105 if (!inFootprint(laser_cloud.points[i])){
00106 filtered_scan.points[num_pts] = input_scan.points[i];
00107 num_pts++;
00108 }
00109 }
00110
00111
00112 filtered_scan.points.resize (num_pts);
00113 pcl::toROSMsg(filtered_scan, filtered_scan2);
00114
00115 return true;
00116 }
00117
00118
00119 bool inFootprint(const pcl::PointXYZ& 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 // PR2_POINT_CLOUD_FOOTPRINT_FILTER_H