路径规划与避障算法(二)---规划层与定位层的接口定义及数据融合

版权声明：本文为博主原创文章，转载请联系博主。https://mp.csdn.net/mdeditor/82777752#

接口概述

1.规划层与定位层的数据交互主要体现在以下几个方面:

• 定位层提供车辆在map坐标系下当前时刻的位置信息,角度信息
• 定位层提供车辆当前时刻的线速度信息,线性加速度信息
• 定位层提供车辆当前时刻的角速度信息,角加速度信息
• 定位层提供车辆上各个传感器,惯导,baselink以及map坐标系之间的转换关系并以tf_tree的形式发布

2.定位层对外发布信息时,需要统一发布的数据所存在的坐标系,通常选用的是东北天坐标系,东北天坐标系指的是右手坐标系下:

• 以正东方向为X轴正半轴
• 以正北方向为Y轴正半轴
• 以向上方向为Z轴正半轴
• 以X正半轴逆时针转向Y轴正半轴角度为角度增加方向.

3.定位层的tf_tree是无人驾驶中很重要的一个组成部分,它描述了车身各个传感器,惯导与baselink,map之间的坐标转换关系:

• 需要考虑各个传感器与baselink的转换关系,不然会造成障碍物位置的偏离

4.由于规划层同时与定位层以及感知层进行数据交互,定位层与感知层的信息发布时间往往是不一致的,这样会对规划层的避障规划结果造成比较大的影响.因此建议首先对激光雷达以及惯导在硬件上进行硬同步操作,同时在软件上利用message_filters对两层进行软同步.

代码详述

接口四元数转换欧拉角的方式:

message_filters::Subscriber<nav_msgs::Odometry> odom_sub(n, "/gps/odom", 1);
//定义callback函数,这里是与感知信息结合做了一个软同步

void dwa_planner_node<