Skip to content

M
multiTraj
Commit ae9077e4 authored by wangdawei's avatar wangdawei
Browse files
Options

cancel traja time duration

parent be1956b5
Hide whitespace changes
Inline Side-by-side
Showing with 14 additions and 13 deletions
apps/function/multi_traj_functions.cpp
View file @ ae9077e4
...@@ -568,7 +568,7 @@ bool initTrajectory( ...@@ -568,7 +568,7 @@ bool initTrajectory(
auto trajPath = taskInfo.trajPath; auto trajPath = taskInfo.trajPath;
string trajFileName = string trajFileName =
boost::filesystem::path(trajPath).filename().string(); boost::filesystem::path(trajPath).filename().string();
// LOG(INFO) << "trajFileName: " << trajFileName; LOG(INFO) << "trajFileName: " << trajFileName;
if(trajFileName == "ie.txt"){ if(trajFileName == "ie.txt"){
trajectory.reset(new Trajectory(trajPath, PPK)); trajectory.reset(new Trajectory(trajPath, PPK));
}else if(trajFileName == "sample.gps"){ }else if(trajFileName == "sample.gps"){
......
libs/trajectory/trajectory.cpp
View file @ ae9077e4
...@@ -351,22 +351,23 @@ void Trajectory::parseAndPush(const vector<string> &vec, uint8_t index) ...@@ -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()); * Eigen::AngleAxisd(ppk_raw_info.roll/180*M_PI, Eigen::Vector3d::UnitY());
trajPoint.nodeId = trajectory_.size(); trajPoint.nodeId = trajectory_.size();
trajPoint.timestamp = timestamp; trajPoint.timestamp = timestamp;
// if(fabs(trajPoint.timestamp - 1675018189.7) < 0.01){ // if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){
// LOG(INFO) << "1675018189.7: " << ppk_raw_info.yaw; // 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(multiTrajectory_[index].size() == 0){
// } multiTrajectory_[index].push_back(trajPoint);
if(trajPoint.timestamp > startTime_ && trajPoint.timestamp < endTime_){ }else{
if(multiTrajectory_[index].size() == 0){ if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
multiTrajectory_[index].push_back(trajPoint); multiTrajectory_[index].push_back(trajPoint);
}else{
if((trajPoint.translation - multiTrajectory_[index].back().translation).norm() > 1){
multiTrajectory_[index].push_back(trajPoint);
}
} }
} }
break; break;
} }
case NODE: case NODE:
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment