From a3d8f76af671aa9e976be9161b35f6853d41b433 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Thu, 7 May 2015 22:37:47 +0200
Subject: [PATCH] =?UTF-8?q?add=20points=20=C2=ABnumber=C2=BB=20field=20to?=
 =?UTF-8?q?=20Plan=20msg?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 msg/Plan.msg             | 1 +
 src/normal_estimator.cpp | 6 ++++--
 2 files changed, 5 insertions(+), 2 deletions(-)

diff --git a/msg/Plan.msg b/msg/Plan.msg
index 1c8921a..e5c6c74 100644
--- a/msg/Plan.msg
+++ b/msg/Plan.msg
@@ -2,3 +2,4 @@ Header header
 geometry_msgs/Point normal
 float64 altitude
 float64 curvature
+int64 number
diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp
index 8a30d83..8dd4bb1 100644
--- a/src/normal_estimator.cpp
+++ b/src/normal_estimator.cpp
@@ -29,7 +29,7 @@ class Callback {
 
       // publication
       ROS_INFO("Plan published");
-      publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp));
+      publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp, msg->width));
     }
 
     Callback(ros::Publisher& pub) :
@@ -43,7 +43,8 @@ class Callback {
     const hand_control::Plan::ConstPtr
     to_Plan(const float& x, const float& y,
             const float& z, const float& h,
-            const float& c, uint32_t seq, uint64_t msec64)
+            const float& c, uint32_t seq,
+            uint64_t msec64, uint64_t number)
     {
       hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
       ros_msg->normal.x = x; 
@@ -51,6 +52,7 @@ class Callback {
       ros_msg->normal.z = z; 
       ros_msg->altitude = h;
       ros_msg->curvature = c; 
+      ros_msg->number = number;
       // uint64_t msec64 is in ms (10-6)
       uint64_t sec64 = msec64 / 1000000;
       uint64_t nsec64 = (msec64 % 1000000) * 1000;