提交保存毫米波雷达的id和tracking信息，并用来做匹配

ros2/RosMsg.h

@@ -17,8 +17,11 @@ struct TrackStructObj
     int dataSource;
     int cameraId;
     int trackingId;
-    uint64_t id;
     int cameraId_real;
+    int radarId;
+    int radarTrackingId;
+    int radarId_real;
+    uint64_t id;
     std::shared_ptr<autoware_auto_perception_msgs::msg::TrackedObject>  obj;
 };
 

ros2/Track3DEx.cpp

@@ -9,7 +9,7 @@
 
 void Track3DEx::Init(const std::vector<float>& data)
 {
-    if (data.size() != 11)
+    if (data.size() != 13)
         return;
     std::vector<float> tmp(data.begin() + 1, data.begin() + 8);
     //修正角度在-PI到PI
@@ -179,7 +179,7 @@ void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<floa
 {
     if (kf_ == nullptr)
         return;
-    if (data.size() != 11)
+    if (data.size() != 13)
     {
         // SDK_LOG(SDK_INFO, "UpdateDataCheck data size is not 11");
         return;
@@ -241,7 +241,7 @@ void Track3DEx::UpdateDataCheck(const std::vector<float>& data, std::vector<floa
 
 double Track3DEx::CalculateIou(const std::vector<float>& data)
 {
-    if (data.size() != 11)
+    if (data.size() != 13)
     {
         // SDK_LOG(SDK_INFO, "CalculateIou data size != 9");
         return 0.0f;
@@ -251,9 +251,13 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
     int input_dataSource = data[8];
     int input_cameraId = data[9];
     int input_trackingId = data[10];
+    int input_radarId = data[11];
+    int input_radarTrackingId = data[12];
     int obj_dataSource = m_obj->dataSource;
     int obj_cameraId = m_obj->cameraId;
     int obj_trackingId = m_obj->trackingId;
+    int obj_radarId = m_obj->radarId;
+    int obj_radarTrackingId = m_obj->radarTrackingId;
 
     if(input_dataSource == obj_dataSource && obj_cameraId > 0 && obj_trackingId > 0)
     {
@@ -262,6 +266,13 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
             return 1.0f;
         }
     }
+    else if(input_dataSource == obj_dataSource && obj_radarId > 0 && obj_radarTrackingId > 0)
+    {
+        if(obj_cameraId == input_cameraId && input_radarTrackingId == obj_radarTrackingId)
+        {
+            return 1.0f;
+        }
+    }
     else if(obj_cameraId > 0 && obj_trackingId > 0)
     {
         if(obj_cameraId == input_cameraId && input_trackingId == obj_trackingId)
@@ -269,6 +280,13 @@ double Track3DEx::CalculateIou(const std::vector<float>& data)
             return 0.1f;
         }        
     }
+    else if(obj_radarId > 0 && obj_radarTrackingId > 0)
+    {
+        if(obj_radarId == input_radarId && input_radarTrackingId == obj_radarTrackingId)
+        {
+            return 0.1f;
+        }        
+    }    
     int input_type = data[0];
     int obj_type = m_obj->type;
     // if (input_type != obj_type)
@@ -302,7 +320,7 @@ void Track3DEx::GetSpeed(float& x, float& y)//获取速度值
 {
     if(m_obj == nullptr)
         return;
-    if(m_obj->dataSource < 10 && m_obj->cameraId_real < 10)
+    if(m_obj->dataSource < 10 && m_obj->radarId_real == 0)
     {
         float totelX = 0,totelY = 0;
         for(int i = 0; i < m_speedXLists.size(); i++)

ros2/TrackingRos2.cpp

@@ -135,6 +135,8 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     data.push_back(dataSource);
     data.push_back(objsPtr->feature_objects[i].camera_id);
     data.push_back(objsPtr->feature_objects[i].tracking_id);
+    data.push_back(objsPtr->feature_objects[i].radar_id);
+    data.push_back(objsPtr->feature_objects[i].radar_tracking_id);
     if (m_tracking.m_using_high_low_ekf == 0 || (dataSource == 1 || pos.x > 80) )
     {
         inputH.emplace_back(data);
@@ -190,7 +192,9 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
     int last_type = -1;
     int is_need_send = 0;  //是否需要发送
     int camera_id = 0;
-    int tracking_id = 0;  
+    int tracking_id = 0;
+    int radar_id = 0;
+    int radar_tracking_id = 0;
     int updateNum = 0;
     // if (updateId.find(iter.first) != updateId.end()) {
     //   input_obj = objsPtr->feature_objects
@@ -238,6 +242,8 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
             obj.object_id = iter.second->m_obj->obj->object_id;
             camera_id = iter.second->m_obj->cameraId;
             tracking_id = iter.second->m_obj->trackingId;
+            radar_id = iter.second->m_obj->radarId;
+            radar_tracking_id = iter.second->m_obj->radarTrackingId;
         }
         else
         {
@@ -322,6 +328,9 @@ void TrackingRos2::TrackingPorcess(objTrackListPtr objsPtr) {
         strObj.cameraId = input_obj.camera_id == 0 ? camera_id: input_obj.camera_id;
         strObj.cameraId_real = input_obj.camera_id;
         strObj.trackingId = input_obj.tracking_id == 0 ? tracking_id: input_obj.tracking_id;
+        strObj.radarId = input_obj.radar_id == 0 ? radar_id: input_obj.radar_id;
+        strObj.radarTrackingId = input_obj.radar_tracking_id == 0 ? radar_tracking_id: input_obj.radar_tracking_id;
+        strObj.radarId_real = input_obj.radar_id;
         strObj.obj = std::make_shared<autoware_auto_perception_msgs::msg::TrackedObject>(obj);
         strObj.id = iter.first;
         iter.second->m_obj = std::make_shared<TrackStructObj>(strObj);  //不仅状态变量还有类型时间戳的m_obj数据更新（Tracker3D.h的智能指针）