fast planner代码解析--planner_manager.cpp

目录

1、void FastPlannerManager::initPlanModules()

2、bool FastPlannerManager::checkTrajCollision(double& distance) 

3、bool FastPlannerManager::kinodynamicReplan()

4、bool FastPlannerManager::planGlobalTraj(const Eigen::Vector3d& start_pos)

5、bool FastPlannerManager::topoReplan(bool collide)

6、void FastPlannerManager::selectBestTraj(NonUniformBspline& traj) 

7、void FastPlannerManager::refineTraj(NonUniformBspline& best_traj, double& time_inc)

8、void FastPlannerManager::updateTrajInfo() 

9、void FastPlannerManager::reparamBspline() 

10、void FastPlannerManager::optimizeTopoBspline()

11、void FastPlannerManager::findCollisionRange() 

12、void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw)

---

整个规划过程进行管理，【响应fsm层，调用算法层】

1、void FastPlannerManager::initPlanModules()

//初始化规划模块，读取算法的参数

2、bool FastPlannerManager::checkTrajCollision(double& distance) 

  //检查轨迹碰撞

3、bool FastPlannerManager::kinodynamicReplan()

核心的kinodynamic规划：

  3.1

  //首先从kino_path_finder_->search找到路径

  //当把起止点状态都设置好后，就利用kino_path_finder的search函数进行路径寻找

  //需要注意的是，search函数最开始会以start_acc作为起始点输入进行查找，

  //如果找不到，则再进行一轮离散化输入的寻找，若都找不到，则路径寻找失败。

  3.2

//成功寻找到一条路径后，利用NonUniformBspline::parameterizeToBspline()函数对所找到的路径进行均匀B样条函数的拟合，然后得到相应控制点。

  3.3

 得到控制点后，

//进行均匀B样条优化，但需要加以注意的是，在当前轨迹只是达到感知距离以外并未达到目标点时，目标函数需要加上ENDPOINT优化项，此时的优化变量应该包含最后pb控制点。但当前端寻找的路径的状态已经是REACH_END时，由于拟合最后pb个控制点已经能保证位置点约束，因此优化项中不再包含EDNPOINT，优化变量也不再包含最后pb个控制点。

  3.4

最后利用非均匀B样条类进行迭代时间调整，将调整后的B样条轨迹赋值给local_data.position_traj. 并利用updateTrajInfo函数对local_data的其他数据进行更新.

bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,
                                           Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,
                                           Eigen::Vector3d end_vel) {
                                            //核心函数–正常情况的规划

  std::cout << "[kino replan]: -----------------------" << std::endl;
  cout << "start: " << start_pt.transpose() << ", " << start_vel.transpose() << ", "
       << start_acc.transpose() << "\ngoal:" << end_pt.transpose() << ", " << end_vel.transpose()
       << endl;

  if ((start_pt - end_pt).norm() < 0.2) {
    cout << "Close goal" << endl;
    return false;
  }

  ros::Time t1, t2;

  local_data_.start_time_ = ros::Time::now();//起始时间
  double t_search = 0.0, t_opt = 0.0, t_adjust = 0.0;//搜索、优化、调整时间

  Eigen::Vector3d init_pos = start_pt;//初始位置
  Eigen::Vector3d init_vel = start_vel;//初始速度
  Eigen::Vector3d init_acc = start_acc;//初始加速度

  // kinodynamic path searching动态可行路径搜索

  t1 = ros::Time::now();

  kino_path_finder_->reset();

  int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);
  //首先从kino_path_finder_->search找到路径
  //当把起止点状态都设置好后，就利用kino_path_finder的search函数进行路径寻找
  //需要注意的是，search函数最开始会以start_acc作为起始点输入进行查找，
  //如果找不到，则再进行一轮离散化输入的寻找，若都找不到，则路径寻找失败。

  if (status == KinodynamicAstar::NO_PATH) {
    cout << "[kino replan]: kinodynamic search fail!" << endl;

    // retry searching with discontinuous initial state
    kino_path_finder_->reset();//如果找不到，则再进行一轮离散化输入的寻找
    status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, false);

    if (status == KinodynamicAstar::NO_PATH) {
      cout << "[kino replan]: Can't find path." << endl;//找不到路径
      return false;
    } else {
      cout << "[kino replan]: retry search success." << endl;//再次尝试查找路径后成功
    }

  } else {
    cout << "[kino replan]: kinodynamic search success." << endl;//成功找到路径
  }

  plan_data_.kino_path_ = kino_path_finder_->getKinoTraj(0.01);//获取轨迹

  t_search = (ros::Time::now() - t1).toSec();//记录搜索时间

  // parameterize the path to bspline

  double                  ts = pp_.ctrl_pt_dist / pp_.max_vel_;//控制点之间的距离除以速度，即控制点之间的时间
  vector<Eigen::Vector3d> point_set, start_end_derivatives;
  kino_path_finder_->getSamples(ts, point_set, start_end_derivatives);//采样

  Eigen::MatrixXd ctrl_pts;//控制点
  NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
  //成功寻找到一条路径后
  //利用NonUniformBspline::parameterizeToBspline()函数对所找到的路径进行均匀B样条函数的拟合，然后得到相应控制点。
  NonUniformBspline init(ctrl_pts, 3, ts);

  // bspline trajectory optimization

  t1 = ros::Time::now();

  int cost_function = BsplineOptimizer::NORMAL_PHASE;

  if (status != KinodynamicAstar::REACH_END) {
    cost_function |= BsplineOptimizer::ENDPOINT;
  }

  ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);

  t_opt = (ros::Time::now() - t1).toSec();//B样条优化时间
  //BsplineOptimizeTraj均匀B样条优化
  //进行均匀B样条优化，但需要加以注意的是，在当前轨迹只是达到感知距离以外并未达到目标点时
  //目标函数需要加上ENDPOINT优化项，此时的优化变量应该包含最后pb控制点。但当前端寻找的路径的状态已经是REACH_END时，
  //由于拟合最后pb个控制点已经能保证位置点约束，因此优化项中不再包含EDNPOINT，优化变量也不再包含最后pb个控制点

  // iterative time adjustment


  t1                    = ros::Time::now();
  NonUniformBspline pos = NonUniformBspline(ctrl_pts, 3, ts);
  double to = pos.getTimeSum();
  pos.setPhysicalLimits(pp_.max_vel_, pp_.max_acc_);
  bool feasible = pos.checkFeasibility(false);

  int iter_num = 0;
  while (!feasible && ros::ok()) {

    feasible = pos.reallocateTime();
    //非均匀B样条迭代时间优化reallocateTime。

    if (++iter_num >= 3) break;
  }

  // pos.checkFeasibility(true);
  // cout << "[Main]: iter num: " << iter_num << endl;

  double tn = pos.getTimeSum();

  cout << "[kino replan]: Reallocate ratio: " << tn / to << endl;
  if (tn / to > 3.0) ROS_ERROR("reallocate error.");

  t_adjust = (ros::Time::now() - t1).toSec();
  最后利用非均匀B样条类进行迭代时间调整，将调整后的B样条轨迹赋值给local_data.position_traj.
  // 并利用updateTrajInfo函数对local_data的其他数据进行更新

  // save planned results

  local_data_.position_traj_ = pos;

  double t_total = t_search + t_opt + t_adjust;//总时间
  cout << "[kino replan]: time: " << t_total << ", search: " << t_search << ", optimize: " << t_opt
       << ", adjust time:" << t_adjust << endl;

  pp_.time_search_   = t_search;
  pp_.time_optimize_ = t_opt;
  pp_.time_adjust_   = t_adjust;

  updateTrajInfo();//更新轨迹信息

  return true;
}

4、bool FastPlannerManager::planGlobalTraj(const Eigen::Vector3d& start_pos)

//生成全局参考轨迹

5、bool FastPlannerManager::topoReplan(bool collide)

//再有障碍物的情况下，解析topo规划

6、void FastPlannerManager::selectBestTraj(NonUniformBspline& traj) 

  //选择最优轨迹

7、void FastPlannerManager::refineTraj(NonUniformBspline& best_traj, double& time_inc)

//优化轨迹

8、void FastPlannerManager::updateTrajInfo() 

  //更新轨迹信息，为当前轨迹的编号、速度、加速度、起始位置、持续时间

9、void FastPlannerManager::reparamBspline() 

//B样条参量化

10、void FastPlannerManager::optimizeTopoBspline()

//优化topoB样条

11、void FastPlannerManager::findCollisionRange() 

//搜索碰撞范围

12、void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw)

把现在规划出来的轨迹进行线性分段，分段的方法是根据轨迹的总运行时间/人为设定的时间增量，进而得到单位时间增量下的，最小航向角增量dt_yaw

通过人为设定的时间增量不断迭代，取出对应时刻轨迹上的控制点，进而得到轨迹上两两相邻的控制点，通过两两控制点的相对位置即可计算得到该条轨迹上每个控制点的航向角yaw，这个航向角yaw也是该轨迹在该点的切线方向。

void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw) {
  //规划偏航角
  //把现在规划出来的轨迹进行线性分段
  //分段的方法是根据轨迹的总运行时间/人为设定的时间增量，进而得到单位时间增量下的，最小航向角增量dt_yaw
  ROS_INFO("plan yaw");
  auto t1 = ros::Time::now();
  // calculate waypoints of heading

  auto&  pos      = local_data_.position_traj_;//轨迹的位置
  double duration = pos.getTimeSum();//轨迹的总持续时间

  double dt_yaw  = 0.3;//偏航角的时间增量
  int    seg_num = ceil(duration / dt_yaw);//轨迹分段数目
  dt_yaw         = duration / seg_num;//最小航向角增量

  const double            forward_t = 2.0;
  double                  last_yaw  = start_yaw(0);//最后的偏航角
  vector<Eigen::Vector3d> waypts;//航迹点
  vector<int>             waypt_idx;//航迹点索引

  // seg_num -> seg_num - 1 points for constraint excluding the boundary states
  //计算路径点waypoints的航向角yaw
  for (int i = 0; i < seg_num; ++i) {//遍历所有的轨迹分段
    double          tc = i * dt_yaw;//迭代计算第i个轨迹的运行时刻
    Eigen::Vector3d pc = pos.evaluateDeBoorT(tc);//根据轨迹运行时刻，获得B样条的第i时刻的控制点，即当前控制点
    double          tf = min(duration, tc + forward_t); //迭代计算轨迹下一段的运行时刻
    Eigen::Vector3d pf = pos.evaluateDeBoorT(tf);//根据轨迹运行时刻，获得B样条的下一段的控制点，注意这是下一段控制点
    Eigen::Vector3d pd = pf - pc;//计算当前控制点与下一段控制点的有向向量

    Eigen::Vector3d waypt;//航迹点
    if (pd.norm() > 1e-6) {//当前控制点与下一段控制点的有向向量达到阈值，就计算yaw
      waypt(0) = atan2(pd(1), pd(0));计算量控制角的夹角，即航向yaw
      waypt(1) = waypt(2) = 0.0;
      calcNextYaw(last_yaw, waypt(0));//计算下一个路径点的航向角yaw
    } else {
      waypt = waypts.back();
    }
    waypts.push_back(waypt);
    waypt_idx.push_back(i);
  }

  // calculate initial control points with boundary state constraints
  //使用边界状态约束计算初始控制点
  Eigen::MatrixXd yaw(seg_num + 3, 1);
  yaw.setZero();

  Eigen::Matrix3d states2pts;
  states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw, 1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw, 1.0,
      dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
  yaw.block(0, 0, 3, 1) = states2pts * start_yaw;

  Eigen::Vector3d end_v = local_data_.velocity_traj_.evaluateDeBoorT(duration - 0.1);
  Eigen::Vector3d end_yaw(atan2(end_v(1), end_v(0)), 0, 0);
  calcNextYaw(last_yaw, end_yaw(0));
  yaw.block(seg_num, 0, 3, 1) = states2pts * end_yaw;

  // 优化器 solve
  bspline_optimizers_[1]->setWaypoints(waypts, waypt_idx);
  int cost_func = BsplineOptimizer::SMOOTHNESS | BsplineOptimizer::WAYPOINTS;
  yaw           = bspline_optimizers_[1]->BsplineOptimizeTraj(yaw, dt_yaw, cost_func, 1, 1);

  // update traj info更新轨迹信息
  local_data_.yaw_traj_.setUniformBspline(yaw, 3, dt_yaw);
  local_data_.yawdot_traj_    = local_data_.yaw_traj_.getDerivative();
  local_data_.yawdotdot_traj_ = local_data_.yawdot_traj_.getDerivative();

  vector<double> path_yaw;
  for (int i = 0; i < waypts.size(); ++i) path_yaw.push_back(waypts[i][0]);
  plan_data_.path_yaw_    = path_yaw;
  plan_data_.dt_yaw_      = dt_yaw;
  plan_data_.dt_yaw_path_ = dt_yaw;
  //通过人为设定的时间增量不断迭代，取出对应时刻轨迹上的控制点，进而得到轨迹上两两相邻的控制点
  //通过两两控制点的相对位置即可计算得到该条轨迹上每个控制点的航向角yaw，这个航向角yaw也是该轨迹在该点的切线方向
  std::cout << "plan heading: " << (ros::Time::now() - t1).toSec() << std::endl;
}

void FastPlannerManager::calcNextYaw(const double& last_yaw, double& yaw) {
  //计算下一个偏航角
  // round yaw to [-PI, PI]

  double round_last = last_yaw;

  while (round_last < -M_PI) {
    round_last += 2 * M_PI;
  }
  while (round_last > M_PI) {
    round_last -= 2 * M_PI;
  }

  double diff = yaw - round_last;

  if (fabs(diff) <= M_PI) {
    yaw = last_yaw + diff;
  } else if (diff > M_PI) {
    yaw = last_yaw + diff - 2 * M_PI;
  } else if (diff < -M_PI) {
    yaw = last_yaw + diff + 2 * M_PI;
  }
}