update

ros2/TrackingRos2.cpp

@@ -33,7 +33,7 @@ TrackingRos2::TrackingRos2(const rclcpp::NodeOptions& node_options)
                 std::placeholders::_1));
   std::string pub_topic = "/perception/object_recognition/tracking/objects";
   m_tracking_res = this->create_publisher<
-      autoware_auto_perception_msgs::msg::TrackedObjects>(pub_topic, 10);
+      wit_perception_msgs::msg::TrackedObjectsEx>(pub_topic, 10);
   m_pubMarker = this->create_publisher<visualization_msgs::msg::MarkerArray>(
       "output/sensors_fusion_obj_markers", 10);
   m_pubMarkerText = this->create_publisher<visualization_msgs::msg::MarkerArray>(
@@ -92,6 +92,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   if (!objsPtr) return;
   visualization_msgs::msg::MarkerArray makerArray = {};
   visualization_msgs::msg::MarkerArray makerTextArray = {};
+  wit_perception_msgs::msg::TrackedObjectsEx sendObjsEx = {};
   autoware_auto_perception_msgs::msg::TrackedObjects sendObjs = {};
   sendObjs.header = objsPtr->header;
   sendObjs.header.frame_id = "top_lidar";
@@ -497,8 +498,13 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
                 float(pt[5] - pt[4])/1000,float(pt[6] - pt[5])/1000,float(pt[7] - pt[6])/1000,float(pt[8] - pt[7])/1000,float(GetCurTime() - start_timestamp)/1000);      
         beginPrint = GetCurTime();
     }      
-
-  m_tracking_res->publish(sendObjs);
+  sendObjsEx.tracks = sendObjs;
+  sendObjsEx.pt = objsPtr->pt;
+  sendObjsEx.pt.push_back(GetCurTime());
+  //打印时间戳
+  //（top收到的时间戳，三个点云同步完成时间戳，点云拼接完的时间戳,点云检测完成时间，检测结果发送时间）
+  // (收到检测结果时间戳，与相机融合之后的时间戳，融合之后发送的时间戳，tracking收到消息时间戳，tracking发送的时间戳）  
+  m_tracking_res->publish(sendObjsEx);
   m_pubMarker->publish(makerArray);
   m_pubMarkerText->publish(makerTextArray);
   // m_pubBoundingBoxes.publish(sendBoxes);

ros2/TrackingRos2.h

@@ -12,6 +12,7 @@
 #include "autoware_auto_perception_msgs/msg/object_classification.hpp"
 #include "wit_perception_msgs/msg/detected_objects_with_feature_ex.hpp"
 #include "wit_perception_msgs/msg/detected_object_with_feature_ex.hpp"
+#include "wit_perception_msgs/msg/tracked_objects_ex.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
 #include "TrackingObj.h"
 //#define ONLINE
@@ -34,7 +35,7 @@ class TrackingRos2 : public rclcpp::Node {
    private:
 
     rclcpp::Subscription<wit_perception_msgs::msg::DetectedObjectsWithFeatureEx>::SharedPtr m_fusion_res;
-    rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr m_tracking_res;
+    rclcpp::Publisher<wit_perception_msgs::msg::TrackedObjectsEx>::SharedPtr m_tracking_res;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pubMarker;
     rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pubMarkerText;