ROS導航包源碼學習5 --- 局部規劃

局部導航可以說是整個導航包最為複雜的一個包了，base_lcoal_planner頭文件30個，代碼近1萬行。從wiki上可以看到，局部導航主要包括Trajectory Rollout 和 Dynamic Window Approach (DWA)兩種方法，理論上來說分別對應base_local_planner和dwa_local_planner兩個包，但其實dwa的大部分代碼都放在了base_local_planner包裡面。

框架

同樣，對於這種大工程，我們還是從類繼承圖來著手：

TrajectoryCostFunction，這個介面主要規定了一個scoreTrajectory函數，也就是走過一個軌跡需要付出的代價，在wiki上可以看到對4種代價的詳細介紹；TrajectorySampleGenerator明顯是產生一系列軌跡用的，介面規定了nextTrajectory用來遍歷所有軌跡；TrajectorySearch介面規定了一個findBestTrajectory，找到最好的軌跡；worldModel，ros注釋裡面有一句介紹：Subclass will implement this method to check a footprint at a given position and orientation for legality in the world。

以上介面是局部規劃的核心，簡單來說，TrajectorySampleGenerator產生一系列軌跡，然後TrajectoryCostFunction遍歷軌跡打分，TrajectorySearch找到最好的軌跡拿來給小車導航；由於小車不是一個質點，worldModel會檢查小車有沒有碰到障礙物。

剩餘的類

仔細觀察include目錄發現，除了前面提到的這些類，還有很多類沒有覆蓋到。排除掉wiki中提到的helper class，剩下有兩個類非常顯眼：trajectory_planner跟trajectory_planner_ros。稍微瀏覽了一下這兩個類的內容，驚了，這兩個類似乎自成一系，像前面提到的產生軌跡 軌跡打分 選最好軌跡都有相應的函數。這是怎麼一回事呢？為什麼前面定義了半天的介面又不用了呢？

用vscode的全局搜索發現，原來這些介面根本就沒在base_lcoal_planner裡面用，而是在dwa_local_planner這個包裡面使用。聯繫wiki裡面提到的兩種方法，我好像明白了其中的原因，原來，事情並不是這麼簡單。

這個trajectory_planner很可能是一開始ros設計的一個局部規劃器，可是後來發現這樣寫成一個大類不利於擴展。於是，ros重新採用介面化的設計思路，將整個規劃器分為好幾個介面重新設計了一個dwa_local_planner，並且不知道為啥不放到dwa_local_planner里，而是跟之前的base_local_planner放一起了。於是，在這個包里明顯感覺到這兩部分是格格不入的。

好了，現在大局已定，整個框架算是清楚了（每次都是先把框架弄清楚），下面深入看看各個介面下的具體實現。

TrajectoryCostFunction

TrajectoryCostFunction可是說是最重要的一個介面。MapGridCostFunction用來評估局部規劃的軌跡離全局規劃的軌跡的距離也可以用來評估到目標的距離，它維護了一個base_local_planner::MapGrid map_，MapGrid是一系列MapCell，而MapCell包含了一個int target_dist，也就是說，MapGridCostFunction建立後隨時知道地圖上一個點到全局規劃軌跡的距離，或者是到目標的距離。具體是在computeTargetDistance中實現的，實現的方法跟之前amcl包維護地圖網格離最近障礙物的距離用到的方法一樣，也是用隊列實現了一格格膨脹的效果（或者叫波動，wave propagation）。在dwa_local_planner里建立了4個mapGrid用來計算4個不同的方面，具體的內容如下：

ObstacleCostFunction就是計算小車在costmap上行走的代價，看看是否撞到障礙。其中ObstacleCostFunction用到了worldModel介面來檢測底座是不是撞到障礙物，從初始化函數可以看出來，ObstacleCostFunction選用了CostmapModel作為具體實現，想改為其他具體實現隨時可以改這一行。

後面兩個costFunction就沒什麼內容了，一個是判斷小車是不是老是震動，一個是判斷小車會不會倒著走。在具體的規劃中，想用哪個就把哪個加進去就得了，非常方便，dwa_local_planer就preferForwardCostFunction沒用，其他都用到了。

SimpleTrajectoryGenerator

SimpleTrajectoryGenerator這個類就是根據加速度的限制產生一系列軌跡（軌跡就是速度跟點的集合，在trajectory.h中定義），以供後面打分挑選。比如當前速度是5，允許的速度範圍是0-10，那麼這個類會產生10個目標速度值，然後根據加速度限制將當前速度盡量加速或減速到目標值，持續一定的步長，並記錄生成的所有軌跡。可見這個軌跡生成類也算是人如其名，too simple。

SimpleScoredSamplingPlanner

SimpleScoredSamplingPlanner這個類目的很明確，就是打分，找到最好的軌跡。看它的兩個變數就知道它要幹嘛了：

private: std::vector<TrajectorySampleGenerator*> gen_list_; std::vector<TrajectoryCostFunction*> critics_;

產生一系列軌跡，然後用一系列costFunction打分加起來，選最好的那個。這裡的軌跡生成器雖然是一個list，但是其實在dwa_local_planner就放了一個進去。costFunction放了6個。

總結

worldModel除了costmapModel在obstacle_cost_function裡面用到之外，其他兩個並沒用到，所以這裡就不看了。回顧整個工程，可以發現這種介面化的設計有種說不出的美感，每一步的實現都具體踏實，調用又簡潔可靠，將一個複雜的項目簡化到遊刃有餘的地步，實在厲害。原來的非介面的trajectory_planner就不看了，有了電驢還要啥自行車（主要因為懶）。

推薦閱讀：