本系列文章用于学习记录Fast-Planner规划算法的相关内容，主要学习资料是深蓝学院的移动机器人运动规划课程
五、Fast Planner程序框架
Fast Planner算法相关的程序存放在fast_planner文件夹内
fast_planner文件夹下包含八个文件夹，各文件夹的作用如下：
（1）、bag：脚本文件
（2）、bspline：非均匀B样条，用于后端时间重分配部分
（3）、bspline_opt ：均匀B样条，用于后端轨迹优化部分
（4）、path_searching：包含A*算法、Hybrid A * 算法、基于拓扑的PRM算法，三个规划算法，用于前端的路径规划
（5）、plan_env：用于地图生成，包括栅格地图生成、ESDF生成等
（6）、plan_manage：主文件夹，主节点所在文件夹，
（7）、poly_traj：定义了一个多项式的轨迹类
（8）、traj_utils：可视化相关内容
要理解Fast Planner的程序框架，就应该从主节点所在的plan_manage文件夹开始：
（1）fast_planner_node.cpp
fast_planner_node.cpp文件是我们通过launch文件启动Fast Planner程序的接口，也是ROS中Fast Planner的主节点，该文件在完成节点初始化后，会根据参数planner的值选择进行kino_replan或者topo_replan的初始化。
（2）kino_replan_fsm.cpp
若参数planner的值为1，则执行kino_replan的初始化，kino_replan的初始化的具体程序在kino_replan_fsm.cpp文件中，这个文件也是ROS跟 Fast Planner程序的接口性程序，包括参数加载、回调、订阅等。
kino_replan_fsm.cpp文件中的execFSMCallback函数是状态基，用于处理在机器人运行过程中遇到各种情况下，是选择急停、还是重新规划、还是继续执行等。
void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e)
kino_replan_fsm.cpp文件中的checkCollisionCallback是用于检查碰撞的函数
void KinoReplanFSM::checkCollisionCallback(const ros::TimerEvent& e)
kino_replan_fsm.cpp文件中的callKinodynamicReplan函数中，会调用kinodynamicReplan函数，本系列前三篇文章介绍的内容全包含在和这个函数中，即局部的包含前端和后端的规划方法。该函数在planner_manager.cpp文件中。
bool KinoReplanFSM::callKinodynamicReplan()
bool plan_success =planner_manager_->kinodynamicReplan(start_pt_, start_vel_, start_acc_, end_pt_, end_vel_);
（3）planner_manager.cpp
kino_replan_fsm.cpp文件通过调用kinodynamicReplan函数来调用该文件。
bool FastPlannerManager::kinodynamicReplan(Eigen::Vector3d start_pt, Eigen::Vector3d start_vel,Eigen::Vector3d start_acc, Eigen::Vector3d end_pt,Eigen::Vector3d end_vel)
kinodynamicReplan函数中完成一些初始化操作后，首先后使用Hybrid A * 算法进行前端的路径搜索，该部分内容通过调用kinodynamic_astar.cpp中的search函数完成。
int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);
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)
接下来会将前端得到的路径转换成B样条曲线的形式，作为后端优化的初值，该部分内容在parameterizeToBspline，这个函数的具体程序在non_uniform_bspline.cpp文件中
NonUniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);
void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,const vector<Eigen::Vector3d>& start_end_derivative,Eigen::MatrixXd& ctrl_pts)
接下来就是后端的使用均匀B样条及优化算法进行轨迹优化了，该部分内容是通过调用BsplineOptimizeTraj函数来完成的，该函数的具体程序在bspline_optimizer.cpp文件中
ctrl_pts = bspline_optimizers_[0]->BsplineOptimizeTraj(ctrl_pts, ts, cost_function, 1, 1);
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,const int& cost_function, int max_num_id,int max_time_id)
最后，就是时间的重分配了，通过调用非均匀的B样条来进行时间的重分配，该部分内容在类NonUniformBspline中完成，该类的具体程序在non_uniform_bspline.cpp文件中
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;}
NonUniformBspline::NonUniformBspline(const Eigen::MatrixXd& points, const int& order,const double& interval) {setUniformBspline(points, order, interval);
}
上面就是 Fast Planner程序的大概流程，跟前三篇文章 介绍的流程是相同的。
我们现在来看一下上面流程中，比较重要的使用均匀B样条曲线及优化算法进行后端优化的位于bspline_optimizer.cpp文件中的BsplineOptimizeTraj函数
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,const int& cost_function, int max_num_id,int max_time_id)
首先是对优化问题进行一些设置，points是控制点的初值，ts是B样条的时间，setCostFunction(cost_function)是选择在优化过程中加入那些类型的代价，如平滑、无碰撞、距离，等代价函数。
setControlPoints(points);setBsplineInterval(ts);setCostFunction(cost_function);setTerminateCond(max_num_id, max_time_id);
然后，执行optimize()函数来进行优化
optimize();
在optimize()函数中需要特别注意的是需要最小化的目标函数的设置，即costFunction函数的计算
opt.set_min_objective(BsplineOptimizer::costFunction, this);
costFunction函数计算目标函数的具体程序如下：
double BsplineOptimizer::costFunction(const std::vector<double>& x, std::vector<double>& grad,void* func_data) {BsplineOptimizer* opt = reinterpret_cast<BsplineOptimizer*>(func_data);double cost;opt->combineCost(x, grad, cost);opt->iter_num_++;/* save the min cost result */if (cost < opt->min_cost_) {opt->min_cost_ = cost;opt->best_variable_ = x;}return cost;
}
具体cost的计算是在其调用的combineCost函数中
void BsplineOptimizer::combineCost(const std::vector<double>& x, std::vector<double>& grad,double& f_combine)
在计算cost时，我们传入的是控制点，得到的是目标函数值及其梯度值
参考资料：
1、深蓝学院-移动机器人运动规划
：深度学习初探——卷积神经网络（CNN）)