Use PointCloud2 header for clustered_grasps msg

The time stamp of the PointCloud2 is more useful in this case, because it gives us information
about the time when the grasps were actually found.
clustered_grasps' frame_id wasn't set to any value before. Using the header of the PointCloud2
sets it to the correct value.

include/nodes/grasp_detection_node.h

@@ -177,6 +177,7 @@ class GraspDetectionNode
   Eigen::Vector3d view_point_; ///< (input) view point of the camera onto the point cloud
 
   CloudCamera* cloud_camera_; ///< stores point cloud with (optional) camera information and surface normals
+  std_msgs::Header cloud_camera_header_; ///< stores header of the point cloud
 
   int size_left_cloud_; ///< (input) size of the left point cloud (when using two point clouds as input)
   bool has_cloud_, has_normals_, has_samples_; ///< status variables for received (input) messages

src/nodes/grasp_detection_node.cpp

@@ -161,13 +161,15 @@ void GraspDetectionNode::cloud_callback(const sensor_msgs::PointCloud2& msg)
       PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal);
       pcl::fromROSMsg(msg, *cloud);
       cloud_camera_ = new CloudCamera(cloud, 0, view_points);
+      cloud_camera_header_ = msg.header;
       ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points and normals.");
     }
     else
     {
       PointCloudRGBA::Ptr cloud(new PointCloudRGBA);
       pcl::fromROSMsg(msg, *cloud);
       cloud_camera_ = new CloudCamera(cloud, 0, view_points);
+      cloud_camera_header_ = msg.header;
       ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points.");
     }
 
@@ -297,7 +299,7 @@ gpd::GraspConfigList GraspDetectionNode::createGraspListMsg(const std::vector<Gr
   for (int i = 0; i < hands.size(); i++)
     msg.grasps.push_back(convertToGraspMsg(hands[i]));
 
-  msg.header.stamp = ros::Time::now();
+  msg.header = cloud_camera_header_;
 
   return msg;
 }