From 656305cd8abb8446b19c44e221db2e783bdf165a Mon Sep 17 00:00:00 2001
From: Unknown <antoine.harle@etu.upmc.fr>
Date: Fri, 29 Jun 2018 14:33:14 +0200
Subject: [PATCH] Detection du centre de l'objet

---
 my_pcl_tutorial/src/plannar_segmentation.cpp | 54 +++++++++++++++++++-
 1 file changed, 52 insertions(+), 2 deletions(-)

diff --git a/my_pcl_tutorial/src/plannar_segmentation.cpp b/my_pcl_tutorial/src/plannar_segmentation.cpp
index 4a162c3..1adbf51 100644
--- a/my_pcl_tutorial/src/plannar_segmentation.cpp
+++ b/my_pcl_tutorial/src/plannar_segmentation.cpp
@@ -11,7 +11,24 @@
 
 #include <pcl/filters/extract_indices.h>
 
+#include <pcl/common/centroid.h>
+
+#include <geometry_msgs/PointStamped.h>
+
+#include <pcl/pcl_config.h>
+
+//  void printinfo()
+// {
+//   std::cout<<"OK"<<std::endl;
+// #if PCL_VERSION_COMPARE(<, 1, 7, 2)
+//     std::cout<<"ARG"<<std::endl;
+// #endif
+// };
+
 ros::Publisher pub;
+ros::Publisher pubc;
+
+using namespace pcl;
 
 void 
 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
@@ -40,7 +57,7 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
   seg.setInputCloud (cloud);
   seg.segment (*inliers, *coefficients);
 
-  ROS_INFO_STREAM("Coeff : "<<coefficients->values[0]<<" / "<<coefficients->values[1]<<" / "<<coefficients->values[2]<<" / "<<coefficients->values[3]);
+  //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>);
@@ -60,14 +77,45 @@ cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
   extract.setNegative(true);
   extract.filter(*pcl_output);
 
+  //Calcul du centre
+  // CentroidPoint<pcl::PointXYZ> centroid;
+  // for (size_t i = 0; i < pcl_output->size (); ++i)
+  // {
+  //   centroid.add(*pcl_output[i]);
+  // }
+  // pcl::PointXYZ c1;
+  // centroid.get (c1);
+  Eigen::Vector4f barycentre;
+  pcl::compute3DCentroid(*pcl_output, barycentre);
 
+  geometry_msgs::PointStamped center_msg;
+  // for (size_t i = 0; i < pcl_output->size (); ++i)
+  // {
+  //   if( pcl_output->points[i].x)
+  //   center.point.x += pcl_output->points[i]._PointXYZ::data[0];
+  //   center.point.y += pcl_output->points[i]._PointXYZ::data[1];
+  //   center.point.z += pcl_output->points[i]._PointXYZ::data[2];
+  //   ROS_INFO_STREAM("Center clioyf : "<<pcl_output->points[i].x<<" / "<<pcl_output->points[i].y<<" / "<<pcl_output->points[i].z);
+  // }
+  // center.point.x = center.point.x/pcl_output->size ();
+  // center.point.y = center.point.y/pcl_output->size ();
+  // center.point.z = center.point.z/pcl_output->size ();
+  center_msg.point.x = barycentre[0];
+  center_msg.point.y = barycentre[1];
+  center_msg.point.z = barycentre[2];
+  center_msg.header.frame_id = input->header.frame_id;
+  // ROS_INFO_STREAM("Center clioyf : "<<pcl_output->points[0]._PointXYZ::data[0]<<" / "<<pcl_output->points[0]._PointXYZ::data[1]<<" / "<<pcl_output->points[0]._PointXYZ::data[2]);
+  // ROS_INFO_STREAM("barycentre : "<<barycentre(1,1)<<" / "<<barycentre(2,1)<<" / "<<barycentre(3,1)<<" / "<< barycentre(4,1));
+  ROS_INFO_STREAM("Center : "<<center_msg.point.x<<" / "<<center_msg.point.y<<" / "<<center_msg.point.z);
+
+  //Envoie du nuage de points
   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);
+  pubc.publish( center_msg);
 }
 
 int main (int argc, char** argv)
@@ -81,7 +129,9 @@ int main (int argc, char** argv)
 
   // Create a ROS publisher for the output point cloud
   pub = nh.advertise<sensor_msgs::PointCloud2> ("/plane_output", 1);
+  pubc = nh.advertise<geometry_msgs::PointStamped> ("/object_center", 1);
 
+  // printinfo();
   // Spin
   ros::spin ();
 }
\ No newline at end of file