本文上接Fast-Planner的前端和后端详解，详解一下整套规划流程运行的逻辑。如有问题，欢迎各位大佬评论指出，带着我一起进步。
四、规划系统运行逻辑
4.1 主函数 fast_planner_node.cpp
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) {
kino_replan.init(nh); //fast-planner第一篇文章
} else if (planner == 2) {
topo_replan.init(nh); //fast-planner第三篇文章
}
ros::Duration(1.0).sleep();
ros::spin();
return 0;
}
Fast-Planner整个代码包含了三篇文章，是中山大学周老师在港科时期的三篇文章
[1] Zhou B , Gao F , Wang L ,et al.Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight[J].IEEE Robotics and Automation Letters, 2019. DOI:10.1109/ LRA. 2019. 2927938.
[2] Zhou B , Gao F , Pan J ,et al.Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths[J]. 2019.DOI:10.48550/arXiv.1912.12644.
[3] Zhou B , Pan J , Gao F ,et al.RAPTOR: Robust and Perception-Aware Trajectory Replanning for Quadrotor Fast Flight[J].IEEE, 2021.DOI:10.1109/TRO.2021.3071527.
目前本系列博客分析的都是第一篇的内容，主函数默认运行的也是第一篇文章的算法，后续会持续更新其他两篇的算法详解和代码分析。
4.2 plan_mannager
plan_mannager 这个包用于对整个规划过程进行管理。
4.2.1 initPlanModules
启动部分规划算法和地图，设置部分参数，里面集成了三篇文章的算法，所以启动部分就能运行。
void FastPlannerManager::initPlanModules(ros::NodeHandle& nh)
4.2.2 checkTrajCollision
用于检测即将执行的轨迹是否与障碍物发生碰撞，方法是这样的，先计算当前的轨迹点，然后以当前轨迹点为起点，每0.02秒采样一个未来的轨迹点，然后计算是否碰撞导障碍物，同时一段轨迹最多检测未来6m的情况，当完整检测到大于6m的轨迹点均未与障碍物发生碰撞就返回true，否则返回false。
/*
根据local_data中属于NonUniformBspline类的position_traj的eveluateDeBoor()函数计算局部轨迹上的每个离散点的值，
并根据SDF类的evaluateCorseEDT()函数计算距离障碍物的距离。
检查一个轨迹是否与环境中的物体发生碰撞
*/
bool FastPlannerManager::checkTrajCollision(double& distance) {
double t_now = (ros::Time::now() - local_data_.start_time_).toSec();
double tm, tmp;
local_data_.position_traj_.getTimeSpan(tm, tmp); //轨迹的时间跨度
Eigen::Vector3d cur_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now); // 通过B样条轨迹获得当前时间点的轨迹点
double radius = 0.0;
Eigen::Vector3d fut_pt; //未来点
double fut_t = 0.02; //未来点到当前点的时间
//以0.02秒为一个时间间隔向未来的B样条轨迹进行采样来检查是否碰撞，但是相对于当前点检查最远到6.0m
while (radius < 6.0 && t_now + fut_t < local_data_.duration_) {
fut_pt = local_data_.position_traj_.evaluateDeBoor(tm + t_now + fut_t);
double dist = edt_environment_->evaluateCoarseEDT(fut_pt, -1.0);
if (dist < 0.1) {
distance = radius;
return false;
}
radius = (fut_pt - cur_pt).norm();
fut_t += 0.02;
}
return true;
}
4.2.3 kinodynamicReplan
主要流程，从kinodynamicA*路径搜索到B样条轨迹拟合优化，最后时间再分配完成路径规划的流程。
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;
//1. 判断搜索的起点和终点的距离，距离小于0.2，认为不需要再寻找，默认二者已经重合
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();
//2.开始带入进行kinoA*搜索
int status = kino_path_finder_->search(start_pt, start_vel, start_acc, end_pt, end_vel, true);
//2.1 当前端没有找到路径时
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;
}
//3.路径搜索成功后，通过0.01s为时间间隔获得更大时间分辨率的轨迹
plan_data_.kino_path_ = kino_path_finder_->getKinoTraj(0.01);
t_search = (ros::Time::now() - t1).toSec();
// parameterize the path to bspline
//4.利用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
//5.B样条轨迹优化
t1 = ros::Time::now();
int cost_function = BsplineOptimizer::NORMAL_PHASE;
//在当前轨迹达到感知距离以外并未达到目标点，目标函数需要加上ENDPOINT优化项，此时的优化变量应该包含最后Pb个控制点；
//当前端寻找的路径状态已经是REACH_END时，由于拟合最后Pb个控制点已经能保证位置约束，因此优化项中不再包含ENDPOINT，优化变量也不再包含最后Pb个控制点。
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样条优化花费的时间
// iterative time adjustment
//6.利用非均匀B样条类进行迭代时间调整
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
//7.保存轨迹结果
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.3 kino_replan_fsm.cpp
main函数默认调用该类，用于Fast-Planner第一篇文章，定义了一个KinoReplanFSM类，这个类包含了一系列的参数，flag，规划数据，ROS组件。主要用于调用规划主流程kinodynamicReplan，执行整一套订阅里程计和目标位置并调用轨迹规划。
4.3.1 KinoReplanFSM::init()
用于设置一系列参数，然后初始化规划主流程kinodynamicReplan。定义了一系列ROS组件，包括定时器，订阅器，发布器。
//设置一系列参数，然后对规划管理类的智能指针对象planner_manager_进行了初始化
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 */
//createTimer定时器，按照一定频率调用回调函数
exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this); //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);
}
上面中含有几个比较重要的回调函数
4.3.2 execFSMCallback
exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this); //this作用在于在当前对象实例中才能调用该函数
这个函数每0.01秒触发一次。
void KinoReplanFSM::execFSMCallback(const ros::TimerEvent& e) {
static int fsm_num = 0;
fsm_num++;
//1. 每1秒打印一次当前执行状态
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_) {
//2.INIT阶段有里程计有trigger_，则状态变为WAIT_TARGET
case INIT: {
if (!have_odom_) {
return;
}
if (!trigger_) {
return;
}
changeFSMExecState(WAIT_TARGET, "FSM");
break;
}
//3.WAIT_TARGET阶段有目标时，则状态变为GEN_NEW_TRAJ
case WAIT_TARGET: {
if (!have_target_)
return;
else {
changeFSMExecState(GEN_NEW_TRAJ, "FSM");
}
break;
}
//4.GEN_NEW_TRAJ阶段，调用callKinodynamicReplan进行规划
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"); //规划失败，则状态依旧为GEN_NEW_TRAJ
}
break;
}
//5.EXEC_TRAJ执行阶段
case EXEC_TRAJ: {
/* 判断是否需要重规划 */
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;
//当前距离与起点距离小于horizion
} else if ((info->start_pos_ - pos).norm() < replan_thresh_) {
// cout << "near start" << endl;
return;
} else {
changeFSMExecState(REPLAN_TRAJ, "FSM"); //进入REPLAN_TRAJ阶段
}
break;
}
//6.REPLAN_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;
}
}
}
4.3.3 checkCollisionCallback
safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);
这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。
//这一执行器回调函数是用来判断目标点是否有障碍物或者是轨迹执行过程中是否有障碍物的。
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);
//小于0.3认为障碍物与目标位置碰撞，可认为将障碍物膨胀了0.3
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;
}
}
}
}
//找到新的目标点，立刻状态切换为重规划REPLAN_TRAJ
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 ---------- */
//如果目标点周围没有障碍物且目前状态是EXEC_TRAJ，则利用checkTrajCollision进行轨迹检查
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");
}
}
}
目前为止针对Fast-Planner第一篇文章的算法及代码分析完毕，后续会更新另外两篇文章和地图部分。。。
1829
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
