better numbering in normal_estimator

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;
     }

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