EGO_Planner代码学习（四）：轨迹规划器流程——上

之前学习了EGO-Planner启动流程、轨迹服务器流程、以及控制器流程，已经给继续学习EGO-Planner规划器打好了基础，下面就详细的记录一下规划器部分的流程以及细节。由于整个的篇幅较长，本节先写一下规划器总体的流程，详细的轨迹生成的细节后文再进行补充。

EGO-Planner轨迹规划器流程

和前几节一样，在学习轨迹规划器的流程之前，要对规划器的功能有一个大概的了解，轨迹规划器在src/planner/planner_manger路径下，是规划器的核心代码，src/planner路径下的其他功能包都是为其服务的。话不多说，直接看伪代码：

ego_planner_node结点的启动流程
//进入函数主接口ego_planner_node.cpp
1->	初始化ros结点	
2->	创建ego_planner状态机：repo_replan
3->	repo_replan_init(nh)		//代码的主要进程
4->	ros::spin()

->	跳转到repo_replan_init(nh)

1->	初始化参数，当前的路标点、进程状态、收到目标标志位、收到里程计数据标志位、收到pre_agent标志位
2->	从参数服务器载入状态机fsm的参数：
		----------------------------------------------------------------------------------------------
		参数			变量名						默认值		取值（single_run_in_exp\advanced_param_exp）
		----------------------------------------------------------------------------------------------
		飞行模式			target_type_				-1			1or2（1：选点，2：waypoint）
		重规划时间阈值	replan_thresh_				-1.0		1.0
		停止规划距离		no_replan_thresh_			-1.0		1.0
		规划视界			planning_horizen_			-1.0		6.0
		规划时间视界		planning_horizen_time_		-1.0		3.0
		应急时间			emergency_time_				 1.0		1.0
		真机运行标志位	flag_realworld_experiment_	false		true
		使能fail_safe	enable_fail_safe_			true		true
		路标点数量		waypoint_num				-1			>1
		路标点xyz		waypoints_[i][j]			/			/
		---------------------------------------------------------------------------------------------
3->	初始化主模块：	//可视化模块、规划管理器
	//均为fsm的子类
	//第一个子类visualization_
	->	初始化可视类指针：visualization_（构造函数）	
		->	创建5个发布者	//大概后面会用这几个发布者发布可视化数据

	//第二个子类planner_manager_
	->	初始化规划管理器类指针：planner_manager_
		//初始化规划器需要的各个模块
		->	planner_manager_->initPlanModoles(&nh,vis)
			->	从参数服务器初始化规划器参数PlanParameters pp_:
				--------------------------------------------------------------
				参数			变量名						默认值		取值
				--------------------------------------------------------------
				最大速度			max_vel						-1.0		0.5
				最大加速度		max_acc						-1.0		6.0
				最大加加速度		max_jerk					-1.0		4
				可行公差？		feasibility_tolerance		 0.0		0.05
				控制点距离		control_points_distance		-1.0		0.4
				规划视界			planning_horizon			5.0			6
				使用独特轨迹？	use_distinctive_trajs		false		false
				飞机id			drone_id					-1			0
				---------------------------------------------------------------

			->	初始化grid_map		//建图部分，权且不看
				->	grid_map->initMap(&np)

			->	初始化bspline_optimizer	//曲线优化器
				->	bspline_optimizer->setParam(&nh) //从参数服务器载入优化器的参数
				--------------------------------------------------------------
				参数			变量名				默认值		取值
				--------------------------------------------------------------
				sommth权重		lambdal1_			-1.0		1.0
				safe权重		lambdal2_			-1.0		0.5
				feasible权重	lambdal3_			-1.0		0.1
				fitness权重		lambdal4_			-1.0		1.0
				距离？			dist0_				-1.0		0.5
				集群无碰撞距离	swarm_clearnce_		-1.0		0.5
				最大速度			max_vel_			-1.0		0.5
				最大加速度		max_acc_			-1.0		6
				-------------------------------------------------------------
				->	bspline_optimizer->setEnvironment( grid_map_,obj_predictor_)	//配置地图
				//ta也有子类，无线套娃
				->	初始化a_star_	//a*搜索
				->	bspline_optimizer->a_star_.initGridMap()
			//fsm 的子类vis 也是planner_manager_的子类
			->	visulization_ = vis	//值传递，有个疑问这里为啥不使用引用传递？莫非这样写代码更鲁棒？
		//集群轨迹存储器
		->	planner_manager_->deliverTrajToOptimizer()	//存储轨迹 
			//swarm_trajs_buf_的数据是从哪来的，是从父类传来的还是有回调函数来接收集群轨迹数据
			//盲猜后面的回调函数会做多机轨迹的处理
			->	把成员变量swarm_trajs_buf_传给子类bspline_optimizer的私有成员变量swarm_trajs_
			
			//这种类间传递的方式可以学习一下
		->	planner_manager_->setDroneIdtoOpt()		//设置飞机id
			->	把成员变量pp_drone_id传给子类bspline_optimizer的私有成员变量drone_id_
//把需要的子类都设置完了，简单总结一下各个类之间的传递关系

     	--------->visualization_
      	|
fsm----->						  				-------->grid_map_
      	|		           		 				|								------>grid_map_
     	--------->planner_manager_------------>	|------->bspline_optimizer_--->	|	
												|								------->a_star_							
												------->visualization_	
	
4->	回调函数部分：
	->	创建定时器：exec_timer_，间隔10ms进入一次回调函数execFSMCallback()	//状态机
	->	创建定时器：safety_timer_，间隔50ms进入一次回调函数checkCollisionCallback()
//回调函数部分结束，两个定时器回调函数是代码的重要部分，后面有介绍
//继续看软件流程
->	创建订阅者odmo_sub_，订阅里程计数据

->	if 飞机id大于等于1
	创建订阅者swarm_trajs_sub_，从 /drone_(id-1)_planning/swarm_trajs上订阅集群轨迹		//果然回调函数是在处理多机轨迹并传给了planner_manager_->swarm_trajs_buf_

->	创建发布者swarm_trajs_pub_，将traj_utils::MultiBsplines 类型数据发布到/drone_(id)_planning/swarm_trajs话题上	

->	创建发布者broadcast_bspline_pub_，将traj_utils::Bsplines 类型数据发布到/planning/broadcast_bspline_from_planner话题上	
->	创建订阅者broadcast_bspline_sub_，从/planning/broadcast_bspline_to_planner话题上订阅数据

	//发布给traj_server的
->	创建发布者bspline_pub_，将traj_utils::Bsplines 类型数据发布到/planning/bspline话题上
->	创建发布者data_disp_pub_，将traj_utils::DataDisp 类型数据发布到/planning/data_display话题上

->	if 飞行模式等于手动模式	//rviz选点
	->	创建订阅者waypoint_sub_，订阅路标点话题/move_base_simple/goal
->	else if 飞行模式等于预设模式	//提前设置路标点
	->	创建订阅者trigger_sub_，订阅话题/traj_start_trigger	//进入悬停状态一段时间，触发！！！（多机的话可以设置一个同步触发机制）
	->	等待1秒
	->	等待收到里程计信息，若在真实环境下还需等待触发
	->	处理路标点
		->	将路标点存入新的变量wps_里
		->	可视化
		->	规划第一个路标点
->	else 报错
//初始化到这就结束了
	

//回调函数流程
execFSMCallback():
->	关闭定时器exec_timer_	//避免堵塞
->	每秒打印一次当前状态
	->	if 没有odmo_数据，报错
	->	if 没有收到目标点，报错
//经典状态机（流程），再推一次，痛并快乐着
->	switch(exec_state_)
	//初始状态，在状态机初始化的时候就被设置为INIT
	->	INIT:
		->	if 没有odmo_数据，goto force_return跳转到函数末尾
		->	状态切换为WAIT_TARGET	//如果两次跳转的状态相同，累加计数
		->	break

	->	WAIT_TARGET:
		->	if 没有收到目标点或没有收到触发，goto force_return，跳转到函数末尾
		->	状态切换为SEQUENTIAL_START		//如果两次跳转的状态相同，累加计数
		->	break

	->	SEQUENTIAL_START:	
		//for swarm
		//swarm_trajs_sub_的回调函数末尾会把have_recv_pre_agent_置1	
		->	if drone_id<=0   或   drone_id>=1并且have_recv_pre_agent_ 
			->	if 里程计信息、目标点、触发都到位了
				->	bool success = planFromGloablTraj(10);
					->	判断状态是否变化，是 flag = false ,否 flag = true
					->	for 循环 10次
						->	if(callReboundReplan(true,flag)==1)	
							->return true;
					->	return false		
				->	if success：		
					->	状态切换为EXEC_TRAJ		//如果两次跳转的状态相同，累加计数
					->	publishSwarmTrajs(true)		//根据规划器局部信息发布集群轨迹以及广播轨迹
						->	根据规划器局部信息生成bspline曲线
						->	if bool ==1
							//多机模式，要向下一架飞机发布集群轨迹
							//集群轨迹buf_的数据由swarm_trajs_sub_的回调函数处理，接收的是上架飞机发布的集群数据
							//记录是哪个飞机发送的
							->	buf_.drone_id_from = drone_id

							//例子 ：0发布了2条轨迹
							->	if  buf_.traj.size == id+1 ，buf_.traj.back() = bspline					
							//例子： 0发布了1条轨迹
							->	else if buf_.traj.size == id ，buf_.traj.push_back() = bspline
							//上个无人机发布的轨迹数量只能等于本机id-1或本机id
							->	else ，报错

							->	swarm_trajs_pub_发布buf_的数据
						->	broadcast_bspline_pub_发布bspline 	//广播地只是单个轨迹吗？
				->	else unsuccess：
					->	报错
					->	状态切换为SEQUENTIAL_START		//如果两次跳转的状态相同，累加计数
			->	else ，报错					
		->	break


	->	GEN_NEW_TRAJ:
		->	bool success = planFromGloablTraj(10)
		->	if success：
			->	状态切换为EXEC_TRAJ		//如果两次跳转的状态相同，累加计数
			->	flag_escape_emergency = 1
			->	publishSwarmTrajs(false)
		->	else unsuccess:
			->	状态切换为GEN_NEW_TRAJ		//如果两次跳转的状态相同，累加计数
		->	break


	->	REPLAN_TRAJ:
		->	bool success = planFromCurrentTraj(1)
		->	if success：
			->	状态切换为EXEC_TRAJ		//如果两次跳转的状态相同，累加计数
			->	publishSwarmTrajs(false)
		->	else unsuccess:
			->	状态切换为REPLAN_TRAJ		//如果两次跳转的状态相同，累加计数		
		->	break


	//执行轨迹
	->	EXEC_TRAJ:
		->	定义局部轨迹指针 *info = &planner_manager->local_data_	//数据来自哪呢？planner_manager有一个专门的函数更新local_data_（updateTrajInfo）
		->	计算当前时间和局部轨迹开始时间之差t_cur，要是t_cur超过局部轨迹的总时间，t_cur不再增加
		->	计算t_cur时刻局部轨迹期望的位置坐标 pos

		->	if 预设点模式，且没规划完最后一个点，且当前位置距离end_pt_小于 停止重规划距离no_replan_thresh
			->	规划下一个路标点

		->	else if 局部终点和全局终点距离太近
			->	if 当前时间大于局部轨迹的总时间
				->	收到目标、收到触发标志位置零	//到终点了呗
				->	if 预设点模式
					->	返回规划第一个路标点
				->	状态切换为WAIT_TARGET	//如果两次跳转的状态相同，累加计数
				->	goto force_return，跳转到函数末尾
			->	else if 当前位置距离end_pt_小于 停止重规划距离no_replan_thresh，且当前时间大于重规划时间replan_thresh_
				->	状态切换为REPLAN_TRAJ		//如果两次跳转的状态相同，累加计数

		->	else if 当前时间t_cur_ > 重规划时间replan_thresh_
			->	状态切换为REPLAN_TRAJ		//如果两次跳转的状态相同，累加计数
		->	break


	->	EMERGENCY_STOP:
		->	if flag_escape_emergency == 1
			->	callEmergecyStop(odmo_pos_)
				->	根据当前里程计位置生成b样条曲线
				->	bspline_pub_发布生成的b样条曲线
		->	else
			->	if enable_fail_safe_ ==1，且速度为0
				->	状态切换为GEN_NEW_TRAJ		//如果两次跳转的状态相同，累加计数
			->	flag_escape_emergency = 0

		->	break
//状态机流程部分结束！！！
->	data_disp_pub_发布可视化数据
//状态机的回调函数流程结束



//碰撞检查的回调函数
checkCollisionCallback():
->	定义局部轨迹指针 *info = &planner_manager->local_data_
->	定义地图 map = planner_manager->grid_map_
->	if 当前状态为等待触发或局部轨迹开始时间非常小
	->	跳出函数
->	if 深度图数据丢失
	->	报错
	->	enable_fail_safe_ = 0
	->	状态切换为EMERGENCY_STOP		//如果两次跳转的状态相同，累加计数
//检查轨迹是否碰撞
->	设置迭代步长，迭代查验当前局部轨迹前2/3是否有碰撞
	->	如果t时刻位置pos在地图中为占据，occ|=1
	//还要查验和其他轨迹的距离
	->	如果当前时刻局部轨迹位置和其他轨迹位置的距离dist<swarm_clearance_
		->	occ = 1
	->	if occ==1
		->	bool success = planFromCurrentTraj()	
		->	if success：
			->	状态切换为EXEC_TRAJ		//如果两次跳转的状态相同，累加计数
			->	publishSwarmTrajs(false)
		->	else unsuccess:
			->	if t-t_cur < emergency_time_
				->	报错
				->	状态切换为EMERGENCY_STOP		//如果两次跳转的状态相同，累加计数
			->	else
				->	状态切换为REPLAN_TRAJ		//如果两次跳转的状态相同，累加计数
//碰撞检查的回调函数结束



//状态机部分还是有细节的子函数需要理解，放在下节看吧
//订阅者的回调函数还需要详细阅读，再回头来看状态机会更清晰
planFromGloablTraj()
planFromCurrentTraj()