EGOSwarm—Prometheus代码阅读笔记【三】——EGOReplanFSM类实现

Prometheusv2中的EGOSwarm代码阅读笔记，个人学习用。
目前还有部分代码没看明白，欢迎大家交流！
EGOPlanner—Prometheus代码阅读笔记汇总

主要参考资料：
Fast Planner——代码解读参考资料整理
EGO-Swarm代码阅读笔记汇总
代码来源：
https://github.com/ZJU-FAST-Lab/ego-planner
https://github.com/amov-lab/Prometheus

---

---

（一）EGOReplanFSM类【状态机转移】

1.源文件位置

ego_planner_swarm\plan_manage\src\ego_replan_fsm.cpp

2.主要功能

• 订阅：无人机位姿odom_world、编号小于该无人机的轨迹/uav*_planning/swarm_trajs、planning/broadcast_bspline_to_planner
• 发布：当前无人机的/uav*_planning/swarm_trajs、planning/broadcast_bspline_from_planner、planning/bspline、planning/data_display
• 在状态间切换，完成初始状态、等待触发、生成轨迹、重规划轨迹、执行轨迹、紧急停止等操作
• 在全局轨迹上按照局部规划半径得到局部目标

3.成员变量

    enum FSM_EXEC_STATE { //状态机FSM状态
      INIT,			//初始状态
      WAIT_TARGET,  //等待触发
      GEN_NEW_TRAJ, //生成轨迹
      REPLAN_TRAJ,  //重规划轨迹
      EXEC_TRAJ,    //执行轨迹
      EMERGENCY_STOP,//紧急停止
      SEQUENTIAL_START //顺序启动
    };
  
    enum TARGET_TYPE {
      MANUAL_TARGET = 1, //手动选择目标点
      PRESET_TARGET = 2, //预设目标点
      REFENCE_PATH = 3
    };

    /* planning utils */
    EGOPlannerManager::Ptr planner_manager_;	//ego过程管理
    PlanningVisualization::Ptr visualization_;	//可视化
    traj_utils::DataDisp data_disp_;
    traj_utils::MultiBsplines multi_bspline_msgs_buf_; //traj_utils/msgs/MultiBsplines.msg

    /* parameters */
    int target_type_; //飞行模式 1：mannual select；2：hard code 使用给定的全局路标点
    double no_replan_thresh_, replan_thresh_;//目标点与无人机当前位置小于no_replan_thresh_就不需要再规划到这个点的轨迹
    double waypoints_[10][3];	//目标点三轴位置
    int waypoint_num_, wp_id_;  //路标点个数，每架飞机一个终点 
    double planning_horizen_, planning_horizen_time_;//局部规划的范围 
    double emergency_time_; //如果距离碰撞的时间小于该值，立刻切换到停止模式
    bool flag_realworld_experiment_;//是否为真实环境实验
    bool enable_fail_safe_;

    /* planning data */
    bool have_trigger_, have_target_, have_odom_, have_new_target_, have_recv_pre_agent_;
    FSM_EXEC_STATE exec_state_;
    int continously_called_times_{0};

    Eigen::Vector3d odom_pos_, odom_vel_, odom_acc_; // odometry state
    Eigen::Quaterniond odom_orient_;

    Eigen::Vector3d init_pt_, start_pt_, start_vel_, start_acc_, start_yaw_; // start state
    Eigen::Vector3d end_pt_, end_vel_;                                       // goal state
    Eigen::Vector3d local_target_pt_, local_target_vel_;                     // local target state
    std::vector<Eigen::Vector3d> wps_;
    int current_wp_;

    bool flag_escape_emergency_; //如果无人机停止或者得到新的无碰撞路径就设为true代表安全了
 
    /* ROS utils */
    ros::NodeHandle node_;
    ros::Timer exec_timer_, safety_timer_;
    ros::Subscriber waypoint_sub_, odom_sub_, swarm_trajs_sub_, broadcast_bspline_sub_, trigger_sub_;
    ros::Publisher replan_pub_, new_pub_, bspline_pub_, data_disp_pub_, swarm_trajs_pub_, broadcast_bspline_pub_;

4.成员函数

EGOReplanFSM::init

参数初始化
设置/parameters/中的成员变量，修改位置：advanced_param.xml

---

定时器
exec_timer_：状态机切换，周期0.01秒。回调函数execFSMCallback
safety_timer_：安全检查，周期0.05秒。回调函数checkCollisionCallback

---

订阅
/odom_world：回调odometryCallback。保存无人机当前里程计信息，包括位置、速度和姿态
/uav*_planning/swarm_trajs：回调swarmTrajsCallback。订阅前一架无人机的MultiBsplines，0号机随便飞
/planning/broadcast_bspline_to_planner：回调BroadcastBsplineCallback

---

发布
/uav*_planning/swarm_trajs：发布当前无人机的MultiBsplines
/planning/broadcast_bspline_from_planner：
/planning/bspline：
/planning/data_display：

---

1.手动设置目标模式：TARGET_TYPE::MANUAL_TARGET
订阅/uav*/prometheus/motion_planning/goal：回调waypointCallback
2.预先设置目标模式：TARGET_TYPE::PRESET_TARGET
订阅/uav*/traj_start_trigger启动信号：回调函数triggerCallback。等待里程计信息和启动信号，两者都收到后调用readGivenWps。

EGOReplanFSM::readGivenWps

预设目标模式下：读取waypoints_中的路标点后可视化，调用planNextWaypoint

EGOReplanFSM::planNextWaypoint

调用EGOPlannerManager::planGlobalTraj（输入始末位置、速度和加速度）得到全局多项式轨迹gloabl_traj_
如果规划成功{
则end_pt_=next_wp；
如果当前状态为WAIT_TARGET，则转移到GEN_NEW_TRAJ状态；
否则等待切换到EXEC_TRAJ状态，并执行轨迹，直到不再是EXEC_TRAJ状态后切换到REPLAN_TRAJ状态。
}

EGOReplanFSM::triggerCallback

have_trigger_ = true;
init_pt_ = odom_pos_;

EGOReplanFSM::waypointCallback

初始位置设为当前位置，调用planNextWaypoint规划到msg的路径并执行。

EGOReplanFSM::odometryCallback

保存无人机的位置、速度和方向到odom_pos_、odom_vel_和odom_orient_
have_odom_=true

EGOReplanFSM::BroadcastBsplineCallback

首先判断接收到的轨迹是不是自己的轨迹，或者接收到的轨迹距离发出已经过去超过0.25秒，如果是就结束
如果接收到的无人机轨迹目前位置，距离当前无人机大于规划距离的4/3就结束
保存收到无人机轨迹的控制点、区间间隔、时长、初始位置和时间到planner_manager_->swarm_trajs_buf_[id]
调用EGOPlannerManager::checkCollision检查轨迹是否会和自己小于安全距离，如果小于安全距离，则切换到REPLAN_TRAJ状态

EGOReplanFSM::swarmTrajsCallback

如果没有当前无人机odom 或 接收到的集群轨迹不等于接收到的无人机id+1 或 B样条轨迹的阶数不为3就结束。
对于接收到的每一条无人机的轨迹{
如果该架无人机目前位置与当前无人机的距离大于规划距离的4/3就跳过
保存收到无人机轨迹的控制点、区间间隔、时长、初始位置和初始时间到planner_manager_->swarm_trajs_buf_[id]
调用checkCollision(int drone_id)
}
最后have_recv_pre_agent_=true

EGOReplanFSM::changeFSMExecState

切换到new_state模式
若新状态与当前状态一致，则continously_called_times_++ ，否则continously_called_times_置1

EGOReplanFSM::timesOfConsecutiveStateCalls

返回当前状态被连续切换的次数continously_called_times_

EGOReplanFSM::printFSMExecState

打印当前无人机的exec_state_

EGOReplanFSM::execFSMCallback【重要】

（待补充一个流程图）

进入状态机时会类似于关中断，防止在中断过程中相应其他中断。每秒会判断是否有里程计信息和目标位置。

---

INIT 如果没程计信息就直接结束，否则切换到WAIT_TARGET
WAIT_TARGET>如果没有目标位置和启动信号就直接结束，否则切换到SEQUENTIAL_START
SEQUENTIAL_START 如果收到了其他无人机的轨迹并且有位姿、目标、启动信号则调用planFromGlobalTraj(10)> 。如果成功，则切换到EXEC_TRAJ 并publishSwarmTrajs(true) ；否则切换到SEQUENTIAL_START 。
GEN_NEW_TRAJ 调用planFromGlobalTraj(10) 。如果成功，则切换到EXEC_TRAJ并publishSwarmTrajs(false)，flag_escape_emergency_=true；否则切换到GEN_NEW_TRAJ。
REPLAN_TRAJ 调用planFromCurrentTraj(1) 。如果成功，则切换到EXEC_TRAJ 并publishSwarmTrajs(false)；否则切换到REPLAN_TRAJ 。
EXEC_TRAJ 如果是预先设置目标模式并且还有路标点没到并且当前位置与当前路标点的位置小于no_replan_thresh_则调用planNextWaypoint()规划到下一个路标点的轨迹；否则如果局部规划的终点与全局终点接近，并且{当前局部轨迹的执行时间已经超过了局部轨迹预计的执行时间，『则have_target_=have_trigger_=false，『如果是预先设置目标模式则规划到第一个路标点的路径』，切换到WAIT_TARGET模式』}，否则如果当前位置与全局目标点距离大于no_replan_thresh_并且当前局部轨迹的执行时间超过了replan_thresh_，则切换到REPLAN_TRAJ；否则如果当前局部轨迹的执行时间超过了replan_thresh_ ，则切换到REPLAN_TRAJ 。
EMERGENCY_STOP 如果flag_escape_emergency_=true，则调用callEmergencyStop(odom_pos_)
在当前位置停下；否则如果enable_fail_safe_=true并且无人机速度小于0.1，则切换到GEN_NEW_TRAJ
。最后flag_escape_emergency_=false 。

---

发布data_disp_ 。开中断。

EGOReplanFSM::planFromGlobalTraj

设置起始位置速度为当前无人机的位置和速度。如果是新切换到当前状态，则flag_random_poly_init=false；否则为true。调用trial_times 次callReboundReplan(true, flag_random_poly_init) ，如果有一次返回True则返回True，否则返回false。

EGOReplanFSM::planFromCurrentTraj

设置起始位置速度为当前时刻局部轨迹计算出的位置和速度。调用callReboundReplan(false, false) ，如果返回false，再调用callReboundReplan(true, false) ，如果返回false，则调用trial_times 次callReboundReplan(true, true) ，如果全都返回false，则函数返回false；否则函数返回true。

EGOReplanFSM::checkCollisionCallback

首先检查是否丢失位姿，如果丢失，enable_fail_safe_ = false ，状态切换到EMERGENCY_STOP 。
从当前时刻开始，以0.01秒为步长，如果t时刻轨迹上为障碍物（包括与其他无人机距离小于安全距离），如果planFromCurrentTraj() 返回true，则切换到EXEC_TRAJ 执行新的安全路径，并publishSwarmTrajs(false) ；如果返回false，如果将要碰撞的时间与现在的时间小于安全时间emergency_time_ 则切换到EMERGENCY_STOP 紧急停止，否则切换到REPLAN_TRAJ 重新规划轨迹。

EGOReplanFSM::callReboundReplan

调用getLocalTarget() 得到局部轨迹的目标点和到达该点时的速度。调用planner_manager_->reboundReplan，令have_new_target_=false。
如果reboundReplan()返回true，则发布新规划出的局部轨迹，并返回true，否则返回false。

EGOReplanFSM::publishSwarmTrajs

如果startup_pub 为true，multi_bspline_msgs_buf_.drone_id_from设为当前无人机。如果multi_bspline_msgs_buf_中轨迹的数量与当前无人机id+1一样，则修改最后一个轨迹为当前无人机此刻的局部B样条轨迹；如果multi_bspline_msgs_buf_中轨迹的数量与当前无人机id一样，则压入当前无人机此刻的局部B样条轨迹。最后发布multi_bspline_msgs_buf_和当前无人机此刻的局部B样条轨迹。
如果startup_pub为false，则只发布当前无人机此刻的局部B样条轨迹。

EGOReplanFSM::callEmergencyStop

规划出到stop_pos 的B样条路径，并用bspline_pub_ 发布

EGOReplanFSM::getLocalTarget

在全局轨迹planner_manager_->global_data_ 找到距离当前无人机位置planning_horizen_处的点作为局部轨迹的目标点，并将全局轨迹上离当前位置最近的点对应的时间赋值给planner_manager_->global_data_.last_progress_time_。
如果局部轨迹的目标点对应的时间大于了全局轨迹的时长那么将全局终点作为局部轨迹的目标点。如果全局终点到局部轨迹的目标点的距离小于无人机以做大速度和加速度可飞行的距离，则局部轨迹的目标点速度设为0；否则局部轨迹的目标点速度为全局轨迹在该点的速度。