test

libs/trajectory/trajectory.cpp

@@ -319,7 +319,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
 
         {
             if(!proj_){
-                boost::unique_lock<boost::mutex> lock(Mtx_);
+                boost::unique_lock<boost::shared_mutex> lock(Mtx_);
                 if(!proj_){
                     proj_.reset(new LocalCartesian(ppk_raw_info.lat,
                                                    ppk_raw_info.lng,
@@ -341,9 +341,10 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
         trajPoint.nodeId = trajectory_.size();
         trajPoint.timestamp = gpsToUtc(ppk_raw_info.gps_week, ppk_raw_info.gps_sec);
         if((index + 1) >= multiTrajectory_.size()){
-            boost::unique_lock<boost::mutex> lock_ppk(Mtx_);
+            boost::unique_lock<boost::shared_mutex> writeLock(Mtx_);
             multiTrajectory_.resize(index + 1);
         }
+        boost::shared_lock<boost::shared_mutex> readLock(Mtx_);
         if(multiTrajectory_[index].size() == 0){
             multiTrajectory_[index].push_back(trajPoint);
         }else{
@@ -394,7 +395,7 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
         trajPoint.rotation = {node_raw_info.w, node_raw_info.qx, node_raw_info.qy, node_raw_info.qz};
         //pose yaw take -M_PI/2!!!!!!!!!!!!!!!!!!
         trajPoint.rotation = trajPoint.rotation * Eigen::AngleAxisd(-M_PI/2, Eigen::Vector3d::UnitZ());
-        boost::unique_lock<boost::mutex> lock_node(Mtx_);
+        boost::unique_lock<boost::shared_lock> lock_node(Mtx_);
         trajectory_.push_back(trajPoint);
 
         break;

libs/trajectory/trajectory.h

@@ -63,7 +63,7 @@ private:
 
     boost::shared_ptr<LocalCartesian> proj_;
 
-    boost::mutex Mtx_;
+    boost::shared_mutex Mtx_;
 };
 
 #endif // TRAJECTORY_H