FAST PLANNER代码阅读笔记

首先挂上github链接

https://github.com/HKUST-Aerial-Robotics/Fast-Plannerhttps://github.com/HKUST-Aerial-Robotics/Fast-Planner前端采用Kinodynamic_Astar进行搜索路径，后端采用--优化轨迹。

运行方法：

首先下载功能包，放到src下面，然后catkin_make可以直接进行编译，我用的是Ubuntu18.04。

然后分别在两个终端运行下面两句代码，就可以启动rviz，然后使用2D Nav Goal给它终点坐标即可规划。

source devel/setup.bash && roslaunch plan_manage rviz.launch
source devel/setup.bash && roslaunch plan_manage kino_replan.launch

遇到的报错：pcl-ros没有安装，安装即可

代码阅读

这个项目代码量非常大，我是按照功能包的实现顺序来阅读

1、plan_manage

这个功能包主要实现前端的kinoastar规划，由于代码量过程，因此下面按照阅读顺序解释：

fast_planner_node.cpp这个文件主要用来启动ROS节点，然后进行初始化

int main(int argc, char** argv) {
  ros::init(argc, argv, "fast_planner_node");
  ros::NodeHandle nh("~");  //创建一个节点句柄，命名为～
  int planner;
  nh.param("planner_node/planner", planner, -1);
  TopoReplanFSM topo_replan;
  KinoReplanFSM kino_replan;
  if (planner == 1) {//planner默认是1
    kino_replan.init(nh);
  } else if (planner == 2) {
    topo_replan.init(nh);
  }
  ros::Duration(1.0).sleep();
  ros::spin();
  return 0;
}

接下来看KinoReplanFSM类以及其init函数

void KinoReplanFSM::init(ros::NodeHandle& nh) {
  current_wp_  = 0;
  exec_state_  = FSM_EXEC_STATE::INIT;
  have_target_ = false;
  have_odom_   = false;

  /*  fsm param  */
  nh.param("fsm/flight_type", target_type_, -1);
  nh.param("fsm/thresh_replan", replan_thresh_, -1.0);
  nh.param("fsm/thresh_no_replan", no_replan_thresh_, -1.0);

  nh.param("fsm/waypoint_num", waypoint_num_, -1);
  for (int i = 0; i < waypoint_num_; i++) {
    nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
    nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
    nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
  }

  /* initialize main modules */
  planner_manager_.reset(new FastPlannerManager);
  planner_manager_->initPlanModules(nh);
  visualization_.reset(new PlanningVisualization(nh));

  /* callback */
  // 创建一个定时器，以某一Duration 秒 的时间间隔执行某一回调函数
  exec_timer_   = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);
  safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);

  waypoint_sub_ =
      nh.subscribe("/waypoint_generator/waypoints", 1, &KinoReplanFSM::waypointCallback, this);
  odom_sub_ = nh.subscribe("/odom_world", 1, &KinoReplanFSM::odometryCallback, this);

  replan_pub_  = nh.advertise("/planning/replan", 10);
  new_pub_     = nh.advertise("/planning/new", 10);
  bspline_pub_ = nh.advertise("/planning/bspline", 10);
}

init函数，以一定的时间间隔调用函数execFSMCallback和checkCollisionCallback。

execFSMCallback函数用来实现对于各种状态的管理

execFSMCallback根据不同状态使用switch语句，对每个状态进行管理，其中重点在GEN_NEW_TRAJ，EXEC_TRAJ和REPLAN_TRAJ

case GEN_NEW_TRAJ: {
      start_pt_  = odom_pos_;
      start_vel_ = odom_vel_;
      start_acc_.setZero();

      Eigen::Vector3d rot_x = odom_orient_.toRotationMatrix().block(0, 0, 3, 1);
      start_yaw_(0)         = atan2(rot_x(1), rot_x(0));
      start_yaw_(1) = start_yaw_(2) = 0.0;

      bool success = callKinodynamicReplan();
      if (success) {
        changeFSMExecState(EXEC_TRAJ, "FSM"); // 如果成功获取一条轨迹，就改状态为EXEC_TRAJ，去执行
      } else {
        // have_target_ = false;
        // changeFSMExecState(WAIT_TARGET, "FSM");
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");  // 如果没成功，就继续保持生成状态
      }
      break;

GEN_NEW_TRAJ中主要语句是

bool success = callKinodynamicReplan();

这句话调用了callKinodynamicReplan函数

bool KinoReplanFSM::callKinodynamicReplan() {
  bool plan_success =
      planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);

然后紧接着调用kinodynamicReplan函数，这个是重点的规划函数

函数输入起点，起始速度，起始加速度，终点位置，终点速度

kinodynamicReplan的主要语句是

  int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);

调用了这个search函数，用来进行kinodynamic_Astar规划

重点函数：KinodynamicAstar::search

int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a,
                             Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start)
{
  start_vel_ = start_v;
  start_acc_ = start_a;

  PathNodePtr cur_node = path_node_pool_[0];
  cur_node->parent = NULL;
  // 一个state是一个6×1的向量，前面三个存位置，后面三个存速度信息
  cur_node->state.head(3) = start_pt;
  cur_node->state.tail(3) = start_v;
  cur_node->index = posToIndex(start_pt);
  cur_node->g_score = 0.0;

  Eigen::VectorXd end_state(6);
  Eigen::Vector3i end_index;
  double time_to_goal;

  end_state.head(3) = end_pt;
  end_state.tail(3) = end_v;
  end_index = posToIndex(end_pt);
  cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);
  cur_node->node_state = IN_OPEN_SET;  // open_set_为优先级队列，比较的为 PathNodePtr 的fScore
  open_set_.push(cur_node);
  use_node_num_ += 1;

  // 考虑时间实时规划
  if (dynamic)
  {
    time_origin_ = time_start;
    cur_node->time = time_start;
    cur_node->time_idx = timeToIndex(time_start);   // 考虑时间因素的话，就选择带有时间信息的insert
    expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node);
    // cout << "time start: " << time_start << endl;
  }
  else //否则采用普通insert
    expanded_nodes_.insert(cur_node->index, cur_node);

  PathNodePtr neighbor = NULL;
  PathNodePtr terminate_node = NULL;
  bool init_search = init;
  const int tolerance = ceil(1 / resolution_); // ceil()函数：求不小于x的最小整数（向上取整）

前面是一些初始化，定义了起点和终点状态，将起点加入open_set

下面进入主循环

while (!open_set_.empty())
  {
    cur_node = open_set_.top();  // 取出cost最小的节点

    // Terminate? 判断扩展的节点是否距离终点过近
    bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;
    bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance &&
                    abs(cur_node->index(1) - end_index(1)) <= tolerance &&
                    abs(cur_node->index(2) - end_index(2)) <= tolerance;

    if (reach_horizon || near_end)
    {
      terminate_node = cur_node;
      retrievePath(terminate_node);
      if (near_end)
      {
        // Check whether shot traj exist
        estimateHeuristic(cur_node->state, end_state, time_to_goal);
        computeShotTraj(cur_node->state, end_state, time_to_goal);
        if (init_search)
          ROS_ERROR("Shot in first search loop!");
      }
    }
    if (reach_horizon)
    {
      if (is_shot_succ_)
      {
        std::cout << "reach end" << std::endl;
        return REACH_END;
      }
      else
      {
        std::cout << "reach horizon" << std::endl;
        return REACH_HORIZON;
      }
    }

    if (near_end)
    {
      if (is_shot_succ_)
      {
        std::cout << "reach end" << std::endl;
        return REACH_END;
      }
      else if (cur_node->parent != NULL)
      {
        std::cout << "near end" << std::endl;
        return NEAR_END;
      }
      else
      {
        std::cout << "no path" << std::endl;
        return NO_PATH;
      }
    }

定义了一些边界条件，如果距离终点或者地图边界过近就返回

// 弹出节点
    open_set_.pop();
    cur_node->node_state = IN_CLOSE_SET;
    iter_num_ += 1;

    double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0;
    Eigen::Matrix cur_state = cur_node->state;
    Eigen::Matrix pro_state;
    vector tmp_expand_nodes;
    Eigen::Vector3d um;
    double pro_t;
    vector inputs;
    vector durations;
    if (init_search)
    {
      inputs.push_back(start_acc_);
      for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
           tau += time_res_init * init_max_tau_)
        durations.push_back(tau);
      init_search = false;
    }
    else
    {
      // inputs里面放入所有可能的三维加速度
      for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
        for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
          for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res)
          {
            um << ax, ay, az;
            inputs.push_back(um);
          }
      // duration里面放所有可能的tau
      for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)
        durations.push_back(tau);
    }

接下来弹出cost最小的节点，然后对周围所有可能的输入加速度以及时间间隔进行遍历，获取数组inputs以及durations

for (int i = 0; i < inputs.size(); ++i)
      for (int j = 0; j < durations.size(); ++j)
      {
        um = inputs[i];
        double tau = durations[j];
        stateTransit(cur_state, pro_state, um, tau);// 记录状态转移：从当前状态转移到pro_state，这个函数得到pro_state
        pro_t = cur_node->time + tau;  // 时间间隔

        Eigen::Vector3d pro_pos = pro_state.head(3);  // 取 pro_state 前3维，作为位置

        // Check if in close set
        Eigen::Vector3i pro_id = posToIndex(pro_pos);  // 坐标转换成idx
        int pro_t_id = timeToIndex(pro_t);  // 时间也转换一下
        // 哈希表搜索这个节点，如果是动态的，搜索加上时间
        PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);
        // 如果没搜索到（==NULL），或者说当前节点在CLOSE LIST内了
        if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET)
        {
          if (init_search)  // 如果是
            std::cout << "close" << std::endl;
          continue;
        }

然后对所有可能的加速度和时间间隔，使用stateTransit函数计算可能转移到的状态

// 状态转移，从state0转移到state1
void KinodynamicAstar::stateTransit(Eigen::Matrix& state0, Eigen::Matrix& state1,
                                    Eigen::Vector3d um, double tau)
{
  for (int i = 0; i < 3; ++i)
    phi_(i, i + 3) = tau;

  Eigen::Matrix integral; // 创建一个向量存增量
  integral.head(3) = 0.5 * pow(tau, 2) * um; //位置为δx = 0.5 * a * t^2 
  integral.tail(3) = tau * um;  // 速度为 δv = a * t

  state1 = phi_ * state0 + integral;
}