【项目解读】fast_planner工程解读

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对fast_planner工程解读做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、规划系统运行逻辑【业务部分】

1.Fast_planner_node.cpp【程序入口】

功能

注册ROS节点，实例化kinoReplanFSM类对象并进行初始化，ROS开始循环

代码实现

//注册ROS节点，实例化kinoReplanFSM类对象并进行初始化，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;  //这里进去，实例化kinoReplanFSM类对象并进行初始化

  if (planner == 1) {
    kino_replan.init(nh);
  } else if (planner == 2) {
    topo_replan.init(nh);
  }

  ros::Duration(1.0).sleep();
  ros::spin();

  return 0;
}

2.kino_replan_fsm.cpp【状态机转移】

状态机设计的有关方法介绍可以转战一下我的业务决策专栏的FSM文章~

功能

订阅无人机位置和目标点信息， 并通过定时回调函数execFsmCallback判断当前状态，利用exec_state判断是否等待目标点，是否进行重规划，是否执行轨迹。利用定时回调函数【防盗标记–盒子君hzj】checkCollisionCallback判断是否需要重新寻找目标点，是否需要进行重规划。规划的接口都是CallKinodynamicReplan函数，其中利用的是planner_manager的kinodynamicReplan函数

代码实现

KinoReplanFSM::init()函数

/*
这一函数首先是设置了一系列参数，然后对规划管理类的智能指针对象planner_manager_进行了初始化.
接着，定义了一系列的ROS组件，包括定时器，订阅器，发布器。规划的具体步骤就在这些组件的调用及回调函数里实现。
*/
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 */
  //定时器
  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<std_msgs::Empty>("/planning/replan", 10);
  new_pub_     = nh.advertise<std_msgs::Empty>("/planning/new", 10);
  bspline_pub_ = nh.advertise<plan_manage::Bspline>("/planning/bspline", 10);

订阅目标点回调函数

void KinoReplanFSM::waypointCallback(const nav_msgs::PathConstPtr& msg) {
  if (msg->poses[0].pose.position.z < -0.1) return;

  cout << "Triggered!" << endl;
  trigger_ = true;

  if (target_type_ == TARGET_TYPE::MANUAL_TARGET) {
    end_pt_ << msg->poses[0].pose.position.x, msg->poses[0].pose.position.y, 1.0;

  } else if (target_type_ == TARGET_TYPE::PRESET_TARGET) {
    end_pt_(0)  = waypoints_[current_wp_][0];
    end_pt_(1)  = waypoints_[current_wp_][1];
    end_pt_(2)  = waypoints_[current_wp_][2];
    current_wp_ = (current_wp_ + 1) % waypoint_num_;
  }

  visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
  end_vel_.setZero();
  have_target_ = true;

  if (exec_state_ == WAIT_TARGET)
    changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
  else if (exec_state_ == EXEC_TRAJ)
    changeFSMExecState(REPLAN_TRAJ, "TRIG");
}

订阅里程计回调函数

//订阅里程计回调函数
void KinoReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr& msg) {
  odom_pos_(0) = msg->pose.pose.position.x;
  odom_pos_(1) = msg->pose.pose.position.y;
  odom_pos_(2) = msg->pose.pose.position.z;

  odom_vel_(0) = msg->twist.twist.linear.x;
  odom_vel_(1) = msg->twist.twist.linear.y;
  odom_vel_(2) = msg->twist.twist.linear.z;

  odom_orient_.w() = msg->pose.pose.orientation.w;
  odom_orient_.x() = msg->pose.pose.orientation.x;
  odom_orient_.y() = msg->pose.pose.orientation.y;
  odom_orient_.z() = msg->pose.pose.orientation.z;

  have_odom_ = true;
}

【重要】执行状态机FSM定时器回调函数

这个函数的触发时间是每0.01秒。首先，每1秒打印一次当前执行状态。然后，根据执行状态变量 exec_state_进入switch循环。

如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。 have_odom 和 trigger 这两个bool变量分别在KinoReplanFSM::odometryCallback()和KinoReplanFSM::waypointCallback() 这两个消息订阅的回调函数中被改变。如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。

如果状态是WAIT_TARGET（等待目标点）, 则判断是否有target，如果有，则改变状态为GEN_NEW_TRAJ，并跳出当前循环。【防盗标记–盒子君hzj】

【重点】如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用callKinodynamicReplan()函数进行规划。成功则改变执行状态为EXEC_TRAJ,失败则改变执行状态为GEN_NEW_TRAJ。这里我们来看callKinodynamicReplan()函数。如果规划成功的话，就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE【防盗标记–盒子君hzj】

如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。如果当前时间距离起始时间已经超过当前执行轨迹的时长，则将have_target置于false，将执行状态变为WAIT_TARGET。如果当前距离与终点距离小于不需要重规划的阈值，或者当前距离与起点距离小于horizon，那么直接返回。反之，则进入重规划阶段。

【重点】如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，进行callKinodynamicReplan重规划，如果成功，则将执行状态更改为EXEC_TRAJ,否则将执行状态更改为GEN_NEW_TRAJ.【防盗标记–盒子君hzj】

//【重要】执行状态机FSM定时器回调函数
void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e) {
  static int fsm_num = 0;
  fsm_num++;
  //这个函数的触发时间是每0.01秒。首先，每1秒打印一次当前执行状态。然后，根据执行状态变量 exec_state_进入switch循环。
  if (fsm_num == 100) {
    printFSMExecState();
    if (!have_odom_) cout << "no odom." << endl;
    if (!trigger_) cout << "wait for goal." << endl;
    fsm_num = 0;
  }

  switch (exec_state_) {
  /*
  如果状态是INIT（初始化），则判断有没有收到odometry 和 plan的 ttrigger是否被触发。
  have_odom 和 trigger 这两个bool变量分别在
  KinoReplanFSM::odometryCallback()和KinoReplanFSM::waypointCallback() 这两个消息订阅的回调函数中被改变。
  如果都通过，改变当前状态为WAIT_TARGET,并跳出当前循环。
  */
    case INIT: {
      if (!have_odom_) {
        return;
      }
      if (!trigger_) {
        return;
      }
      changeFSMExecState(WAIT_TARGET, "FSM");
      break;
    }

  /*
  如果状态是WAIT_TARGET（等待目标点）, 则判断是否有target，
  如果有，则改变状态为GEN_NEW_TRAJ，并跳出当前循环。
  */
    case WAIT_TARGET: {
      if (!have_target_)
        return;
      else {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

  /*
  如果状态是GEN_NEW_TRAJ(生成轨迹)，则调用callKinodynamicReplan()函数进行规划。
  成功则改变执行状态为EXEC_TRAJ,失败则改变执行状态为GEN_NEW_TRAJ。
  这里我们来看callKinodynamicReplan()函数。如果规划成功的话，就通过Bspline_pub把相应的B样条轨迹发布出去，否则返回FALSE
  */
    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");
      } else {
        // have_target_ = false;
        // changeFSMExecState(WAIT_TARGET, "FSM");
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }

  /*
  如果状态为EXEC_TRAJ(执行轨迹)，则判断是否需要进行重规划。
  如果当前时间距离起始时间已经超过当前执行轨迹的时长，
  则将have_target置于false，将执行状态变为WAIT_TARGET。
  如果当前距离与终点距离小于不需要重规划的阈值，或者当前距离与起点距离小于horizon，那么直接返回。
  反之，则进入重规划阶段。
  */
    case EXEC_TRAJ: {
      /* determine if need to replan */
      LocalTrajData* info     = &planner_manager_->local_data_;
      ros::Time      time_now = ros::Time::now();
      double         t_cur    = (time_now - info->start_time_).toSec();
      t_cur                   = min(info->duration_, t_cur);

      Eigen::Vector3d pos = info->position_traj_.evaluateDeBoorT(t_cur);

      /* && (end_pt_ - pos).norm() < 0.5 */
      if (t_cur > info->duration_ - 1e-2) {
        have_target_ = false;
        changeFSMExecState(WAIT_TARGET, "FSM");
        return;

      } else if ((end_pt_ - pos).norm() < no_replan_thresh_) {
        // cout << "near end" << endl;
        return;

      } else if ((info->start_pos_ - pos).norm() < replan_thresh_) {
        // cout << "near start" << endl;
        return;

      } else {
        changeFSMExecState(REPLAN_TRAJ, "FSM");
      }
      break;
    }

  /*
  如果状态为REPLAN_TRAJ(重新规划)，则利用当前的位置，速度，状态，进行callKinodynamicReplan重规划，
  如果成功，则将执行状态更改为EXEC_TRAJ,否则将执行状态更改为GEN_NEW_TRAJ.
  */
    case REPLAN_TRAJ: {
      LocalTrajData* info     = &planner_manager_->local_data_;
      ros::Time      time_now = ros::Time::now();
      double         t_cur    = (time_now - info->start_time_).toSec();

      start_pt_  = info->position_traj_.evaluateDeBoorT(t_cur);
      start_vel_ = info->velocity_traj_.evaluateDeBoorT(t_cur);
      start_acc_ = info->acceleration_traj_.evaluateDeBoorT(t_cur);

      start_yaw_(0) = info->yaw_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(1) = info->yawdot_traj_.evaluateDeBoorT(t_cur)[0];
      start_yaw_(2) = info->yawdotdot_traj_.evaluateDeBoorT(t_cur)[0];

      std_msgs::Empty replan_msg;
      replan_pub_.publish(replan_msg);

      bool success = callKinodynamicReplan();
      if (success) {
        changeFSMExecState(EXEC_TRAJ, "FSM");
      } else {
        changeFSMExecState(GEN_NEW_TRAJ, "FSM");
      }
      break;
    }
  }
}

.
.

【重要】路径规划并检查规划是否成功（planner_manager的入口）

//检查路径规划是否成功
bool KinoReplanFSM::callKinodynamicReplan() {
  bool plan_success =
      planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);

  if (plan_success) {

    planner_manager_->planYaw(start_yaw_);

    auto info = &planner_manager_->local_data_;

    /* publish traj */
    plan_manage::Bspline bspline;
    bspline.order      = 3;
    bspline.start_time = info->start_time_;
    bspline.traj_id    = info->traj_id_;

    Eigen::MatrixXd pos_pts = info->position_traj_.getControlPoint();

    for (int i = 0; i < pos_pts.rows(); ++i) {
      geometry_msgs::Point pt;
      pt.x = pos_pts(i, 0);
      pt.y = pos_pts(i, 1);
      pt.z = pos_pts(i, 2);
      bspline.pos_pts.push_back(pt);
    }

    Eigen::VectorXd knots = info->position_traj_.getKnot();
    for (int i = 0; i < knots.rows(); ++i) {
      bspline.knots.push_back(knots(i));
    }

    Eigen::MatrixXd yaw_pts = info->yaw_traj_.getControlPoint();
    for (int i = 0; i < yaw_pts.rows(); ++i) {
      double yaw = yaw_pts(i, 0);
      bspline.yaw_pts.push_back(yaw);
    }
    bspline.yaw_dt = info->yaw_traj_.getInterval();

    bspline_pub_.publish(bspline);

    /* visulization */
    auto plan_data = &planner_manager_->plan_data_;
    visualization_->drawGeometricPath(plan_data->kino_path_, 0.075, Eigen::Vector4d(1, 1, 0, 0.4));
    visualization_->drawBspline(info->position_traj_, 0.1, Eigen::Vector4d(1.0, 0, 0.0, 1), true, 0.2,
                                Eigen::Vector4d(1, 0, 0, 1));

    return true;

  } else {
    cout << "generate new traj fail." << endl;
    return false;
  }
}

【重要】判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的回调函数

这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。如果找到了，就直接改变状态进入REPLAN_TRAJ。【防盗标记–盒子君hzj】如果目标点周围没有障碍物且目前状态是EXEX_TRAJ,则利用planner_manager的checkTrajCollision函数进行轨迹检查，如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划【防盗标记–盒子君hzj】

/*
这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。
如果目标点有障碍物，就在目标点周围通过离散的半径及角度循环来寻找新的安全的目标点。
如果找到了，就直接改变状态进入REPLAN_TRAJ。
如果目标点周围没有障碍物且目前状态是EXEX_TRAJ,则利用planner_manager的checkTrajCollision函数进行轨迹检查，
如果轨迹不发生碰撞，则无事发生，如果轨迹碰撞，则状态变为REPLAN_TRAJ,进行重规划
*/
void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e) {
  LocalTrajData* info = &planner_manager_->local_data_;

  if (have_target_) {
    auto edt_env = planner_manager_->edt_environment_;

    double dist = planner_manager_->pp_.dynamic_ ?
        edt_env->evaluateCoarseEDT(end_pt_, /* time to program start + */ info->duration_) :
        edt_env->evaluateCoarseEDT(end_pt_, -1.0);

    if (dist <= 0.3) {
      /* try to find a max distance goal around */
      bool            new_goal = false;
      const double    dr = 0.5, dtheta = 30, dz = 0.3;
      double          new_x, new_y, new_z, max_dist = -1.0;
      Eigen::Vector3d goal;

      for (double r = dr; r <= 5 * dr + 1e-3; r += dr) {
        for (double theta = -90; theta <= 270; theta += dtheta) {
          for (double nz = 1 * dz; nz >= -1 * dz; nz -= dz) {

            new_x = end_pt_(0) + r * cos(theta / 57.3);
            new_y = end_pt_(1) + r * sin(theta / 57.3);
            new_z = end_pt_(2) + nz;

            Eigen::Vector3d new_pt(new_x, new_y, new_z);
            dist = planner_manager_->pp_.dynamic_ ?
                edt_env->evaluateCoarseEDT(new_pt, /* time to program start+ */ info->duration_) :
                edt_env->evaluateCoarseEDT(new_pt, -1.0);

            if (dist > max_dist) {
              /* reset end_pt_ */
              goal(0)  = new_x;
              goal(1)  = new_y;
              goal(2)  = new_z;
              max_dist = dist;
            }
          }
        }
      }

      if (max_dist > 0.3) {
        cout << "change goal, replan." << endl;
        end_pt_      = goal;
        have_target_ = true;
        end_vel_.setZero();

        if (exec_state_ == EXEC_TRAJ) {
          changeFSMExecState(REPLAN_TRAJ, "SAFETY");
        }

        visualization_->drawGoal(end_pt_, 0.3, Eigen::Vector4d(1, 0, 0, 1.0));
      } else {
        // have_target_ = false;
        // cout << "Goal near collision, stop." << endl;
        // changeFSMExecState(WAIT_TARGET, "SAFETY");
        cout << "goal near collision, keep retry" << endl;
        changeFSMExecState(REPLAN_TRAJ, "FSM");

        std_msgs::Empty emt;
        replan_pub_.publish(emt);
      }
    }
  }

  /* ---------- check trajectory ---------- */
  if (exec_state_ == FSM_EXEC_STATE::EXEC_TRAJ) {
    double dist;
    bool   safe = planner_manager_->checkTrajCollision(dist);

    if (!safe) {
      // cout << "current traj in collision." << endl;
      ROS_WARN("current traj in collision.");
      changeFSMExecState(REPLAN_TRAJ, "SAFETY");
    }
  }
}

.
.

3.planner_manager.cpp【算法拼接部分】

功能

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

FastPlannerManager首先是包含了一系列plan的接口函数，然后包含了一系列规划算法包中的数据结构和类。同时包含了其他模块的支持API服务响应

【核心函数–正常情况的规划】FastPlannerManager::kinodynamicReplan()

kinodynamicReplan函数中，
首先从kino_path_finder_->search找到路径，
再到NonUniformBspline::parameterizeToBspline对B样条进行拟合，
再到BsplineOptimizeTraj均匀B样条优化，【防盗标记–盒子君hzj】
再到非均匀B样条迭代时间优化reallocateTime。
分别对应了Path-finding包，Bspline包，Bspline-Optimization包。

详细步骤如下：
第一步：当把起止点状态都设置好后，就利用kino_path_finder的search函数进行路径寻找。需要注意的是，search函数最开始会以start_acc作为起始点输入进行查找，如果找不到，则再进行一轮离散化输入的寻找，若都找不到，则路径寻找失败。【防盗标记–盒子君hzj】

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

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

第四步：最后利用非均匀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
  //当把起止点状态都设置好后，就利用kino_path_finder的search函数进行路径寻找

  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);

  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
  //B样条函数的拟合

  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 init(ctrl_pts, 3, ts);

  // bspline trajectory optimization
  //进行均匀B样条优化

  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();

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

  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();

    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();

  // 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;
}

.
.

【核心函数–航向角yaw规划】FastPlannerManager::planYaw()

为什么要进行航向角yaw规划

在路径规划中，对路径位置进行规划可以理解，对轨迹的速度和加速度进行规划也可以理解
为什么还要对轨迹的方向也要进行规划呢？
当路径的位置一定确定下来了，路径的航向角yaw不是已经确定下来了吗？那为什么还要对路径的航向角在规划呢？【防盗标记–盒子君hzj】

我的想法如下：
当路径的位置一定确定下来了，路径的航向角yaw已经确定下来了，但是我们还是要把航向角yaw计算出来，作为一个属性添加到轨迹之中，因为航向角yaw有时候是作为控制器的输入的，怎么理解呢，就是像纯跟踪pure_suit这种控制器输入一系列离散的路径点就可以进行路径跟随，但是PID控制输入一系列离散的路径点显然是不行的，要先通过航向角yaw规划得到每个路径点的航向位置yaw作为PID控制器输入，才能实现PID的航向角位置闭环控制【防盗标记–盒子君hzj】

同理，若要实现机器人的（航向）角速度闭环控制，或者实现机器人的（航向）角加速度闭环控制，还要进一步进行航向角速度w的规划和航向角加速度a的规划

同理，计算航向角速度w与航向角加速度a，可以进行航向角yaw对时间就一阶、二阶导数实现，若有进行轨迹的速度、加速度规划（如mini_snap），则可以取出轨迹的速度曲线、加速度曲线，进行分段就对应角度（与求航向角yaw方法类似）

我在写什么呢？？？表述的有点冗余，哎，理科生就这样，大家凑合着理解吧~【防盗标记–盒子君hzj】

核心思想

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

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

//航向规划
void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw) {
  ROS_INFO("plan yaw");
  auto t1 = ros::Time::now();

  //（1）对带段轨迹进行分段
  auto&  pos      = local_data_.position_traj_; //获取该轨迹的位置
  double duration = pos.getTimeSum();           //获取该轨迹的总运行时间
  double dt_yaw  = 0.3;                         //设置航向角yaw的时间增量--300ms
  int    seg_num = ceil(duration / dt_yaw);     //计算轨迹分段数，轨迹分段数 = 该轨迹的总运行时间/时间增量
  dt_yaw         = duration / seg_num;          //设置航向角yaw的角度增量，角度增量 = 该轨迹的总运行时间/轨迹分段数

  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点用于约束，不包括边界状态
  //（2）计算路径点waypoints的航向角yaw
  for (int i = 0; i < seg_num; ++i) {   //遍历所有的轨迹分段
    double          tc = i * dt_yaw;             //迭代计算轨迹现在的运行时刻
    Eigen::Vector3d pc = pos.evaluateDeBoorT(tc);//根据轨迹运行时刻，获得B样条的当前的控制点，注意这是当前控制点
    
    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);
  }

  // （3）使用边界状态约束计算初始控制点
  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;

  std::cout << "plan heading: " << (ros::Time::now() - t1).toSec() << std::endl;
}



//计算下一个路径点的航向角yaw
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;
  }
}

其他函数接口

这个相对没有这么重要，细节我到时候在更~

毕竟其他函数接口还挺多的

.
.
.

二、规划系统算法部分

1.前端kinodynamic A*算法动力学路径搜索

这部分时继承A类的图搜索算法来的，单独写了一个kinodynamic A的博客，kinodynamic A*的理论和原理实现都已经详细描述，请移步到我planning的专栏【防盗标记–盒子君hzj】
https://blog.csdn.net/qq_35635374/article/details/121541745
.
.

2.后端轨迹优化

（1）Bspline曲线拟合

这部分单独写了一个Bspline曲线的博客，Bspline曲线的理论和原理实现都已经详细描述，请移步到我planning的专栏【防盗标记–盒子君hzj】
https://blog.csdn.net/qq_35635374/article/details/120730023

.
.

（2）bspline_opt非线性优化器

这部分单独写了一个Bspline曲线的博客，Bspline曲线的理论和原理实现都已经详细描述，请移步到我planning的专栏【防盗标记–盒子君hzj】
https://blog.csdn.net/qq_35635374/article/details/121542692
.
.

3.安全检查线程（safetyCallback）

功能

按照固定频率对输入目标点、生成的轨迹进行安全性检查，保证系统随着地图更新也能一直安全。

原理

FastPlannerManager::checkTrajCollision（）函数则是根据Local_data中属于NonUniformBspline类的position_traj的eveluateDeBoor()函数计算局部轨迹上的每个离散点的值，并根据SDF类的【防盗标记–盒子君hzj】evaluateCorseEDT（）函数计算距离障碍物的距离。如果轨迹中的任一点距离障碍物的距离小于阈值，则返回当前局部轨迹从起始点到检查到碰撞点的距离（即最远可执行的安全距离）
.
.

4.轨迹处理控制指令traj_serve

功能

把得到的bspline轨迹进行处理，得到cmd (prometheus_msgs::PositionReference) 指令，发送给飞控使用【飞控几乎是单独的】

订阅话题
“/prometheus/planning/bspline” : 由规划器生成的样条轨迹
“/prometheus/fast_planning/replan”: 重规划标志
“/prometheus/drone_odom”: 里程计数据

发布话题
“/prometheus/planning/fastplanner/desired_trajecotry”: 给无人机控制指令
“/prometheus/planning/traj”: 显示轨迹
“/prometheus/planning/state”: 显示运动

三、Gazebo仿真环境运行步骤

1.使用激光雷达作为传感器

运行launch文件（请查看launch文件参数说明，并进行调整） roslaunch prometheus_gazebo sitl_fast_planning_3dlidar.launch
在打开的rviz窗口中勾选Fast_Planner、Octomap_Mapping及Ground_Truth显示
输入3选择Fast Planner算法，无人机将自动起飞
在rviz中通过3D Nav Goal按钮指定目标点，点选该按钮后，同时按住鼠标左右键在rviz窗口中选择一点向上拉3D Nav Goal
红色线代表规划的路径,黄色线代表优化后的轨迹
也可以通过终端发布目标点【防盗标记–盒子君hzj】
rostopic pub /prometheus/planning/goal …
通过终端查看算法相关信息

.
.

2.使用RGBD相机作为传感器

运行launch文件（请查看launch文件参数说明，并进行调整） roslaunch prometheus_gazebo sitl_fast_planning_rgbd.launch
在打开的rviz窗口中勾选Fast_Planner,RTAB建图及Ground_Truth显示
其他同上
【防盗标记–盒子君hzj】
.
.

---

总结

fast_planner的运行逻辑梳理

1、程序入口在Fast_planner_node.cpp
主要是完成注册ROS节点，实例化kinoReplanFSM类对象并进行初始化，ROS开始循环

2、接下来就是【业务拼接部分】了，主要是根据场景使用状态机FSM进行系统各个状态任务管理

3、然后是【算法拼接部分】了，主要是统筹每个算法功能包来响应系统当前状态任务，主要是路径规划任务

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

再到NonUniformBspline::parameterizeToBspline对B样条进行拟合，

再到BsplineOptimizeTraj均匀B样条优化，

再到非均匀B样条迭代时间优化reallocateTime。

4、最后是【算法的实现】，每个算法都封装成为了一个包，负责某一个具体效果的实现

分别对应了Path-finding包，Bspline包，Bspline-Optimization包。

参考资料

基于飞行走廊的后端轨迹优化
https://blog.csdn.net/Travis_X/article/details/115709442