提交日志打印修改

ros2/TrackingRos2.cpp

@@ -264,6 +264,8 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
                     continue;
                 }
             }
+            TrackLineInfo tInfo = {};
+            m_trackLineInfo[iter.first] = tInfo;
         }
         std::vector<float> data;
         if (iter.second->IsValid(updateNum) &&
@@ -352,8 +354,8 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     linear.y = speed_y;
     geometry_msgs::msg::Point& poss =
         obj.kinematics.pose_with_covariance.pose.position;    
-    if (is_need_send == 1 && orient.y < 2 && poss.y > -15 && poss.y < 15) {
-
+    if (is_need_send == 1 && orient.y < 2 && poss.y > -15 && poss.y < 15) 
+    {
       float rot_y_angle = orient.y;
       Eigen::AngleAxisd v3(rot_y_angle, Eigen::Vector3d(0, 0, 1));
       Eigen::Quaterniond rquat(v3);
@@ -429,9 +431,60 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
             iter.first,obj.classification[0].label,cof,poss.x,poss.y,poss.z,linear.x,linear.y);       
         sendObjs.objects.emplace_back(obj);
       }
+      if(m_trackLineInfo.find(iter.first) != m_trackLineInfo.end())
+      {
+        TrackLineInfo& tInfo = m_trackLineInfo[iter.first];
+        if(tInfo.startFrameNum == 0)
+        {
+            tInfo.startFrameNum = frameNum;
+            tInfo.startX = poss.x;
+            tInfo.startY = poss.y;
+            tInfo.startZ = poss.z;
+            tInfo.startRotY = orient.y;
+            tInfo.startDataSource = input_obj.data_source;
+            tInfo.startLabel = obj.classification[0].label;
+        }
+        else
+        {
+            tInfo.endFrameNum = frameNum;
+            tInfo.endX = poss.x;
+            tInfo.endY = poss.y;
+            tInfo.endZ = poss.z;
+            tInfo.endRotY = orient.y;
+            tInfo.endDataSource = input_obj.data_source;
+            tInfo.endLabel = obj.classification[0].label;
+            tInfo.updateNum = updateNum;
+        }
+      }
     }
   }
-  for (int i = 0; i < lostId.size(); i++) {
+  for (int i = 0; i < lostId.size(); i++) 
+  {
+    if(m_trackLineInfo.find(lostId[i]) != m_trackLineInfo.end())
+    {
+        TrackLineInfo& tInfo = m_trackLineInfo[lostId[i]];
+        static int count1 = 0,count2 = 0,count3 = 0,count4 = 0;
+        if(tInfo.startX < -40 && tInfo.endX > 60)
+            count1++;
+        else if(tInfo.startX > 60 && tInfo.endX < -40)
+            count1++;
+        if(tInfo.startX < -40 && tInfo.endX > -40 && tInfo.endX < 60)
+            count2++;
+        else if(tInfo.startX < 60 && tInfo.startX > -40  && tInfo.endX < -40)
+            count2++;
+        if(tInfo.startX > -40 && tInfo.startX < 60 && tInfo.endX > 60)
+            count3++;
+        else if(tInfo.startX > 60 && tInfo.endX > -40  && tInfo.endX < 60)
+            count3++;
+        if(tInfo.startX > -40 && tInfo.startX < 60 && tInfo.endX > -40 && tInfo.endX < 60)
+            count4++;                        
+        RCLCPP_INFO(this->get_logger(),"m_trackLineInfo id = %llu, count = %d, update = %d, start = [%f,%f,%f][%d][%d],end = [%f,%f,%f][%d][%d]",
+                        lostId[i],tInfo.endFrameNum - tInfo.startFrameNum,tInfo.updateNum,
+                        tInfo.startX,tInfo.startY,tInfo.startZ,tInfo.startDataSource,tInfo.startLabel,
+                        tInfo.endX,tInfo.endY,tInfo.endZ,tInfo.endDataSource,tInfo.endLabel);
+        RCLCPP_INFO(this->get_logger(),"m_trackLineInfoCount count1 = %d, count2 = %d, count3 = %d, count4 = %d",count1,count2,count3,count4);
+        m_trackLineInfo.erase(lostId[i]);
+    }
     for(auto iter: m_yoloTrackingObj)
     {
         if(iter.second == lostId[i])

ros2/TrackingRos2.h

@@ -20,6 +20,25 @@
 namespace juefx_tracking {
 
 
+struct TrackLineInfo
+{
+    float startX;
+    float startY;
+    float startZ;
+    float startRotY;
+    int startLabel;
+    int startDataSource;
+    int startFrameNum = 0;
+    float endX;
+    float endY;
+    float endZ;
+    float endRotY;
+    int endLabel;
+    int endDataSource;
+    int endFrameNum = 0;
+    int updateNum = 0;
+};
+
 /**
  * 
  *
@@ -45,6 +64,7 @@ class TrackingRos2 : public rclcpp::Node {
     void TrackingPorcess(objTrackListPtr objsPtr);
 
     std::map<int, int64_t> m_yoloTrackingObj;
+    std::map<int64_t,TrackLineInfo> m_trackLineInfo;//记录轨迹的数据结构
     TrackingObj m_tracking;
     YAML::Node m_config;
 };