cancel traja time duration

apps/function/multi_traj_functions.cpp

@@ -568,7 +568,7 @@ bool initTrajectory(
     auto trajPath = taskInfo.trajPath;
     string trajFileName =
             boost::filesystem::path(trajPath).filename().string();
-//    LOG(INFO) << "trajFileName: " << trajFileName;
+    LOG(INFO) << "trajFileName: " << trajFileName;
     if(trajFileName == "ie.txt"){
         trajectory.reset(new Trajectory(trajPath, PPK));
     }else if(trajFileName == "sample.gps"){

libs/trajectory/trajectory.cpp

@@ -351,22 +351,23 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index)
                 * Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY());
         trajPoint.nodeId = trajectory_.size();
         trajPoint.timestamp = timestamp;
-//        if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){
-//            LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw;
+//        if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
+//            if(multiTrajectory_[index].size() == 0){
+//                multiTrajectory_[index].push_back(trajPoint);
+//            }else{
+//                if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
+//                    multiTrajectory_[index].push_back(trajPoint);
+//                }
+//            }
 //        }
-//        if(fabs(trajPoint.timestamp - 1675021549.1) < 0.01){
-//            LOG(INFO) << "1675021549.1: " << ppk_raw_info.yaw;
-//        }
-        if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
-            if(multiTrajectory_[index].size() == 0){
+
+        if(multiTrajectory_[index].size() == 0){
+            multiTrajectory_[index].push_back(trajPoint);
+        }else{
+            if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
                 multiTrajectory_[index].push_back(trajPoint);
-            }else{
-                if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
-                    multiTrajectory_[index].push_back(trajPoint);
-                }
             }
         }
-
         break;
     }
     case NODE: