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

订阅：odom定位
作用：1、设置无人机当前位置、当前速度、当前航向   
     2、标志位置于true（表示接受到定位数据）

5. EGOReplanFSM::changeFSMExecState

输入：给定执行状态    字符串   
作用：1、如果给定状态与exec_state_相同，则持续呼叫次数变量+1；若不同，则将持续呼叫次数变量置于1      
     2、将exec_state_状态改变为给定值，并进行相关提示信息的打印

6. EGOReplanFSM::timesOfConsecutiveStateCalls

作用：得到键值对《当前执行状态被连续调用的次数  当前执行状态exec_state_》

7. EGOReplanFSM::printFSMExecState

作用：状态机当前状态打印

8. EGOReplanFSM::execFSMCallback

作用：0.01s执行一次状态机切换判断，本质是根据各种触发条件完成规划器在各状态间的转移，具体状态切换过程见下图

9. EGOReplanFSM::planFromCurrentTraj

作用：从当前局部轨迹的当前状态【PVA】进行新的局部轨迹更新
    【依次尝试原始局部轨迹提取b样条分段端点-------》eigen采样提取b样条分段端点----->插值后采用minimum snap采样提取b样条分段端点，只要成功就不执行后续选择】
返回值：新的局部轨迹更新成功返回true；否则false

10. EGOReplanFSM::checkCollisionCallback

作用概述：0.05s执行一次避障回调函数
局部轨迹碰撞检测范围：当前时刻-----》当前局部轨迹定义域终点时间，时间采样间隔0.01s：
                  起始当前时刻小于定义域时间2/3，则仅仅碰撞检测局部轨迹整体的前2/3；
                  起始当前时刻大于定义域时间2/3，则碰撞检测当前时刻-----》定义域终点时刻
局部轨迹碰撞处理：若某个时刻发生碰撞，则从当前局部轨迹的当前状态【PVA】进行新的局部轨迹更新，更新成功则exec_state_状态改为EXEC_TRAJ退出；
               若失败且轨迹碰撞的时刻与当前时刻的间隔小于紧急停止时间：则将则将exec_state_状态改为EMERGENCY_STOP；
               若失败且轨迹碰撞的时刻与当前时刻的间隔大于紧急停止时间，执行重新规划：将exec_state_状态改为REPLAN_TRAJ

11. EGOReplanFSM::callReboundReplan

作用：从全局路径根据局部规划视野确定局部子目标点的位置速度
     调用ego planner论文方法产生满足始末条件约束的三次均匀b样条轨迹
     规划成功则进行b样条信息的发布和控制点可视化
返回值：成功规划局部轨迹，返回true；否则false

• 11.1核心函数planner_manager_->reboundReplan

输入：起点位置  起点速度  起点加速度{局部轨迹规划为GEN_NEW_TRAJ状态首次调用为（0，0，0）}  局部规划终点位置   局部规划终点速度  
     标志位1【是否直接采用五次多项式轨迹，或者基于原轨迹提取b样条端点】    
     标志位2：局部轨迹规划为GEN_NEW_TRAJ状态首次调用则false；否则为true【是否需要插值『决定用eigen还是minimum  jerk』】

作用：经过init、optimal、refine步骤，规划满足始末条件约束的三次均匀b样条控制点位置
     备注：起点是起点，终点不一定为给定终点【但是很靠近局部终点，因为需要minimum snap采样
          局部规划成功则进行局部轨迹信息local_data_更新
     init： 根据无人机当前位置速度、局部子目标位置速度规划局部参考路径【minimum snap或者eigen】；局部参考路径提取采样点或或者基于原轨迹+新生成五次多项式【原轨迹终点-----》给定局部子目标点】组合提取采样点（作为三次b样条分段的始末点）；通过采样点进行三次b样条曲线的控制点的解算；控制点序列中碰撞段的提取、无碰A*路径规划、碰撞段的控制点[p、V]对的生成；无碰A*路径和局部轨迹的可视化
     optimal：采用lbfgs算法进行三次均匀b样条曲线的轨迹优化【平滑项、障碍物项、动力学项】----------保留优化后的控制点位置
     refine：如果无人机不满足动力学约束，则综合考虑平滑项、动力学项及拟合项采用三次均匀b样条进行轨迹优化，如果优化成功则进行相关均匀b样条信息的更新

返回值：成功规划轨迹返回true
      若始末距离近、基于原始局部轨迹采样但是采样点过少；2步或3步优化失败返回false

备注：初始局部参考路径采用minimum snap条件：程序启动第一次进入，本次调用需要，基于原轨迹得到采样点路径太长

12. EGOReplanFSM::callEmergencyStop

输入：无人机当前位置
作用：确定无人机紧急停止时的三次局部b样条轨迹并发布【控制点位置重合为原无人机位置【多控制点重合】，故速度、加速度也都为0】

13. EGOReplanFSM::getLocalTarget

作用：1、全局路径大于局部规划视野时，进行变量last_progress_time_更新【为距离起始点距离最近的轨迹点时间】
     2、进行局部规划目标点位置和速度的确定『局部规划视野大于全局路径长度：局部规划目标点为最后终点，速度为0』
                【局部规划视野小于全局路径长度：局部规划目标点为局部视野附近的点，如果局部规划目标点距离终点很近【小于V^2/A】则速度为0】
                【局部规划视野小于全局路径长度：局部规划目标点为局部视野附近的点，如果局部规划目标点距离终点较远【大于V^2/A】则速度为对应全局轨迹点速度】

状态机：状态转移图