1. 前言
在之前的笔记No.2 中，我们直接采用ndt_matching的方法实现定位，但需要在打开rviz中，通过2D Pose Estimate指定初始位置。加入GNSS后，可以帮助ndt_matching找到初始位置，同时如果ndt_matching在运动过程中匹配失败，GNSS可以帮助重定位。
在/ndt_matching算法中，有两个条件使用GNSS重定位：
(1) 如果设置ndt_matching的config中设置了GNSS，而非Initial Pos，那么ndt_matching会在程序初始运行中采用GNSS作为初值；
(2) 在车辆运行过程中，当topic /ndt_stat中的score值大于600（与地图匹配程度，值越大匹配的越差），同样会使用GNSS重定位。
下面介绍干货
2. GNSS读入数据配置
我们采用某品牌的GNSS设备，通过串口读入信息，所以首先在Autoware中设置为串口读入，如下图：
首先配置GNSS下的Serial GNSS的config，我们的串口GNSS设备的设备名称为/dev/ttyS0，波特率为115200，设置成功后，点击OK保存，然后点击Serial GNSS选项，这时会提示错误：
ERROR: cannot launch node of type [nmea_navsat_driver/nmea_topic_serial_reader]: nmea_navsat_driver ROS path [0]=/opt/ros/kinetic/share/ros ROS path [1]=/home/a/autoware/ros/src ROS path [2]=/home/a/catkin_ws/src ROS path [3]=/opt/ros/kinetic/share ERROR: cannot launch node of type [nmea_navsat_driver/nmea_topic_driver]: nmea_navsat_driver ROS path [0]=/opt/ros/kinetic/share/ros ROS path [1]=/home/a/autoware/ros/src ROS path [2]=/home/a/catkin_ws/src ROS path [3]=/opt/ros/kinetic/share
这时由于Autoware中的串口驱动没有装，根据autoware/ros/src/sensing/drivers/gnss/packages/nmea_navsat/scripts/README.md文件，需要下载驱动，运行
$ sudo apt-get install ros-kinetic-nmea-navsat-driver
如果不能更新，也可以从源码下载，然后在autoware的工作空间下安装，源码的下载地址为：https://github.com/ros-drivers/nmea_navsat_driver.git，下载后切换到适合的kinetic分支即可运行。
这时在勾选Serial GNSS选项，通过查看topic /nmea_sentence，如果有输出，且在sentence中有内容，即GNSS设备数据读取成功。/nmea_sentence的数据内容：
--- header: seq: 223 stamp: secs: 1568168594 nsecs: 571928024 frame_id: "/gps" sentence: "$×××××,0,7847.600,360.000,1.663,0.720,0.0000000,0.0000000,0.00,0.000,0.000,0.000,0.000,0,0,0C*0C"
$*****代表nmea的标准，主流的标准有$GPGGA、$GPGSA、$GPGSV、$GPRMC、$GPVTG、$GPGLL。而我们采用的传感器不是这些标准，所以需要自己写nmea的数据解析。
不同的GNSS设备，sentence的中内容的标准会有不同。
如果想了解nmea，请参考博客：https://blog.csdn.net/jickjiang/article/details/79086202
3. 数据解析
Autoware的数据解析程序为nmea2tfpose包，由于我们采用的设备不是标准协议，但我们的协议和$GPGGA很接近，所以我们采用在$GPGGA的基础上做了改进nmea2tfpose_core.cpp。
我们的修改思路为：
(1) 首先讲基站的经纬度作为原点；
(2) 采用$GPGGA的方式，将经纬度坐标系映射到以基站为原点的平面坐标系；
(3) 通过tf变换，将基站为原点的坐标系的转换到地图坐标系。
我们没有修改源码，而是将nmea2tfpose包拿出来修改，编译，然后单独发布。
nmea2tfpose包最终发布的topic为/gnss_pose。查看/gnss_pose与/ndt_pose发布的数据是否基本一致，如果基本一致，那么成功。这里我公布出部分源码，供大家参考（nmea2tfpose_core.cpp）：
// 设置基站经纬度 geo_.set_origin(longitude_, latitude_);
if (nmea.at(0) == "$×××××") //heading pitch roll { // 直接读取gnss中的roll, pitch, yaw position_time_ = stod(nmea.at(2)); roll_ = stod(nmea.at(5)) * M_PI / 180.; pitch_ = -1 * stod(nmea.at(4)) * M_PI / 180.; yaw_ = -stod(nmea.at(3)) * M_PI / 180. + M_PI*2; //ROS_INFO("roll:%f, pitch:%f, yaw:%f.", roll_,pitch_,yaw_); // 根据GPGGA计算从经纬度到局部坐标系的映射 double lat = stod(nmea.at(6)); double lon = stod(nmea.at(7)); double h = stod(nmea.at(8)); geo_.set_llh_nmea_degrees(lat, lon, h); }
// 基站为原点坐标系到地图坐标系的变换 tf::Pose tf_pose; tf::poseMsgToTF(pose.pose, tf_pose); tf::Quaternion q; q.setRPY(0, 0, 3.2277); // 基站为原点坐标系与地图坐标系夹角 tf::Transform tf_trans(q, tf::Vector3(-28.11, 10.79, 0)); // 基站为原点坐标系与地图坐标系原点的差 tf_pose = tf_trans.inverse() * tf_pose; tf::poseTFToMsg(tf_pose, pose.pose);
4. 结果
上面的代码完成后，可以按照Autoware 笔记4的内容进行设置，这时无论车停在轨迹上的任何位置，可以直接找到位置，无需2D Pose Estimate指定初始位置。