1 #include <ros/ros.h>
   2 // PCL specific includes
   3 #include <sensor_msgs/PointCloud2.h>
   4 #include <pcl_conversions/pcl_conversions.h>
   5 #include <pcl/point_cloud.h>
   6 #include <pcl/point_types.h>
   7 
   8 #include <pcl/filters/voxel_grid.h>
   9 
  10 ros::Publisher pub;
  11 
  12 void 
  13 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
  14 {
  15   // Container for original & filtered data
  16   pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  17   pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  18   pcl::PCLPointCloud2 cloud_filtered;
  19 
  20   // Convert to PCL data type
  21   pcl_conversions::toPCL(*cloud_msg, *cloud);
  22 
  23   // Perform the actual filtering
  24   pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  25   sor.setInputCloud (cloudPtr);
  26   sor.setLeafSize (0.1, 0.1, 0.1);
  27   sor.filter (cloud_filtered);
  28 
  29   // Convert to ROS data type
  30   sensor_msgs::PointCloud2 output;
  31   pcl_conversions::moveFromPCL(cloud_filtered, output);
  32 
  33   // Publish the data
  34   pub.publish (output);
  35 }
  36 
  37 int
  38 main (int argc, char** argv)
  39 {
  40   // Initialize ROS
  41   ros::init (argc, argv, "my_pcl_tutorial");
  42   ros::NodeHandle nh;
  43 
  44   // Create a ROS subscriber for the input point cloud
  45   ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2> ("input", 1, cloud_cb);
  46 
  47   // Create a ROS publisher for the output point cloud
  48   pub = nh.advertise<sensor_msgs::PointCloud2> ("output", 1);
  49 
  50   // Spin
  51   ros::spin ();
  52 }