adds Header and timing to estimated plans

CMakeLists.txt

@@ -27,7 +27,9 @@ generate_messages(
   geometry_msgs
 )
 
-catkin_package()
+catkin_package(
+CATKIN_DEPENDS message_runtime
+)
 
 include_directories(
   ${catkin_INCLUDE_DIRS}

msg/Plan.msg

@@ -1,3 +1,4 @@
+Header header
 geometry_msgs/Point normal
 float64 altitude
 float64 curvature

src/normal_estimator.cpp

@@ -3,6 +3,7 @@
 #include <pcl/point_types.h>
 #include <pcl/features/normal_3d_omp.h>
 #include <hand_control/Plan.h>
+#include <time.h>
 
 typedef pcl::PointXYZRGB Point;
 typedef pcl::PointCloud<Point> PointCloud;
@@ -28,7 +29,7 @@ class Callback {
 
       // publication
       ROS_INFO("Plan published");
-      publisher.publish(to_Plan(x, y, z, h, c));
+      publisher.publish(to_Plan(x, y, z, h, c, msg->header.stamp));
     }
 
     Callback(ros::Publisher& pub) : publisher(pub), estimator() {}
@@ -36,12 +37,13 @@ class Callback {
   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)
+            const float& c, uint64_t msec64)
     {
       hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
       ros_msg->normal.x = x; 
@@ -49,6 +51,13 @@ class Callback {
       ros_msg->normal.z = z; 
       ros_msg->altitude = h;
       ros_msg->curvature = c; 
+      // uint64_t msec64 is in ms (10-6)
+      uint64_t sec64 = msec64 / 1000000;
+      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.frame_id = "0";
       return ros_msg;
     }