update

ros2/TrackingRos2.cpp

@@ -71,6 +71,8 @@ void TrackingRos2::ReceiveFusionResMsg(
       wit_perception_msgs::msg::DetectedObjectsWithFeatureEx>();
 
   *objsPtr = msg;
+  objsPtr->pt.push_back(GetCurTime());//（top收到的时间戳，三个点云同步完成时间戳，点云拼接完的时间戳,点云检测完成时间，检测结果发送时间）
+                                      // (收到检测结果时间戳，与相机融合之后的时间戳，融合之后发送的时间戳，tracking收到消息时间戳）
   m_tracking.PushData(objsPtr);
 
   static int countR = 0;
@@ -437,6 +439,23 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   {
     trackers.erase(iter);
   }
+  
+  //打印时间戳
+  //（top收到的时间戳，三个点云同步完成时间戳，点云拼接完的时间戳,点云检测完成时间，检测结果发送时间）
+  // (收到检测结果时间戳，与相机融合之后的时间戳，融合之后发送的时间戳，tracking收到消息时间戳）
+  auto& pt = objsPtr->pt;
+  uint64_t start_timestamp = pt.size() > 0 ? pt[0] : 0;
+  RCLCPP_INFO(this->get_logger(),"detector timestamp = %lld, delay time = [%lld,%lld,%lld,%lld,%lld,%lld,%lld,%lld,%lld,%lld]",start_timestamp,
+        pt.size() > 0 ? pt[0] - start_timestamp : 0,
+        pt.size() > 1 ? pt[1] - start_timestamp : 0,
+        pt.size() > 2 ? pt[2] - start_timestamp : 0,
+        pt.size() > 3 ? pt[3] - start_timestamp : 0,
+        pt.size() > 4 ? pt[4] - start_timestamp : 0,
+        pt.size() > 5 ? pt[5] - start_timestamp : 0,
+        pt.size() > 6 ? pt[6] - start_timestamp : 0,
+        pt.size() > 7 ? pt[7] - start_timestamp : 0,
+        pt.size() > 8 ? pt[8] - start_timestamp : 0,
+        GetCurTime() - start_timestamp);
 
   m_tracking_res->publish(sendObjs);
   m_pubMarker->publish(makerArray);