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