pointcloud_mapper.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <pcl_ros/point_cloud.h>
00003 #include <pcl_ros/transforms.h>
00004 #include <tf/transform_listener.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/filters/voxel_grid.h>
00007 #include <pcl/filters/passthrough.h>
00008 
00013 class PointCloudMapper
00014 {
00015 public:
00016 
00017   typedef pcl::PointXYZRGB      Point;
00018   typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
00019 
00020   PointCloudMapper() :
00021     nh_(), nh_priv_("~")
00022   {
00023     nh_priv_.param("fixed_frame", fixed_frame_, std::string("/map"));
00024     // Read the parameters from the parameter server (set defaults)
00025     nh_priv_.param("x_filter_min", x_filter_min_, -30.0);
00026     nh_priv_.param("x_filter_max", x_filter_max_, 30.0);
00027     nh_priv_.param("y_filter_min", y_filter_min_, -30.0);
00028     nh_priv_.param("y_filter_max", y_filter_max_, 30.0);
00029     nh_priv_.param("z_filter_min", z_filter_min_, 2.0);
00030     nh_priv_.param("z_filter_max", z_filter_max_, 30.0);
00031     nh_priv_.param("voxel_size", voxel_size_, 0.1);
00032     nh_priv_.param("filter_map", filter_map_, false);
00033 
00034     cloud_sub_ = nh_.subscribe<PointCloud>("input", 10, &PointCloudMapper::callback, this);
00035     bool latched = true;
00036     cloud_pub_ = nh_priv_.advertise<PointCloud>("output", 1, latched);
00037     pub_timer_ = nh_.createTimer(ros::Duration(10.0), &PointCloudMapper::publishCallback, this);
00038   }
00039 
00040   void callback(const PointCloud::ConstPtr& cloud)
00041   {
00042     ROS_INFO_STREAM("received cloud with " << cloud->points.size() << " points.");
00043     PointCloud transformed_cloud;
00044     bool success = pcl_ros::transformPointCloud(fixed_frame_, *cloud, transformed_cloud, tf_listener_);
00045     if (success)
00046     {
00047       accumulated_cloud_ += transformed_cloud;
00048     }
00049     else
00050     {
00051       ROS_ERROR("Could not transform point cloud to %s", fixed_frame_.c_str());
00052     }
00053 
00054     if(filter_map_){
00055       PointCloud::Ptr cloud_downsampled = filter(accumulated_cloud_.makeShared());
00056       accumulated_cloud_ = *cloud_downsampled;
00057     }
00058 
00059 
00060     ROS_INFO_STREAM("Map has " << accumulated_cloud_.points.size() << " points.");
00061   }
00062 
00063   void publishCallback(const ros::TimerEvent&)
00064   {
00065     if (cloud_pub_.getNumSubscribers() > 0)
00066     {
00067       cloud_pub_.publish(accumulated_cloud_);
00068     }
00069   }
00070 
00071   PointCloud::Ptr filter(PointCloud::Ptr cloud)
00072   {
00073     // Copy the point cloud
00074     PointCloud::Ptr cloud_ptr(new PointCloud);
00075 
00076     // NAN and limit filtering
00077     PointCloud::Ptr cloud_filtered_ptr(new PointCloud);
00078     pcl::PassThrough<Point> pass_;
00079 
00080     // X-filtering
00081     pass_.setFilterFieldName("x");
00082     pass_.setFilterLimits(x_filter_min_, x_filter_max_);
00083     pass_.setInputCloud(cloud);
00084     pass_.filter(*cloud_filtered_ptr);
00085 
00086     // Y-filtering
00087     pass_.setFilterFieldName("y");
00088     pass_.setFilterLimits(y_filter_min_, y_filter_max_);
00089     pass_.setInputCloud(cloud_filtered_ptr);
00090     pass_.filter(*cloud_filtered_ptr);
00091 
00092     // Z-filtering
00093     pass_.setFilterFieldName("z");
00094     pass_.setFilterLimits(z_filter_min_, z_filter_max_);
00095     pass_.setInputCloud(cloud);
00096     pass_.filter(*cloud_filtered_ptr);
00097 
00098     // Downsampling using voxel grid
00099     pcl::VoxelGrid<Point> grid_;
00100     PointCloud::Ptr cloud_downsampled_ptr(new PointCloud);
00101     double plane_detection_voxel_size_ = voxel_size_;
00102 
00103     grid_.setLeafSize(plane_detection_voxel_size_,
00104                       plane_detection_voxel_size_,
00105                       plane_detection_voxel_size_);
00106     grid_.setDownsampleAllData(true);
00107     grid_.setInputCloud(cloud_filtered_ptr);
00108     grid_.filter(*cloud_downsampled_ptr);
00109 
00110     return cloud_downsampled_ptr;
00111   }
00112 
00113 private:
00114   // Filter parameters
00115   double x_filter_min_;
00116   double x_filter_max_;
00117   double y_filter_min_;
00118   double y_filter_max_;
00119   double z_filter_min_;
00120   double z_filter_max_;
00121   double voxel_size_;
00122   bool filter_map_;
00123 
00124   ros::NodeHandle nh_;
00125   ros::NodeHandle nh_priv_;
00126 
00127   ros::Subscriber cloud_sub_;
00128   ros::Publisher cloud_pub_;
00129 
00130   ros::Timer pub_timer_;
00131 
00132   std::string fixed_frame_;
00133   tf::TransformListener tf_listener_;
00134   PointCloud accumulated_cloud_;
00135 };
00136 
00137 int main(int argc, char **argv)
00138 {
00139   ros::init(argc, argv, "cloud_mapper");
00140   PointCloudMapper mapper;
00141   ros::spin();
00142   return 0;
00143 }
00144 


pointcloud_tools
Author(s): Pep Lluis Negre
autogenerated on Mon Jan 6 2014 11:48:56