From 418b0cef95bf7dec44e8f9b369da1a91a589d401 Mon Sep 17 00:00:00 2001
From: Louis-Guillaume DUBOIS <contact@lgdubois.fr>
Date: Sat, 2 May 2015 18:28:40 +0200
Subject: [PATCH] better numbering in normal_estimator

---
 src/normal_estimator.cpp     | 9 ++++-----
 src/random_pcl_publisher.cpp | 8 +++++++-
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/normal_estimator.cpp b/src/normal_estimator.cpp
index ac433c5..8a30d83 100644
--- a/src/normal_estimator.cpp
+++ b/src/normal_estimator.cpp
@@ -29,22 +29,21 @@ class Callback {
 
       // publication
       ROS_INFO("Plan published");
-      publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp));
+      publisher.publish(to_Plan(x, y, z, h, c, msg->header.seq, msg->header.stamp));
     }
 
     Callback(ros::Publisher& pub) :
-      publisher(pub), estimator(), number(0) {}
+      publisher(pub), estimator() {}
 
   private:
     ros::Publisher publisher;
     pcl::NormalEstimationOMP<Point, pcl::Normal> estimator;
-    uint32_t number;
 
     inline
     const hand_control::Plan::ConstPtr
     to_Plan(const float& x, const float& y,
             const float& z, const float& h,
-            const float& c, uint64_t msec64)
+            const float& c, uint32_t seq, uint64_t msec64)
     {
       hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
       ros_msg->normal.x = x; 
@@ -57,7 +56,7 @@ class Callback {
       uint64_t nsec64 = (msec64 % 1000000) * 1000;
       ros_msg->header.stamp.sec = (uint32_t) sec64;
       ros_msg->header.stamp.nsec = (uint32_t) nsec64;
-      ros_msg->header.seq = number++;
+      ros_msg->header.seq = seq;
       ros_msg->header.frame_id = "0";
       return ros_msg;
     }
diff --git a/src/random_pcl_publisher.cpp b/src/random_pcl_publisher.cpp
index 63c2acd..394f0d9 100644
--- a/src/random_pcl_publisher.cpp
+++ b/src/random_pcl_publisher.cpp
@@ -1,4 +1,5 @@
 #include <ros/ros.h>
+#include <time.h>
 #include <pcl_ros/point_cloud.h>
 #include <pcl/point_types.h>
 // for UniformGenerator
@@ -13,7 +14,7 @@ class Generator
 {
   public:
     Generator(int len, double m, double M)
-    : length(len), min(m), max(M), cgen()
+    : length(len), min(m), max(M), cgen(), number(0)
     {
       UGenerator::Parameters params(min, max, -1);
       cgen.setParameters(params);
@@ -30,6 +31,10 @@ class Generator
         pcl->points[i].g = (uint8_t) 255;
         pcl->points[i].b = (uint8_t) 0;
       }
+      ros::Time now = ros::Time::now();
+      pcl->header.stamp =  now.toNSec() / 1000;
+      pcl->header.seq = number++;
+      pcl->header.frame_id = "0";
       return pcl;
     }
 
@@ -37,6 +42,7 @@ class Generator
     pcl::common::CloudGenerator<pcl::PointXYZRGB, UGenerator> cgen;
     int length;
     double min, max;
+    uint32_t number;
 };
 
 int