Autoware 笔记 No. 5——基于GNSS的定位

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指定初始位置。