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 "sensor_msgs/PointCloud.h"
00048 #include "ros/ros.h"
00049
00050 namespace pr2_laser_filters
00051 {
00052
00053 class PR2PointCloudFootprintFilterNew : public filters::FilterBase<sensor_msgs::PointCloud>
00054 {
00055 public:
00056 PR2PointCloudFootprintFilterNew() {}
00057
00058 bool configure()
00059 {
00060 if(!getParam("inscribed_radius", inscribed_radius_))
00061 {
00062 ROS_ERROR("PR2PointCloudPointCloudFootprintFilter needs inscribed_radius to be set");
00063 return false;
00064 }
00065 return true;
00066 }
00067
00068 virtual ~PR2PointCloudFootprintFilterNew()
00069 {
00070
00071 }
00072
00073 bool update(const sensor_msgs::PointCloud& input_scan, sensor_msgs::PointCloud& filtered_scan)
00074 {
00075 if(&input_scan == &filtered_scan){
00076 ROS_ERROR("This filter does not currently support in place copying");
00077 return false;
00078 }
00079 sensor_msgs::PointCloud laser_cloud;
00080
00081 try{
00082 tf_.waitForTransform(input_scan.header.frame_id, "base_link", input_scan.header.stamp, ros::Duration(0.2));
00083 tf_.transformPointCloud("base_link", input_scan, laser_cloud);
00084 }
00085 catch(tf::TransformException& ex){
00086 ROS_ERROR("Transform unavailable %s", ex.what());
00087 return false;
00088 }
00089
00090 filtered_scan.header = input_scan.header;
00091 filtered_scan.points.resize (input_scan.points.size());
00092 filtered_scan.channels.resize (input_scan.channels.size());
00093 for (unsigned int d = 0; d < input_scan.get_channels_size (); d++){
00094 filtered_scan.channels[d].values.resize (input_scan.points.size());
00095 filtered_scan.channels[d].name = input_scan.channels[d].name;
00096 }
00097
00098 int num_pts = 0;
00099 for (unsigned int i=0; i < laser_cloud.points.size(); i++)
00100 {
00101 if (!inFootprint(laser_cloud.points[i])){
00102 filtered_scan.points[num_pts] = input_scan.points[i];
00103 for (unsigned int d = 0; d < filtered_scan.get_channels_size (); d++)
00104 filtered_scan.channels[d].values[num_pts] = input_scan.channels[d].values[i];
00105 num_pts++;
00106 }
00107 }
00108
00109
00110 filtered_scan.points.resize (num_pts);
00111 for (unsigned int d = 0; d < filtered_scan.get_channels_size (); d++)
00112 filtered_scan.channels[d].values.resize (num_pts);
00113
00114 return true;
00115 }
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_POINT_CLOUD_FOOTPRINT_FILTER_H