1.获取车辆的Odometry原始信息

在carla中，通过订阅/carla/ego_vecle/odometry 可以查看车辆的全局位置信息,例如：

> header: seq: 118872stamp: secs: 5946nsecs:   5720187frame_id: "map"
child_frame_id: "ego_vehicle"
pose: pose: position: x: 96.5346984863y: -188.643692017z: 0.221679911017orientation: x: 1.09737910971e-07y: 5.40849999846e-07z: 0.0368271997411w: 0.999321648599covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: twist: linear: x: -0.00358968405948y: -3.63046108104e-06z: 2.80475536936e-07angular: x: 2.17012864219e-05y: -0.000262381265741z: -0.00483244467764covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

其中，pose.pose.position.x 和 pose.pose.position.y提供了全局x,y坐标，pose.pose.orientation提供了汽车的旋转角度。

1.1 全局坐标

全局坐标和GNSS传感器获得的经纬度坐标可以相互转换，统一地图内，两种坐标都可以用于车辆定位。

1.2 旋转角度

这里的旋转角度的坐标以N方向为零度，E为90度，W为-90度，S偏E侧为+180度，S偏W侧为-180度，如图所示：

这里值得注意的是：CARLA的odometry坐标系与真实世界的坐标系不同。正常坐标系是上北下南，左西右东，而CARLA里面是上北下南，左东右西。

该角度的具体计算的方法为：

//导入tf/transform_datatypes.h库
#include <tf/transform_datatypes.h>//使用tf::getYaw获取四元数中的偏航角，然后转换成角度
tf::getYaw(carla_odometry.pose.pose.orientation) * 180.0 / M_PI

计算出来与显示界面的Heading是相同的。
在这里插入图片描述

2. 坐标系变换

我们想要把Odometry坐标系变换成下面的角度坐标系:
在这里插入图片描述

首先，Odometry坐标系需要逆时针旋转90度，那么对应的角度应当减去90度：
$θ1=θ0−90°\theta_1=\theta_0 - 90°$
然后，取值范围需要变换到0-360度，对于原角度位于[90°,180°]的，减去90°后已经是变换后的角度，对于[0,90°]，[-0,-180°]角度，减去90°后会变成负数，需要增加360°变成正数。

$θ2={θ1,θ1>=0θ1+2π,θ1<0\theta_2 = \left\{\begin{matrix} \theta_1, \theta_1 >= 0\\ \theta_1 + 2\pi , \theta_1 <0 \\ \end{matrix}\right.$
变换过程如图所示：
在这里插入图片描述