ego planner代码解读——ego_replan_fsm.cpp文件【状态机】

概述

ego_replan_fsm.cpp订阅实时定位和目标点信息，每隔0.01s执行一次状态机，进行状态切换；每隔0.05s执行一次碰撞检测，按需进行重新规划或者紧急停止。核心为执行变量exec_state_

主要函数及作用

1. EGOReplanFSM::init

输入：句柄
作用：1、参数初始化
     2、launch文件参数读取
     3、规划管理及可视化指针初始化
     4、odom和目标点的订阅、定时器、发布声明等

2. EGOReplanFSM::planGlobalTrajbyGivenWps

作用：1、获取终点位置     
     2、子目标点序列可视化   
     3、采用mimimum snap或者eigen库求解得到全局五次多项式轨迹[成功:全局轨迹可视化  终点速度为0  目标点相关标志位置于true  exec_state_状态改为GEN_NEW_TRAJ]

3. EGOReplanFSM::waypointCallback

订阅：孤立目标点   
作用：1、保存无人机目标位置、初始位置       
     2、目标点可视化    
     3、minimum jerk【若插入点】或者采用eigen库规划当前位置与目标位置的多项式全局轨迹表达式，成功则进行全局轨迹采样点可视化、目标速度为0、exec_state_变量状态更改、标志位置于true等

4. EGOReplanFSM::odometryCallback