提交更新

ros2/TrackingRos2.cpp

@@ -179,7 +179,7 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
   // jsk_recognition_msgs::BoundingBoxArray sendBoxes = {};
   // visualization_msgs::MarkerArray markerArray = {};
   // sendBoxes.header.frame_id = "/rslidar";
-  std::map<int,int> sameObj;
+  std::vector<uint64_t> deleteObj;
   for (auto& iter : trackers) {
     autoware_auto_perception_msgs::msg::TrackedObject obj = {};
     wit_perception_msgs::msg::DetectedObjectWithFeatureEx input_obj = {};
@@ -210,6 +210,27 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
       else
       {
         obj.object_id = generate_uuid();
+        if(input_obj.data_source > 1 && input_obj.camera_id > 0 && input_obj.tracking_id > 0)//这个是相机的检测结果
+        {
+            if(m_yoloTrackingObj.find(input_obj.tracking_id) == m_yoloTrackingObj.end())
+            {
+                m_yoloTrackingObj[input_obj.tracking_id] = iter.first;
+            }
+            else
+            {
+                deleteObj.push_back(iter.first);
+                //已经存在了这个trackingId，那么就跳过这个物体
+                // TrackStructObj strObj = {};
+                // strObj.frame = timestamp;
+                // strObj.type = input_obj.object.classification[0].label;
+                // strObj.dataSource = input_obj.data_source;
+                // strObj.cameraId = input_obj.camera_id == 0 ? camera_id: input_obj.camera_id;
+                // strObj.trackingId = input_obj.tracking_id == 0 ? tracking_id: input_obj.tracking_id;
+                // strObj.obj = std::make_shared<autoware_auto_perception_msgs::msg::TrackedObject>(obj);
+                // iter.second->m_obj = std::make_shared<TrackStructObj>(strObj);  //不仅状态变量还有类型时间戳的m_obj数据更新（Tracker3D.h的智能指针）
+                continue;
+            }
+        }
       }
 
       std::vector<float> data;
@@ -357,18 +378,18 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         markerText.text = str;        
         makerTextArray.markers.emplace_back(markerText);        
         sendObjs.objects.emplace_back(obj);
-        if(iter.second->m_obj->cameraId > 0 && iter.second->m_obj->trackingId > 0)
-        {
-            if(sameObj.find(iter.second->m_obj->trackingId) == sameObj.end())
-            {
-                sameObj[iter.second->m_obj->trackingId] = 0;
-            }
-            sameObj[iter.second->m_obj->trackingId]++;
-        }
       }
     }
   }
   for (int i = 0; i < lostId.size(); i++) {
+    for(auto iter: m_yoloTrackingObj)
+    {
+        if(iter.second == lostId[i])
+        {
+            m_yoloTrackingObj.erase(iter.first);
+            break;
+        }
+    }
     visualization_msgs::msg::Marker marker = {};
     marker.lifetime = rclcpp::Duration(1);
     marker.header.frame_id = "top_lidar";
@@ -412,6 +433,10 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     markerText.color.a = 0.5;
     makerTextArray.markers.emplace_back(marker);
   }
+  for(auto iter: deleteObj)
+  {
+    trackers.erase(iter);
+  }
 
   m_tracking_res->publish(sendObjs);
   m_pubMarker->publish(makerArray);

ros2/TrackingRos2.h

@@ -42,7 +42,7 @@ class TrackingRos2 : public rclcpp::Node {
 
     void TrackingPorcess(objTrackListPtr objsPtr);
 
-
+    std::map<int, int64_t> m_yoloTrackingObj;
     TrackingObj m_tracking;
     YAML::Node m_config;
 };