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