fuel无人机自主探索代码解读3——fast_exploration_fsm.cpp【状态机】

一、概述

fast_exploration_fsm.cpp订阅实时定位和目标点信息，每隔0.01s执行一次状态机，进行状态切换；每隔0.05s执行一次碰撞检测，按需进行重新规划；每0.5s执行一次边界回调定时器，对于处于WAIT_TRIGGER和FINISH状态的规划器进行边界生成、视点计算和代价求解等。核心为执行变量state_。

二、主要函数及作用

1、FastExplorationFSM::init

作用：
1、fsm参数及数据初始化
2、探索管理类及规划可视化类的初始化
3、状态机变量初始化
4、ros中订阅发布及定时器的声明

2、FastExplorationFSM::FSMCallback

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

3、FastExplorationFSM::callExplorationPlanner

作用：根据当前起点位置 起点速度  起点加速度  起点航向【yaw  yaw‘  yaw“】进行全流程的探索规划。
     如果探索规划成功【当前位置、速度、加速度、yaw-----》最优视点【位置、yaw】】，则进行fd_->newest_traj_【最新局部轨迹】的赋值：
     b样条曲线次数、起始时间、索引id、局部轨迹位置b样条控制点位置和节点向量、局部轨迹yaw的b样条的控制点和时间间隔
流程：                  1、基于当前帧点云的区域生长边界搜索、pca主成分分析拆分、过时边界删除，得到临时边界簇【珊格坐标、体素滤波后珊格坐标、AABB包围框的下边界、AABB包围框的上边界、平均位置】
                       2、临时边界视点信息【位置、yaw【边界相对于视点位置】、可视珊格数】计算，并将其分裂为有效边界【按照可视边界珊格数进行视点排序】和无效边界，并进行【有效边界的索引更新】
                       3、进行TSP的全局引导路径生成，基于dijsktra的局部视点修正，得到局部规划的终点位置和yaw【某视点位置与yaw】
                       4、以当前点为起点，3中给点视点为终点，采用nlopt进行全参数的局部轨迹规划、b样条的yaw轨迹优化，更新局部轨迹信息【local_data_中除了start_time_未更新】
返回值：1、NO_FRONTIER：空间没有有效边界      2、FAIL：探索规划的起点和终点之间不存在A*无碰路径        
       3、FAIL：无碰A*路径的长度在最近和最远距离阈值范围之间：基于动力学路径搜索得到全局路径，采样后基于fast planner使用nlopt得到优化后的局部轨迹控制点位置、时间间隔，失败
       4、SUCCEED：成功完成本次探索规划

4、FastExplorationFSM::visualize

作用：
1、进行当前有效边界【立方体序列，涉及多余的失效边界的清空】的可视化显示【/planning_vis/frontier】
2、进行局部轨迹b样条采样点【时间间隔：0.01】和控制点【此处不显示】的可视化显示【球列：/planning_vis/trajectory】

5、FastExplorationFSM::clearVisMarker

作用：函数体为空

6、FastExplorationFSM::frontierCallback

作用：0.5s的边界回调定时器，当探索规划器状态机处于WAIT_TRIGGER和FINISH，进行环境边界簇更新、视点计算、cost计算、有效边界簇的可视化显示

7、FastExplorationFSM::triggerCallback

作用：子目标点序列订阅。若状态机当前状态为WAIT_TRIGGER，且目标点有效【大于-0.1时】，则状态机的当前状态由WAIT_TRIGGER-----》PLAN_TRAJ

8、FastExplorationFSM::safetyCallback

作用：0.05s执行一次的安全检测定时器：当前状态机处于EXEC_TRAJ状态，且与当前位置的6m范围内局部轨迹存在发生碰撞的轨迹点，则状态切入PLAN_TRAJ

9、FastExplorationFSM::odometryCallback

作用：odom定位的订阅【经过话题重映射】。无人机当前位置、速度、航向四元数及yaw的更新、标志位置于true【表明接受过定位数据】

10、FastExplorationFSM::transitState

输入：状态机的新状态值  字符串
作用：状态机的当前状态值更新为给定状态，并打印相关状态转移信息

三、状态机：状态转移图

四、FUEL链接

1、fuel无人机自主探索代码解读1——map_ros.cpp【地图ros接口】
2、fuel无人机自主探索代码解读2——sdf_map.cpp【占据栅格地图、膨胀占据栅格地图、ESDF】