Ajout du test PCL

Debut utilisation PCL pour segmenter une image -> entrée interface Rviz

my_pcl_tutorial/src/plannar_segmentation.cpp

@@ -0,0 +1,74 @@
+#include <ros/ros.h>
+// PCL specific includes
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+
+
+ros::Publisher pub;
+
+void 
+cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
+{
+  // Create a container for the data.
+  sensor_msgs::PointCloud2 output;
+
+  //Conversions des donnée d'entrée en PCL
+  pcl::PCLPointCloud2 pcl_pc2;
+  pcl_conversions::toPCL(*input,pcl_pc2);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromPCLPointCloud2(pcl_pc2,*cloud);
+
+  //Processing
+  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+  // Create the segmentation object
+  pcl::SACSegmentation<pcl::PointXYZ> seg;
+  // Optional
+  seg.setOptimizeCoefficients (true);
+  // Mandatory
+  seg.setModelType (pcl::SACMODEL_PLANE);
+  seg.setMethodType (pcl::SAC_RANSAC);
+  seg.setDistanceThreshold (0.01);
+
+  seg.setInputCloud (cloud);
+  seg.segment (*inliers, *coefficients);
+
+  ROS_INFO_STREAM("Coeff : "<<coefficients->values[0]<<" / "<<coefficients->values[1]<<" / "<<coefficients->values[2]<<" / "<<coefficients->values[3]);
+
+  //Créations de l'output
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
+
+  for (size_t i = 0; i < inliers->indices.size (); ++i)
+  {
+    pcl_output->points.push_back( cloud->points[inliers->indices[i]]);
+  }
+  pcl::toPCLPointCloud2(*pcl_output,pcl_pc2);
+  pcl_conversions::fromPCL(pcl_pc2, output);
+
+  output.header.frame_id = input->header.frame_id;
+
+  // Publish the data.
+  pub.publish (output);
+}
+
+int main (int argc, char** argv)
+{
+  // Initialize ROS
+  ros::init (argc, argv, "my_pcl_tutorial");
+  ros::NodeHandle nh;
+
+  // Create a ROS subscriber for the input point cloud
+  ros::Subscriber sub = nh.subscribe ("/camera/depth_registered/points", 1, cloud_cb);
+
+  // Create a ROS publisher for the output point cloud
+  pub = nh.advertise<sensor_msgs::PointCloud2> ("/plane_output", 1);
+
+  // Spin
+  ros::spin ();
+}