| 343 | } |
| 344 | |
| 345 | bool FastPlannerManager::topoReplan(bool collide) { |
| 346 | ros::Time t1, t2; |
| 347 | |
| 348 | /* truncate a new local segment for replanning */ |
| 349 | ros::Time time_now = ros::Time::now(); |
| 350 | double t_now = (time_now - global_data_.global_start_time_).toSec(); |
| 351 | double local_traj_dt, local_traj_duration; |
| 352 | |
| 353 | Eigen::MatrixXd ctrl_pts = paramLocalTraj(t_now, local_traj_dt, local_traj_duration); |
| 354 | NonUniformBspline init_traj(ctrl_pts, pp_.bspline_degree_, local_traj_dt); |
| 355 | local_data_.start_time_ = time_now; |
| 356 | |
| 357 | std::cout << "dt: " << local_traj_dt << ", dur: " << local_traj_duration << std::endl; |
| 358 | |
| 359 | if (!collide) { |
| 360 | // No collision detected, but we can further refine the trajectory |
| 361 | refineTraj(init_traj); |
| 362 | double time_change = init_traj.getTimeSum() - local_traj_duration; |
| 363 | local_data_.position_traj_ = init_traj; |
| 364 | global_data_.setLocalTraj( |
| 365 | local_data_.position_traj_, t_now, local_traj_duration + time_change + t_now, time_change); |
| 366 | // local_data_.position_traj_ = init_traj; |
| 367 | // global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + t_now, 0.0); |
| 368 | } else { |
| 369 | // Find topologically distinctive path and guide optimization in parallel |
| 370 | plan_data_.initial_local_segment_ = init_traj; |
| 371 | vector<Eigen::Vector3d> colli_start, colli_end, start_pts, end_pts; |
| 372 | findCollisionRange(colli_start, colli_end, start_pts, end_pts); |
| 373 | |
| 374 | if (colli_start.size() == 1 && colli_end.size() == 0) { |
| 375 | ROS_WARN("Init traj ends in obstacle, no replanning."); |
| 376 | local_data_.position_traj_ = init_traj; |
| 377 | global_data_.setLocalTraj(init_traj, t_now, local_traj_duration + t_now, 0.0); |
| 378 | } else { |
| 379 | // Call topological replanning when local segment is in collision |
| 380 | /* Search topological distinctive paths */ |
| 381 | ROS_INFO("[Topo]: ---------"); |
| 382 | plan_data_.clearTopoPaths(); |
| 383 | list<GraphNode::Ptr> graph; |
| 384 | vector<vector<Eigen::Vector3d>> raw_paths, filtered_paths, select_paths; |
| 385 | topo_prm_->findTopoPaths(colli_start.front(), colli_end.back(), start_pts, end_pts, graph, |
| 386 | raw_paths, filtered_paths, select_paths); |
| 387 | |
| 388 | if (select_paths.size() == 0) { |
| 389 | ROS_WARN("No path."); |
| 390 | return false; |
| 391 | } |
| 392 | plan_data_.addTopoPaths(graph, raw_paths, filtered_paths, select_paths); |
| 393 | |
| 394 | /* Optimize trajectory using different topo guiding paths */ |
| 395 | ROS_INFO("[Optimize]: ---------"); |
| 396 | t1 = ros::Time::now(); |
| 397 | |
| 398 | plan_data_.topo_traj_pos1_.resize(select_paths.size()); |
| 399 | plan_data_.topo_traj_pos2_.resize(select_paths.size()); |
| 400 | vector<thread> optimize_threads; |
| 401 | for (int i = 0; i < select_paths.size(); ++i) { |
| 402 | optimize_threads.emplace_back(&FastPlannerManager::optimizeTopoBspline, this, t_now, |
no test coverage detected