From d45cde8b6dda4f5ee2b39559278412a93722cdc4 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 23 Apr 2015 17:58:31 +0200
Subject: [PATCH] ajout message Plan

---
 hand_control/CMakeLists.txt           | 21 +++++++--------
 hand_control/msg/Plan.msg             |  3 +++
 hand_control/package.xml              |  6 +++++
 hand_control/src/normal_estimator.cpp | 37 +++++++++++++--------------
 4 files changed, 38 insertions(+), 29 deletions(-)
 create mode 100644 hand_control/msg/Plan.msg

diff --git a/hand_control/CMakeLists.txt b/hand_control/CMakeLists.txt
index 87e7100..49d245f 100644
--- a/hand_control/CMakeLists.txt
+++ b/hand_control/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   freenect_camera
   freenect_launch
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -49,11 +50,10 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+ add_message_files(
+   FILES
+   Plan.msg
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -70,10 +70,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   std_msgs
-# )
+ generate_messages(
+   DEPENDENCIES
+   std_msgs
+   geometry_msgs
+ )
 
 ###################################
 ## catkin specific configuration ##
@@ -87,7 +88,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES hand_control
-#  CATKIN_DEPENDS ardrone_autonomy freenect_stack pcl_ros perception_pcl roscpp std_msgs
+  CATKIN_DEPENDS ardrone_autonomy freenect_stack pcl_ros perception_pcl roscpp std_msgs message_runtime
 #  DEPENDS system_lib
 )
 
diff --git a/hand_control/msg/Plan.msg b/hand_control/msg/Plan.msg
new file mode 100644
index 0000000..9b30a24
--- /dev/null
+++ b/hand_control/msg/Plan.msg
@@ -0,0 +1,3 @@
+geometry_msgs/Point normal
+float64 altitude
+float64 curvature
diff --git a/hand_control/package.xml b/hand_control/package.xml
index 46a48cd..dd5511e 100644
--- a/hand_control/package.xml
+++ b/hand_control/package.xml
@@ -26,6 +26,11 @@
   <build_depend>pcl_conversions</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>freenect_stack</build_depend>
+  <build_depend>perception_pcl</build_depend>
+  <run_depend>perception_pcl</run_depend>
+  <run_depend>freenect_stack</run_depend>
   <run_depend>ardrone_autonomy</run_depend>
   <run_depend>freenect_camera</run_depend>
   <run_depend>freenect_launch</run_depend>
@@ -35,6 +40,7 @@
   <run_depend>pcl_conversions</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/hand_control/src/normal_estimator.cpp b/hand_control/src/normal_estimator.cpp
index 7c6cc6d..71ad3b5 100644
--- a/hand_control/src/normal_estimator.cpp
+++ b/hand_control/src/normal_estimator.cpp
@@ -2,12 +2,10 @@
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/features/normal_3d.h>
-#include <geometry_msgs/Quaternion.h>
+#include <hand_control/Plan.h>
 
 typedef pcl::PointXYZRGB Point;
 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
 
 class Callback {
   public:
@@ -15,16 +13,15 @@ class Callback {
     operator()(const PointCloud::ConstPtr& msg)
     {
       ROS_INFO("PointCloud received");
-      PCLCoord pcl_coord();
+      Eigen::Vector4f 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));
-        */
+                                    pcl_coord, curvature);
+      // publication
+      ROS_INFO("Plan published")
+      publisher.publish(to_Plan(pcl_coord, curvature));
     }
 
     Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
@@ -32,17 +29,18 @@ class Callback {
   private:
     ros::Publisher publisher;
 
-    pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
+    pcl::NormalEstimationOMP<Point, Eigen::Vector4f> estimator;
 
-    const ROSCoord::ConstPtr
-    to_ROSCoord(const PCLCoord& pcl_coord)
+    const hand_control::Plan::ConstPtr
+    to_Plan(const Eigen::Vector4f& pcl_coord, const float& curvature)
     {
-      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;
+      hand_control::Plan::Ptr ros_msg(new Plan());
+      ros_msg->normal.x = pcl_coord(0); // a
+      ros_msg->normal.y = pcl_coord(1); // b
+      ros_msg->normal.z = pcl_coord(2); // c
+      ros_msg->altitude = pcl_coord(3); // d
+      ros_msg->curvature = curvature; // \lambda
+      return ros_msg;
     }
 };
 
@@ -53,8 +51,9 @@ main(int argc, char** argv)
   ros::NodeHandle node("estimator");
 
   // initialisation
-  ros::Publisher  publisher = node.advertise<ROSCoord>("output", 1);
+  ros::Publisher  publisher = node.advertise<hand_control::Plan>("output", 1);
   Callback callback(publisher);
+
   ros::Subscriber subscriber = node.subscribe<PointCloud>("input", 1, callback);
 
   // démarrage