提交更新

TrackingRosEx.cpp

@@ -683,6 +683,7 @@ objTrackListPtr TrackingRos::GetNearestCloudMsg(unsigned long long timestamp)
     objTrackListPtr msg = nullptr;
     int interval = 1000;
     std::lock_guard<std::mutex> lock(m_mtx);
+    unsigned long long tmpT = 0;
     for (int i = 0; i < m_objsCloudQueue.size(); i++)
     {
         //unsigned long long time_s = m_objsCloudQueue[i]->cloud.header.stamp.toSec() * 1000000;
@@ -692,12 +693,17 @@ objTrackListPtr TrackingRos::GetNearestCloudMsg(unsigned long long timestamp)
             msg = m_objsCloudQueue[i];
             findIdx = i;
         }
+        tmpT = time_s;
         interval = labs(timestamp - time_s);
     }
     if (findIdx != -1)
     {
         m_objsCloudQueue.erase(m_objsCloudQueue.begin(), m_objsCloudQueue.begin() + findIdx + 1);
     }
+    else
+    {
+        SDK_LOG(SDK_INFO, "GetNearestCloudMsg timestamp = %llu, cloud timestamp = %llu", timestamp, tmpT);
+    }
     return msg;
 }