test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -148,9 +148,9 @@ void AdjustPPK::OnReceivedCloud(const PPointCloud &cloud)
 
     LocateCloud(cloudInfo);
 
-    PointCloudJ mathedCloud;
-    pcl::transformPointCloud(*cloudInfo.frame, mathedCloud, mapPose_.cast<float>());
-    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + "_matched.pcd", mathedCloud);
+//    PointCloudJ mathedCloud;
+//    pcl::transformPointCloud(*cloudInfo.frame, mathedCloud, mapPose_.cast<float>());
+//    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(cloudInfo.timestamp) + "_matched.pcd", mathedCloud);
 }
 
 void AdjustPPK::LoadPPK()
@@ -330,7 +330,7 @@ bool AdjustPPK::Undisort(
     PointCloudJ mappedFrame;
     CloudPreprocess::Instance()->Undisort(
                             frame, undisortedFrame, &mappedFrame, rawFrame.cloud.back().time, &periodPose, framePose, increasePose);
-    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
+//    pcl::io::savePCDFileBinary(ieBaseDir_ + "/baseCloud/" + to_string(rawFrame.timestamp) + "_mapped.pcd", mappedFrame);
     cloudInfo.frame = undisortedFrame;
     cloudInfo.filterPoseInfo.increasePose = increasePose;
     cloudInfo.timestamp = rawFrame.timestamp;
@@ -369,6 +369,9 @@ bool AdjustPPK::ConfigMap(double timestamp)
         return true;
     }
     auto meshId = ppkPeriodVec_.at(periodIndex_).meshId;
+    if(meshVec_.find(meshId) == meshVec_.end()){
+        return false;
+    }
     while(!meshVec_.at(meshId).matcher){
         std::this_thread::sleep_for(std::chrono::milliseconds(100));
     }