EGO_Planner代码学习（二）：轨迹服务器ego_planner/traj_server代码流程_ego-planner

上一节我们学习了EGO_Planner的启动流程，下面我们来看一看 roslaunch ego_planner single_run_in_exp.launch启动了 ego_planner/traj_server结点后，该结点都做了什么工作呢

ego_planner/traj_server代码流程

我们查看一下src/planner/plann_manage文件夹下的CMakeLists文件，发现ego_planner/traj_server是通过编译该文件夹下src/traj_server.cpp文件生成的，下面我们就看看这个traj_server.cpp的代码流程。

1->	创建结点  traj_server
2->	订阅规划轨迹 ：planner/bspline
	回调函数：bsplineCallback()
	2.1->	收到轨迹，创建两个变量pos_pts konts分别接收geometry_msgs/Point[]类型的pos_pts 和时间变量konts
	2.2->	用收到的pos_pts，konts创建新的均匀B样条曲线pos_traj
	2.3->	计算后面要用的变量：
		start_time_	//等于收到轨迹的成员变量:起始时间
		traj_id_	//等于收到轨迹的成员变量:id
		traj_		//插入pos_traj及其一阶导、二阶导
		traj_duration_	///总时间
	2.4->	receive_traj_=true
3->	创建发布者：pos_cmd_pub ，往 /position_cmd 话题上发布 PositionCommand 类型的数据
4->	创建定时器，间隔10ms进入回调函数一次	
	回调函数 cmdCallback()：
	4.1->	是否收到了轨迹receive_traj_=true，没有的话跳出函数
	4.2->	计算现在的时间和起始时间的间隔 t_cur = time_now-start_time_
	4.3->	判断 t_cur  在不在总时间区间内
		4.3.1->	t_cur < traj_duration_	
			计算当前t_cur的pos vel acc yaw(后面导航命令用)，再算一个pos_f(不知道干嘛用的)
		4.3.2->	t_cur > traj_duration_
			计算终点pos ，vel acc 设0，yaw不变； 
		4.3.3->	t_cur < 0
			报错
	4.4->	把pos vel acc yaw等信息装入cmd里，pos_cmd_pub 发布一次cmd到/position_cmd
5->	在cmd指令里设置控制器增益系数
6->	1.0sleep，ros::spin()

	traj_server.cpp 子函数calculate_yaw()
	->参数：double t_cur，vector3d &pos ，ros::Time time_now ，ros::Time time_last
	->功能：计算yaw角方向，变化率，并对输出进行限幅，把yaw输出限制在[-PI，PI]
	->输出：pair of <yaw，yaw_dot>	
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

通过上述分析，我们发现ego_planner/traj_server结点其实就是为了将规划器发布的轨迹planner/bspline转化为控制器指令cmd并上传到position_cmd话题上。
下一节，我们分析ego_planner_node结点在启动后经历了那些流程。