ROS導航包源碼學習4 --- 全局規劃

04-27

好了，前面研究了這麼多，如果你還沒忘記包的名字的話會發現：我們的目的是導航啊，這咋還沒開始呢？好吧，只能說現在才進入導航的核心---規劃。

導航包里有一個nav_core，是一個純粹的介面，裡面規定了核心的幾個類應該有的樣子。BaseGlobalPlanner是全局導航的介面，規定了一個功能makePlan，也就是給定起始跟目標，輸出一系列pose走過去；BaseLocalPlanner規定了一個核心功能computeVelocityCommands，就是計算局部地圖下一步的控制指令；還有一個RecoveryBehavior，規定一個runBehavior，也就是小車卡住情況下如何運動恢復到正常的導航。

除去amcl包跟costmap_2d，剩下的基本都是實現這三個核心功能的代碼，加起來估計有三萬多。也許你要問了，為啥就這麼簡單的三個功能要這麼多代碼。其實這裡面有兩個因素，一是歷史遺留，以前全局規劃用的navfn包，局部規劃用的TrajectoryPlanner，新的實現為了更好的效果，遵循插件化、模塊化、介面化的原則重新寫了這些規劃器，以前的規劃也留在了navigation stack里（讀代碼的時候明顯感覺有兩種風格）；二是因為新的介面化設計原則讓代碼變多了，但其實讀起來是更輕鬆的。我們的原則是儘可能懶，所以舊的代碼就沒必要看了，我們先來看global_planner包吧。

global_planner插件化設計的脈絡清晰可見，以下三張圖可以說明：

首先，作為BaseGlobalPlanner的具體實現，自然要繼承BaseGlobalPlanner；expander這個就不用解釋了吧，看到A* D...就懂了，這種設計可以實現隨時替換核心演算法；traceback是不同的軌跡類型，在global_planner - ROS Wiki上可以直觀的看到區別；另外在global_planner的include目錄里，除了這些還有兩個calculator，這個在wiki上也提到了，「Slightly different calculation for the potential」。還有一個OrientationFilter，這個可以理解為給path「順毛」，因為A*演算法規劃的path只能保證路線的連續性，還要保證拐彎的角度別變得太快。

可以看到，這種介面化的設計能讓我們不用讀幾行代碼就能掌握整個框架，有一種什麼感覺呢，嗯，我覺得庖丁解牛這個詞特別貼切。

expander規定了calculatePotentials的介面，根據costmap跟起始終止點計算網格的potential。關於演算法，D..就不看了，A*比D...要好。A*演算法youtube(A*)上有個視頻講解地特別棒，這裡我就不多說了。

Traceback規定了getpath介面，根據potential把路徑找出來，輸出一系列軌跡點（std::vector<std::pair<float, float> >& path）。軌跡點找出來就好辦了，在GlobalPlanner::makePlan里調用GlobalPlanner::getPlanFromPotential，把path簡單轉換下變成std::vector<geometry_msgs::PoseStamped>& plan，然後makePlan再用OrientationFilter順下毛，最後發布出去。

可以看到，全局規劃還是很簡單的，ros設計的也非常合理，容易理解。