send pointcloud header for better ROS compatibility

Author: juliangaal

src/demo.cpp

@@ -19,9 +19,10 @@ ros::Publisher pub_ground;
 ros::Publisher pub_non_ground;
 
 template<typename T>
-sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, std::string frame_id = "map") {
+sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud<T> cloud, const ros::Time& stamp, std::string frame_id = "map") {
     sensor_msgs::PointCloud2 cloud_ROS;
     pcl::toROSMsg(cloud, cloud_ROS);
+    cloud_ROS.header.stamp = stamp;
     cloud_ROS.header.frame_id = frame_id;
     return cloud_ROS;
 }
@@ -38,12 +39,12 @@ void callbackCloud(const sensor_msgs::PointCloud2::Ptr &cloud_msg)
 
     PatchworkppGroundSeg->estimate_ground(pc_curr, pc_ground, pc_non_ground, time_taken);
 
-    cout << "\033[1;32m" << "Result: Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() <<  "/ NonGround: " << pc_non_ground.size()
-         << " (running_time: " << time_taken << " sec)" << "\033[0m" << endl;
+    ROS_INFO_STREAM("\033[1;32m" << "Input PointCloud: " << pc_curr.size() << " -> Ground: " << pc_ground.size() <<  "/ NonGround: " << pc_non_ground.size()
+         << " (running_time: " << time_taken << " sec)" << "\033[0m");
 
-    pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.frame_id));
-    pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.frame_id));
-    pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.frame_id));
+    pub_cloud.publish(cloud2msg(pc_curr, cloud_msg->header.stamp, cloud_msg->header.frame_id));
+    pub_ground.publish(cloud2msg(pc_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));
+    pub_non_ground.publish(cloud2msg(pc_non_ground, cloud_msg->header.stamp, cloud_msg->header.frame_id));
 }
 
 int main(int argc, char**argv) {