概述

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

主要函数及作用

1. KinoReplanFSM::init

输入：句柄
作用：launch参数读取、定时器声明、pub声明、sub声明

2. KinoReplanFSM::waypointCallback

作用：孤立目标点回调函数：设置目标点位置、速度并进行其球形的可视化、执行状态变量更新

3. KinoReplanFSM::odometryCallback

作用：定位回调函数：位置、航向、三轴速度、标志位置true（表明接受到定位数据）

4. KinoReplanFSM::changeFSMExecState

作用：按照给定参数进行执行状态变量的更新，并打印输出

5.KinoReplanFSM::printFSMExecState

作用：打印当前执行状态变量的数值

6. KinoReplanFSM::execFSMCallback

作用：定时器，0.01s执行一次：状态机。本质是根据各种触发条件完成规划器在各状态间的转移

7. KinoReplanFSM::checkCollisionCallback

作用：0.05s的定时器，碰撞检测。  
1、当前局部轨迹未来一段时间和障碍物存在碰撞风险则重规划            
2、接收到目标点，目标点处于视野范围内，但是目标点距离障碍物太近则重规划（目标点附近进行采样，寻找可替代目标点）

8. KinoReplanFSM::callKinodynamicReplan

作用：
1.进行起始状态到终止状态间的满足动力学、避障、顺滑性、局部子目标点的局部轨迹，若成功则进行局部轨迹yaw的规划 
2.进行局部轨迹与yaw规划的b样条消息发布
3.进行初始动力学路径和局部轨迹b样条采样点控制点的可视化
返回值：局部规划成功返回true；失败返回false

状态机：状态转移图