[fix] Fix the program so that the time stamps of the points are the same with the original image topic

Author: ShigemichiMatsuzaki

include/pytorch_ros/pytorch_seg_trav_path_ros.h

@@ -38,10 +38,7 @@ class PyTorchSegTravPathROS {
   image_transport::Publisher  pub_prob_image_;
   ros::Publisher pub_start_point_;
   ros::Publisher pub_end_point_;
-
-  // For listening to messages
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener * tf_listener_;
+  ros::Time stamp_of_current_image_;
 
   PyTorchCppWrapperSegTravPath pt_wrapper_;
 

src/pytorch_seg_trav_path_ros.cpp

@@ -47,6 +47,7 @@ PyTorchSegTravPathROS::image_callback(const sensor_msgs::ImageConstPtr& msg)
 
   // Convert the image message to a cv_bridge object
   cv_bridge::CvImagePtr cv_ptr = msg_to_cv_bridge(msg);
+  stamp_of_current_image_ = msg->header.stamp;
 
   // Run inference
   sensor_msgs::ImagePtr label_msg;
@@ -178,9 +179,9 @@ PyTorchSegTravPathROS::tensor_to_points(const at::Tensor point_tensor, const int
   auto points_a = points.accessor<float, 2>();
 
   // Initialize messgaes
-  start_point_msg->header.stamp = ros::Time::now();
+  start_point_msg->header.stamp = stamp_of_current_image_;//ros::Time::now();
   start_point_msg->header.frame_id = "kinect2_rgb_optical_frame";
-  end_point_msg->header.stamp = ros::Time::now();
+  end_point_msg->header.stamp = stamp_of_current_image_;//ros::Time::now();
   end_point_msg->header.frame_id = "kinect2_rgb_optical_frame";
   // Point tensor has coordinate values normalized with the width and height.
   // Therefore each value is multiplied by width or height.