AMCL源碼閱讀（ros2版本）

閱讀amcl_ros2版本源碼

源碼git位址：https://github.com/ros2/navigation.git

本人注釋的git位址：https://github.com/adayimaxiga/cartographer_migrate_to_ros2/tree/master/amcl

這個amcl執行過程與ros1版本沒啥差別

首先閱讀amcl_node.cpp

主函數調用這一句

amcl_node_ptr.reset(new AmclNode(node, use_map_topic));       //這裡構造了AMCL類，調用了AMCL構造函數，所有程式入口
           

這個檔案結構比較清晰，主函數中調用AmclNode構造函數，這個函數主要幹了這麼幾件事：讀取參數，初始化位置，釋出/訂閱話題（釋出話題：amcl_pose，particlecloud）（訂閱話題：initialpose，map）（服務：global_localization，request_nomotion_update，set_map）調用關系大概是這樣的：

最關鍵的一個函數是雷射雷達話題的回調函數，所有算法在這裡運作。來看laserReceived函數都在幹啥：

1. 首先擷取了laser相對于baselink的tf
2. 然後擷取了baselink相對于odom的tf，這樣就知道laser在世界坐标系下的位置可以繼續進行比對了。
3. 接下來分為兩種情況，pf是否第一次運作，如果是第一次就将裡程計值賦給pf_odom_pose_。如果是正常疊代過程，則計算裡程計變化值，并判斷裡程計變化是不是達到了我們要進行更新的門檻值。
4. 裡程計運動達到更新門檻值即調用UpdateAction()函數，打開該函數，這裡可以看出來是根據設定的運動預測模型進行采樣，這就是粒子濾波的預測模型了。
5. 然後回到laserReceived函數中，讀取了雷射資料後，将最大範圍與最小範圍傳給amcl_laser。然後由于amcl算法并未對最小距離做處理，是以如果小于range_min就指派為range_max做與max同樣處理。之後處理好的資料調用UpdateSensor，将雷射資料上傳，我們打開這個函數
6. UpdateSensor對于雷射資料處理，根據模型不同調用了pf_update_sensor，一般情況下，雷射觀測模型為LikelihoodFieldModel這個經典模型。這裡根據似然域來做更新。
7. 看一下LikelihoodFieldModel這個函數，這裡我了解的是由于裡程計運動大于一定值才進行預測及更新，這裡會取與裡程計時間相近的一次雷射，模型裡計算流程大概是：對于每一個粒子，首先計算雷射坐标系，然後濾去大于max_range的雷射點（這裡由于上一步的處理，連min_range一起濾掉了），濾去not a number,計算每一束雷射落點栅格，計算落點栅格與地圖上障礙點最近曼哈頓距離，根據雷射測距的高斯模型機率，每個粒子權重為每一束雷射單獨掃描的乘積，再計算總權重。這裡是雷射觀測模型的計算流程。
8. 回到laserReceived函數中，這裡完成更新部分之後進行重采樣，重采樣函數為pf_update_resample()函數。重采樣的方法比較經典，根據w值增加了一部分随機生成的粒子，當drand48<w_diff，随機均勻在地圖範圍内産生一個粒子。這裡是調用uniformPoseGenerator産生的。而drand48>w_diff時，将PDF積分得到CDF，然後drand48來對應離散CDF中采樣，這樣drand48落在哪個點上就把哪個粒子複制一份，将所有粒子權重置為1，完成重采樣。然後對每個粒子權重進行歸一化，重置fast和slow，重新計算統計資料（均值，方差）
9. 回到laserReceived函數中，最後一步是釋出計算好的點雲資料。