trd join

apps/localize_ppk.cpp

@@ -71,7 +71,7 @@ int main(int argc, char *argv[])
             auto start = std::chrono::high_resolution_clock::now();
             adjustPPK.ReadBag(bag_iter->path().string());
             auto end = std::chrono::high_resolution_clock::now();
-            auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end - start).count() * 1000;
+            auto time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end - start).count();
             LOG(INFO) << "ReadBag time_used " << time_used;
             break;
         }

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.cpp

@@ -24,6 +24,7 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
     std::ofstream ofs(ieBaseDir_ + "/CloudPredictPose.txt", ios::out);
     ofs.close();
 
+    ofs_.open(ieBaseDir_ + "/gnss_result.txt", ios::out);
     Matrix4d m;
     /////////////// cpt ///////////////
     m << -0.9999146, 00.0128011, 00.0026317, 00.3097527,
@@ -37,7 +38,14 @@ AdjustPPK::AdjustPPK(const string &ieBaseDir, const string &mapDir)
     Vector3d rpy= RotationQuaternionToEulerVector(q);
     LOG(INFO) << "calib_: " << calib_.translation().transpose()
               << " rpy: " << rpy.transpose();
-    boost::thread trd(boost::bind(&AdjustPPK::LoadMap, this));
+    trd_.reset(new boost::thread(boost::bind(&AdjustPPK::LoadMap, this)));
+}
+
+AdjustPPK::~AdjustPPK()
+{
+    ofs_.close();
+    requestQuit_ = true;
+    trd_->join();
 }
 
 void AdjustPPK::ConfigConvert(const string &firetimeFile, const string &correctionFile)
@@ -283,7 +291,6 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
         double backPTime, double frontPTime)
 {
     vector<IsometryData> periodPose;
-    std::ofstream ofs(ieBaseDir_ + "/gnss_result.txt", ios::out);
     periodPose.reserve(150);
     for(; ppkIndex_ < localPoseVec_.size(); ppkIndex_++){
         const auto &pose = localPoseVec_.at(ppkIndex_);
@@ -300,7 +307,7 @@ vector<IsometryData> AdjustPPK::HandleLocalPPK(
         Quaterniond q(mapPose_.linear());
         Vector3d rpy= RotationQuaternionToEulerVector(q);
         auto rawData = rawData_.at(ppkIndex_);
-        ofs << setprecision(16) << "qx" << ", " << rawData.week << ", " << rawData.seconds
+        ofs_ << setprecision(16) << "qx" << ", " << rawData.week << ", " << rawData.seconds
             << ", " << calibedBLH.x() << ", " << calibedBLH.y() << ", " << calibedBLH.z()
             << ", " << rawData.n_velocity << ", " << rawData.e_velocity << ", " << rawData.u_velocity
             << ", " << rpy.x() << ", " << rpy.y() << ", " << rpy.z() << ", " << rawData.status << endl;
@@ -442,37 +449,36 @@ void AdjustPPK::LoadMap()
         boost::filesystem::create_directory(outputDir);
     }
     for(const auto &period : ppkPeriodVec_){
+        if(requestQuit_){
+            return;
+        }
         auto meshId = period.meshId;
         if(meshVec_.find(meshId) == meshVec_.end()){
             LOG(WARNING) << "meshVec_ no meshId: " << meshId;
             continue;
         }
         auto &meshInfo = meshVec_.at(meshId);
-        if(meshInfo.matcher){
-            for(size_t id = period.startId; id <= period.endId; id += 100){
-                auto pose = localPoseVec_.at(id).pose;
-                if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
-                    meshInfo.matcher->loadArea(pose.translation());
-                }
+        if(!meshInfo.matcher){
+            string streamPath = outputDir + to_string(meshId) + ".stream";
+
+            if(!exists(streamPath)){
+                Compress(meshInfo.pcdPathVec, streamPath);
+                LOG(INFO) << "mesh compressed: " << meshId;
+            }else{
+                LOG(INFO) << "mesh exist: " << meshId;
             }
-            continue;
-        }
-        string streamPath = outputDir + to_string(meshId) + ".stream";
 
-        if(!exists(streamPath)){
-            Compress(meshInfo.pcdPathVec, streamPath);
-            LOG(INFO) << "mesh compressed: " << meshId;
-        }else{
-            LOG(INFO) << "mesh exist: " << meshId;
+            if(LoadMesh(streamPath, meshInfo.matcher)){
+                LOG(INFO) << "mesh loaded: " << meshId;
+            }
         }
-
-        if(LoadMesh(streamPath, meshInfo.matcher)){
-            LOG(INFO) << "mesh loaded: " << meshId;
-            for(size_t id = period.startId; id <= period.endId; id += 100){
-                auto pose = localPoseVec_.at(id).pose;
-                if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
-                    meshInfo.matcher->loadArea(pose.translation());
-                }
+        for(size_t id = period.startId; id <= period.endId; id += 100){
+            if(requestQuit_){
+                return;
+            }
+            auto pose = localPoseVec_.at(id).pose;
+            if(!meshInfo.matcher->IsMapLoaded(pose.translation())){
+                meshInfo.matcher->loadArea(pose.translation());
             }
         }
     }

libs/locate_system/adjust_ppk_by_locate/adjust_ppk.h

@@ -47,6 +47,8 @@ public:
     using Ptr = boost::shared_ptr<AdjustPPK>;
     AdjustPPK(const string &ieBaseDir, const string &mapDir);
 
+    ~AdjustPPK();
+
     void ConfigConvert(const std::string& firetimeFile, const std::string &correctionFile);
 
     void ReadBag(const string &bagPath);
@@ -98,6 +100,8 @@ private:
 
     boost::shared_ptr<pandar_pointcloud::Convert> convert_;
 
+    boost::shared_ptr<boost::thread> trd_;
+
     size_t ppkIndex_ = 0;
 
     size_t periodIndex_ = 0;
@@ -113,6 +117,10 @@ private:
     Isometry3d mapPose_ = Isometry3d::Identity();
 
     uint32_t cloudCnt_ = 0;
+
+    std::ofstream ofs_;
+
+    bool requestQuit_ = false;
 };
 
 } // end of namespace