From 0c093313dc22527881ec0830c71aadbbf1b4aeb4 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 23 Apr 2015 14:22:53 +0200
Subject: [PATCH] squelette classe Callback

---
 hand_control/src/normal_estimator.cpp | 46 +++++++++++++++++++++------
 1 file changed, 36 insertions(+), 10 deletions(-)

diff --git a/hand_control/src/normal_estimator.cpp b/hand_control/src/normal_estimator.cpp
index feb1e2a..7c6cc6d 100644
--- a/hand_control/src/normal_estimator.cpp
+++ b/hand_control/src/normal_estimator.cpp
@@ -9,16 +9,42 @@ typedef pcl::PointCloud<Point> PointCloud;
 typedef Eigen::Vector4f PCLCoord; // vecteur, accès par v(0), v(1)...
 typedef geometry_msgs::Quaternion ROSCoord; // struct : x, y, z, w
 
-const ROSCoord::ConstPtr
-PCLCoord_to_ROSCoord(const PCLCoord& pcl_coord)
-{
-  ROSCoord::Ptr ros_coord(new ROSCoord());
-  ros_coord->x = pcl_coord(0); // a
-  ros_coord->y = pcl_coord(1); // b
-  ros_coord->z = pcl_coord(2); // c
-  ros_coord->w = pcl_coord(3); // d
-  return ros_coord;
-}
+class Callback {
+  public:
+    void
+    operator()(const PointCloud::ConstPtr& msg)
+    {
+      ROS_INFO("PointCloud received");
+      PCLCoord pcl_coord();
+      float curvature;
+      std::vector<int> indices();
+      // TODO : choisir tous les indices
+      estimator.computePointNormal(*msg, indices,
+                                    PCLCoord, curvature);
+      /* TODO
+      if (curvature <  ?
+        publisher.publish(to_ROSCoord(pcl_coord));
+        */
+    }
+
+    Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
+
+  private:
+    ros::Publisher publisher;
+
+    pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
+
+    const ROSCoord::ConstPtr
+    to_ROSCoord(const PCLCoord& pcl_coord)
+    {
+      ROSCoord::Ptr ros_coord(new ROSCoord());
+      ros_coord->x = pcl_coord(0); // a
+      ros_coord->y = pcl_coord(1); // b
+      ros_coord->z = pcl_coord(2); // c
+      ros_coord->w = pcl_coord(3); // d
+      return ros_coord;
+    }
+};
 
 int
 main(int argc, char** argv)