test

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -16,9 +16,9 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
     LoadMapInfo();
     CalLocalPose();
     CalPPKPeriod();
-    std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::out);
+    std::ofstream ofs("/home/juefx/wangdawei/CloudPredictPose.txt", ios::out);
     ofs.close();
-    ofs.open(ieBaseDir_ + "/LocalPPK.txt", ios::out);
+    ofs.open("/home/juefx/wangdawei/LocalPPK.txt", ios::out);
     ofs.close();
     Matrix4d m;
     /////////////// cpt ///////////////
@@ -35,6 +35,7 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
 void AdjustPPK::ConfigConvert(const string &firetimeFile, const string &correctionFile)
 {
     convert_.reset(new pandar_pointcloud::Convert(firetimeFile, correctionFile));
+    LOG(INFO) << "ConfigConvert done";
 }
 
 void AdjustPPK::ReadBag(const string &bagPath)

libs/locate_system/adjust_ppk_by_locate/convert.cc

@@ -200,8 +200,6 @@ Convert::Convert(const std::string &firetimeFile, const std::string &correctionF
 //  boost::thread publishPointsThr(
 //      boost::bind(&Convert::publishPointsThread, this));
 
-  init();
-
 }
 
 bool Convert::processScan(
@@ -592,6 +590,10 @@ void Convert::moveTaskEndToStartAngle() {
 }
 
 void Convert::init() {
+    static bool inited = false;
+    if(inited){
+        return;
+    }
 	while (1) {
 		if(!m_PacketsBuffer.hasEnoughPackets()) {
 			usleep(1000);
@@ -690,6 +692,7 @@ void Convert::init() {
       m_OutMsgArray[1] = outMag1;
       break;
     }
+    inited = true;
 }
 
 void Convert::publishPoints() {
@@ -1422,10 +1425,10 @@ bool Convert::processPacket(PPointCloud& pc)
     static int pktCount = 0;
     static int cursor = 0;
     if (!m_PacketsBuffer.hasEnoughPackets()) {
-      usleep(1000);
+//      usleep(1000);
       return false;
-    }
-
+    }    
+    init();
     if (0 == checkLiadaMode()) {
       // ROS_WARN("checkLiadaMode now!!");
       m_OutMsgArray[cursor]->clear();