【在实习期的快乐生活】给hdl_localization加上GPS

前期准备

GNSS RTK需要输出航向角有关数据，经过驱动发布/heading，航向角与RTK设备输出的协议有关。
以GSVTG为例，航向角以顺时针为+。

$GPVTG,<1>,T,<2>,M,<3>,N,<4>,K,<5>*hh

<1> 以真北为参考基准的地面航向（000~359度，前面的0也将被传输）
<2> 以磁北为参考基准的地面航向（000~359度，前面的0也将被传输）
<3> 地面速率（000.0~999.9节，前面的0也将被传输）
<4> 地面速率（0000.0~1851.8公里/小时，前面的0也将被传输）
<5> 模式指示（仅NMEA0183 3.00版本输出，A=自主定位，D=差分，E=估算，N=数据无效）

思路

比较简单：
1.在建图时候，需要保存建图初始位置的经纬高，以及航向角。
2.定位时载入提前保存的经纬高，以及航向角。因为建图初始位置就是原点，所以以这个点为基准，做utm变换。
3.程序订阅/fix与/heading，当match_error大于2时，进行重定位。类似于rviz直接给点。

缺陷

实习时使用的RTK海拔高度测量偏差较大。就默认z轴为0，与原点齐平。

github：https://github.com/z1047941150/hdl_localization_gps