1 #include <ros/ros.h>
   2 // PCL specific includes
   3 #include <pcl/ros/conversions.h>
   4 #include <pcl/point_cloud.h>
   5 #include <pcl/point_types.h>
   6 
   7 #include <pcl/sample_consensus/model_types.h>
   8 #include <pcl/sample_consensus/method_types.h>
   9 #include <pcl/segmentation/sac_segmentation.h>
  10 
  11 ros::Publisher pub;
  12 
  13 void 
  14 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
  15 {
  16   // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
  17   pcl::PointCloud<pcl::PointXYZ> cloud;
  18   pcl::fromROSMsg (*input, cloud);
  19 
  20   pcl::ModelCoefficients coefficients;
  21   pcl::PointIndices inliers;
  22   // Create the segmentation object
  23   pcl::SACSegmentation<pcl::PointXYZ> seg;
  24   // Optional
  25   seg.setOptimizeCoefficients (true);
  26   // Mandatory
  27   seg.setModelType (pcl::SACMODEL_PLANE);
  28   seg.setMethodType (pcl::SAC_RANSAC);
  29   seg.setDistanceThreshold (0.01);
  30 
  31   seg.setInputCloud (cloud.makeShared ());
  32   seg.segment (inliers, coefficients); 
  33   
  34   // Publish the model coefficients
  35   pub.publish (coefficients);
  36 }
  37 
  38 int
  39 main (int argc, char** argv)
  40 {
  41   // Initialize ROS
  42   ros::init (argc, argv, "my_pcl_tutorial");
  43   ros::NodeHandle nh;
  44 
  45   // Create a ROS subscriber for the input point cloud
  46   ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
  47 
  48   // Create a ROS publisher for the output point cloud
  49   pub = nh.advertise<pcl::ModelCoefficients> ("output", 1);
  50 
  51   // Spin
  52   ros::spin ();
  53 }