本文純屬轉載，并認真學習一遍，感謝大佬分享！
本文章配套源代碼位址：https://github.com/Little-Potato-1990/localization_in_auto_driving
測試資料：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg 提取碼: n9ys
本篇文章對應的代碼Tag為 3.0
代碼在後續可能會有調整，如和文章有出入，以實際代碼為準
========================================
一、概述
為了寫好我們這套定位系統，架構自然是首要考慮的事情，設計架構要結合需求和環境針對性地設計。
此處我們的環境就是ROS系統，而需求就是從我們上一篇文章 制作的bag檔案中接收各傳感器資訊和标定資訊，以供算法使用，在算法運算完成以後把結果發送出去。
是以我們架構的基本就包括接收子產品、發送子產品和與将來編寫的算法對接的接口。
接收子產品，具體包括 接收bag檔案中GNSS資訊、IMU資訊、雷達點雲資訊和各傳感器之間的标定資訊。
發送子產品，具體包括發送目前 點雲、全局地圖、局部地圖、裡程計資訊、載體運動軌迹等。
接口，具體就是要設計合理的資料結構，能夠把算法需要的輸入資訊和算法的輸出資訊條例合理地規劃好，以使輸入輸入輸出更清晰友善。
除了以上功能以外，我們還需要一些小的技巧，以使工程結構更清晰，檔案中代碼也更清晰。當然其中一些可能隻是我個人的一些使用習慣，各位如果有更好的習慣也歡迎交流。
本篇文章對應的工程包是工程檔案中的 lidar_localization
二、ROS工程設計
這一部分是本篇文章的重點，但是關于怎樣使用ROS建立一個簡單的工程包，這裡不做詳細介紹，網上的資料随處可見，了解不太多的讀者麻煩先看一些資料，抱歉。
為了使工程架構更符合我們的任務需求，我們對工程做了一些改動，此處重點要介紹的就是這些改動部分，這可能和大家經常見到的工程包的結構不一樣。
1. 消息的訂閱和釋出
消息的訂閱和釋出大家應該不會陌生，這是每個ROS工程都必備的東西，我們常見的使用方式是在main函數中定義subscriber和publisher，每個subscriber會有一個callback函數與之對應。
這種使用方式會帶來一些問題，那就是如果訂閱的topic比較多，那這個node檔案就會充斥大量的callback函數，而且如果有些資訊需要在callback内部做比較多的解析和處理，那這個node檔案的代碼長度會很長，這會影響程式的清晰度。
針對這個問題，我們把每一類資訊的訂閱和釋出封裝成一個類，它的callback做為類内函數存在，這樣我們在node檔案中想要訂閱這個消息的時候隻需要在初始化的時候定義一個類的對象，就可以在正常使用過程中從類内部直接取它的資料了。
這樣用文字說可能比較抽象，我們找其中一個訂閱類來舉例子。
就用訂閱GNSS資訊的例子好了，代碼中，它的頭檔案是 gnss_subscriber.h，源檔案是 gnss_subscriber.cpp。在頭檔案中，類的聲明如下
class GNSSSubscriber {
public:
GNSSSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size);
GNSSSubscriber() = default;
void ParseData(std::deque<GNSSData>& deque_gnss_data);
private:
void msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr);
private:
ros::NodeHandle nh_;
ros::Subscriber subscriber_;
std::deque<GNSSData> new_gnss_data_;
};
其中 msg_callback 就是它的 callback 函數，也就是接收和處理資訊的地方，它在 源檔案 中的實作如下
void GNSSSubscriber::msg_callback(const sensor_msgs::NavSatFixConstPtr& nav_sat_fix_ptr) {
GNSSData gnss_data;
gnss_data.time = nav_sat_fix_ptr->header.stamp.toSec();
gnss_data.latitude = nav_sat_fix_ptr->latitude;
gnss_data.longitude = nav_sat_fix_ptr->longitude;
gnss_data.altitude = nav_sat_fix_ptr->altitude;
gnss_data.status = nav_sat_fix_ptr->status.status;
gnss_data.service = nav_sat_fix_ptr->status.service;
new_gnss_data_.push_back(gnss_data);
}
類中的函數 ParseData 就是實作 從類裡取資料的功能，在源檔案中的實作如下
void GNSSSubscriber::ParseData(std::deque<GNSSData>& gnss_data_buff) {
if (new_gnss_data_.size() > 0) {
gnss_data_buff.insert(gnss_data_buff.end(), new_gnss_data_.begin(), new_gnss_data_.end());
new_gnss_data_.clear();
}
}
經過這樣的改造，我們在node檔案中使用它時，隻需要完成類對象定義和取資料兩步
// 定義類對象指針
std::shared_ptr<GNSSSubscriber> gnss_sub_ptr = std::make_shared<GNSSSubscriber>(nh, "/kitti/oxts/gps/fix", 1000000);
ros::Rate rate(100);
while (ros::ok()) {
ros::spinOnce();
//取資料
gnss_sub_ptr->ParseData(gnss_data_buff);
rate.sleep();
}
這樣node檔案中代碼量就會大大減少，使程式更清晰。
2. 傳感器資料結構
每種傳感器專門封裝了對應的資料結構，在sensor_data檔案夾下，目前有imu_data.hpp、gnss_data.hpp、cloud_data.hpp分别對應IMU資料、GNSS資料、點雲資料。
這種封裝就是為了适應一開始提到的接口功能，同時也可以配合第一步封裝的訂閱類和釋出類使用，把訂閱的資料直接封裝好再供主程式取，這樣封閉性更強。
3. 緩沖區機制 - 注意
這種機制完全是由于ROS自身的缺陷導緻的，而且我在以前的試驗中也多次遇到過這個問題。
這個問題和ROS訂閱資訊時緩沖區讀取有關，ROS在每次循環時，會逐個周遊各個subscriber的緩沖區，并且把緩沖區中的資料讀完，不管有多少。我們在subscriber的callback中解析資料的時候，一般都是把資料賦給一個變量，然後在融合的時候使用最後更新的值作為輸入。
如果覺得不好了解，我們使用僞代碼舉一個小例子，假如目前有雷達和GNSS資訊，我們要融合它。
gnss_callback {
gnss 資料解析，賦給變量 gnss_data
}
lidar_callback {
雷達資料解析，得到lidar_data
融合(lidar_data, gnss_data)
}
這樣看好像沒什麼問題，問題在于當融合算法處理時間比較長，超出了傳感器資訊的發送周期的時候，未被接收的資料會被放在每個subscriber對應的緩沖區中，等目前融合步驟處理完之後，下次ros從緩沖區中讀取資料的時候，會先把gnss的資料讀完，然後再讀lidar的資料，這就導緻，我們再一次進入lidar_callback函數時，使用的gnss_data已經不是和這個lidar_data同一時刻的資料了，而是它後面時刻的資料。--- 資料不同步，效果一定不好 ！
（不知道我有沒有講清楚，如果還沒講清楚就評論區留言吧）
為了解決這一問題，辦法也很簡單，就是我們不用單個變量來存儲資料，而是用容器。各位這時候可以去第一步看我們舉的那個GNSS資訊訂閱類的例子，在它的 msg_callback 函數裡，資訊解析完之後是放在一個 deque 容器裡的。
這樣算法再使用資料的時候，應該從容器中去找。隻不過找的時候要注意，多個傳感器産生了多個容器，往算法子產品裡輸入的時候，應該按照各容器第一個資料的時間戳，把最早的那個輸入送進去，循環這個過程，直到所有容器資料送完為止。
4. CMakeLists檔案規劃
這一部分介紹的都是一些小的使用習慣，我認為這些習慣可以使 CMakeLists 更清晰。
1）把各個包放在單獨的cmake檔案中
調用一個包，就是正常三步：find_package，include_directions，target_link_libraries
有時候還需要一些判斷，就加一些 if else。
這樣也是同樣的問題，包多的時候代碼太雜。是以我們把每個包對應的這些操作放在cmake檔案夾下對應的XX.cmake檔案中，然後在CMakeLists中 include(cmake/XX.cmake)一行代碼就可以搞定。
2）合并變量
為了避免target_link_libraries後面跟很長一串庫的名字，而且庫增減的時候它也得跟着增減，我們在CMakeLists檔案一開始定義一個變量
set(ALL_TARGET_LIBRARIES "")
然後在每個庫對應的XX.cmake檔案中，把庫的名字合并到這個變量中去
list(APPEND ALL_TARGET_LIBRARIES ${XX_LIBRARIES})
這樣在target_link_libraries使就隻使用ALL_TARGET_LIBRARIES這一個變量就好了。
除了庫對應的變量，還有檔案名字對應的變量，我們在add_executable的時候要把所需要的cpp檔案路徑都要寫進去，檔案多的時候也是太麻煩，是以可以使用下面的指令把所有cpp檔案合并到一個變量中
file(GLOB_RECURSE ALL_SRCS "*.cpp")
但是，當工程中有多個node檔案的時候，就要把他們從這個變量中踢出去，因為多個node檔案編到一個可執行檔案中會出錯。用下面的代碼踢
file(GLOB_RECURSE NODE_SRCS "src/*_node.cpp")
list(REMOVE_ITEM ALL_SRCS ${NODE_SRCS})
5. GLog使用
GLog是google開源的代碼日志開源庫，它把資訊分為INFO、WARNING、ERROR幾個等級，使用時如果想添加日志資訊，隻需要一行代碼就可以了
LOG(INFO) << "自定義日志資訊";
日志資訊會自動存儲在你定義的目錄中。
總之使用起來還是比直接使用std::cout要友善很多，我們就把它加入到這個工程裡面來了，麻煩各位裝一個吧。
三、一個小例程
啰嗦這麼多，我們要試一下剛才設計的這些東西好不好使，這個小功能就是在播放bag檔案的同時，把采集資料時車的軌迹實時顯示出來，并且把目前點雲也顯示在車目前的位置上。
基本思路就是訂閱 GNSS、IMU、lidar 資訊，然後把GNSS資訊中的位置、IMU資訊中的姿态資訊解析出來，然後用odometry釋出出去，把訂閱的點雲資訊按照解析的位姿資料轉換到目前車的位置和方向上去，再釋出出去。
該功能對應的 node 檔案是 test_frame_node.cpp，檔案内容很簡單，而且上面的介紹也零散提到一些，此處就不放代碼了。
編譯完成之後運作程式
roslaunch lidar_localization test_frame.launch
最終實作的效果應該是這樣的
這時候看到的就不是原地不動的點雲了，而是跟着車的運動前進的點雲，而且軌迹實時更新顯示。
最終bag播放完之後，軌迹效果是這樣的
熟悉的形狀，kitti的形狀
上一篇：從零開始做自動駕駛定位(二): 資料集
下一篇：從零開始做自動駕駛定位(四): 前端裡程計之初試