修改崩溃

TrackingRosEx.cpp

@@ -395,12 +395,13 @@ void TrackingRos::TrackingCameraCallBackFunc(const jfx_common_msgs::InferResesCo
         countC = 0;
     }
 
-    double  d_timestamp = msg->header.stamp.toSec();
-    unsigned long long timestamp = (unsigned long long)(d_timestamp * 1000);
+    unsigned long long timestamp = (unsigned long long)(msg->header.stamp.sec * 1000 + msg->header.stamp.nsec * 1e-6);
+    SDK_LOG(SDK_INFO, "msg time = %llu", timestamp);
 
     objTrackListPtr cloudPtr = GetNearestCloudMsg(timestamp);
-
-    timestamp = cloudPtr->cloud.header.stamp.toSec() * 1000000;
+    if(cloudPtr)
+        timestamp =(unsigned long long) (cloudPtr->cloud.header.stamp.sec * 1000000 + cloudPtr->cloud.header.stamp.nsec * 1e-3);
+    SDK_LOG(SDK_INFO, "cloudPtr time = %llu", timestamp);
 
     objTrackListPtr objsPtr = std::make_shared<jfx_common_msgs::det_tracking_array>();
     if (cloudPtr)
@@ -541,7 +542,8 @@ objTrackListPtr TrackingRos::GetNearestCloudMsg(unsigned long long timestamp)
     std::lock_guard<std::mutex> lock(m_mtx);
     for (int i = 0; i < m_objsCloudQueue.size(); i++)
     {
-        unsigned long long time_s = m_objsCloudQueue[i]->cloud.header.stamp.toSec() * 1000000;
+        //unsigned long long time_s = m_objsCloudQueue[i]->cloud.header.stamp.toSec() * 1000000;
+        unsigned long long time_s = (unsigned long long)(m_objsCloudQueue[i]->cloud.header.stamp.sec * 1000000 + m_objsCloudQueue[i]->cloud.header.stamp.nsec* 1e-3);
         if (labs(timestamp - time_s) < std::min(100, interval))
         {
             msg = m_objsCloudQueue[i];