雖然我還沒有物件，但已不知面向了多少物件

——沃.茲基碩德

全域規劃器的理論實作

上次寫到宕開一筆去看move_base原始碼，死磕后也勉強是理解了，但是效率低效果差，屬于是筋疲力盡且無效果，所以最開始如果想先看move_base的話，只需要了解一下類的繼承關系和主要函式功能及執行順序即可，

1. move_base實作流程

首先，move_base是什么？可以從幾個不同的觀點來看：

1. 從最后結果來看，它是ROS中的一個節點：作為ROS中的一個節點，提供各種話題和服務；
2. 從代碼上來看，他是一個C++C類及其實體化：move_base功能的實作都是基于Movebase類的實體化來完成的，理解這個類的性質和功能也就理解了move_base的性能；
3. 從實作原理來看：它是ROS框架下一個actionServer，在回呼函式中執行各項供能；

因此對move_base的分析也可以從三點，實際上是兩點即供能和實作，功能上篇已經分析過了，而實作則要從原始碼開始
該部分的原始碼大概分如下幾部分：

• 加載幾個yaml中的引數并初始化節點和各個功能的發布者、訂閱者；
• 初始化ActionSsrver和區域、全域規劃器及代價地圖；
• 初始化action回呼函式執行緒
• 等待接受goal，接收到后進入回呼函式；
• 開啟全域路徑規劃執行緒，并在該執行緒中執行全域路徑規劃函式；
• 執行完全域路徑規劃執行緒后再次回到action回呼函式；
• 將規劃好的全域路徑傳入區域規劃函式，完成區域避障、結束判斷等功能，并在該函式中輸出cmd_vel指令；

大致來說就是以上幾步，當然我做了一些簡化省略了規則檢查、坐標變換、搶占機制、和區域規劃函式中繁雜的判斷實作，如果想理解規劃器的呼叫流程，理解到這里就夠了，如果想深入了解：
ROS Navigation之move_base完全詳解
原始碼分析系列
原始碼分析（側重流程）

2. move_base與規劃器聯系

實作上述流程涉及到一些關鍵的類成員函式，具體如下：

• MoveBase::executeCb()：整個move_base節點的回呼函式，接收到目標后再該函式內回圈執行，直到到達目標點或者出現意外中止；
• MoveBase::planThread()：路徑規劃執行緒（本質也是一個函式），嵌套在executeCb函式中，當 runPlanner_ = true時該執行緒開始執行；
• MoveBase::makePlan()：路徑規劃函式，嵌套在planThread()中；在該函式內呼叫全域路徑規劃器，最后輸出一條全域規劃路徑；
• MoveBase::executeCycle()：區域規劃函式，該函式的邏輯最為復雜我一時也沒太明白，只能大概能理解其在executeCb()函式中，在路徑規劃執行緒結束后開始回圈執行，傳入引數為全域路徑和目標點坐標，在函式內呼叫區域路徑規劃器實作區域避障、速度指令生成和結束判斷功能；
• MoveBase::reconfigureCB()：動態引數配置；

1. 創建和初始化global planner
 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
 planner_ = bgp_loader_.createInstance(global_planner);

//使用的planner為 global_planner/GlobalPlanner
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);

2  創建和初始化local planner
 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;
 tc_ = blp_loader_.createInstance(local_planner);
 
 //使用的planner為teb_local_planner/TebLocalPlannerROS
 tc_->initialize(blp_loader_.getName(local_planner), &tf_,  controller_costmap_ros_);
 4 創建并啟動global planner執行緒
 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
 
 5 創建一個action server：MoveBaseActionServer，
 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base",    boost::bind(&MoveBase::executeCb, this, _1), false);

到現在我們大概已經理解了兩個路徑規劃器在move_base中的呼叫流程，下一步就是理解規劃器的具體功能和實作了；

3. Plugin和nav_core

ROS中采用插件機制來實作路徑規劃器功能，實作該功能有一套標準的流程：ROS中的插件plugin機制，該部分內容在此博客中有較為詳細的描寫，總的來說分為以下幾點：

• 創建基類，定義了插件應該實作哪些功能；
• 定義功能類，此類繼承了基類，面向具體型別實作基類中定義的功能；
• 同時在功能類對應的cpp檔案中，我們需要申明注冊創建好的插件；
• 之后需要將插件編譯成為lib檔案，并注冊到ROS中，使得catkin系統能夠直接查找到對應的元件；
• 最后，在應用的時候，我們需要用到ROS中pluginlib函式庫，創建一個ClassLoader，用來加載plugin；

具體到move_base中，基類即nav_core，在該文章中有詳細的介紹，該基類中的成員函式對應兩個路徑規劃器的功能；然后基于該基類繼承除了兩種全域規劃器：
global_planner: 一個快速的，內插值的路徑規劃器，其能更靈活地代替navfn；
navfn: 一個基于柵格的全域路徑規劃器，利用導航函式來計算路徑；
該程序的實作與上文插件流程一樣，具體見此：ROS: global_planner 整體決議

4. 兩個路徑規劃器實作的功能

看完move_base原始碼在看nav_core這個基類，真的可以說是如天堂一樣，沒有執行緒、沒有邏輯判斷甚至沒有具體實作，只有簡簡單單的功能描述，直接淚目啊喂，言歸正傳，nav_cove這個基類最主要的內容就是提供了插件功能的介面，換種說法來講它定義了兩個規劃器和一個恢復插件的基本功能：

• base_global_planner.h：實作引數初始化和全域路徑規劃的功能；
• base_local_planner.h：引數初始化、區域軌跡規劃、判斷是否到達目標點、由區域軌跡計算出速度指令；

這篇文章主要分享全域規劃器的實作流程，以GlobalPlanner規劃器為例

5. 全域規劃器的實作流程

功能類較基類相比內容要擴展很多，具體到GlobalPlanner規劃器，其核心是planner_core類，除此之外還有一些輔助類，這些類相互繼承、呼叫構成了很復雜的一套代碼，具體簡要介紹如下：

• planner_node.cpp：規劃器的入口，在此實體化了Global_planner類；
• planner_core.h：規劃器的功能類，此類繼承nav_core，在該類內完成規劃器的主要功能；
• expandar.h：相當于另一個插件的基類，A*和D演算法的規劃部分的類都繼承此類來實作；
• astar.h：繼承Expandar類，實作A*演算法的搜索程序；
• dijkstra.h：繼承Expandar類，實作D演算法的搜索程序；
• gradient_path.h：梯度下降法檢索路徑實作的類，實作梯度下降法在啟發式演算法結束后檢索出一條路徑；
• grid_path.h：柵格法檢索路徑實作的類，實作柵格法法在啟發式演算法結束后檢索出一條路徑；
• potential_calculator.h：檢索器類，在astar.h和dijkstra.h兩個類中作為搜索器傳入，作為其中的一個成員變數；
• quadratic_calculator.h：作用同上，且和上個類一樣，在功能類初始化階段·就被選擇；；

基于以上幾個類來實作全域規劃的功能，按照原始碼中的嵌套關系大概分為以下流程（以A*為例）：

1. 合法性檢驗和預處理，在planner_core.h定義的類中實作，同時也加載ros_param，并通過邏輯判斷選擇規劃器、路徑檢索方式和路徑擬合方式：
2. 節點檢索：在像素坐標系內開始搜索（每一個像素為一個開啟點），輸出一個px*py長度的一維陣列，存放最后各個節點的開啟狀態，該部分在astar.h或者dijkstra.h定義的類（繼承Expandar）內完成，入口函式為GlobalPlanner::makePlan函式中：

bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
                                                    nx * ny * 2, potential_array_);

3. 路徑搜索：呼叫柵格法或梯度下降法處理上一步中生成的陣列，輸出為一個陣列，其元素為（float，float）分別存放路點的x和y坐標，該部分在兩個搜索類中的一個完成（視引數決定），入口在GlobalPlanner::makePlan函式：

getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan))

//更為準確來說，是這個函式里面嵌套的：
path_maker_->getPath(potential_array_, start_x, start_y, goal_x, goal_y, path)

4. 路徑發布：將路徑轉化為geometry_msgs::PoseStamped 格式后結束規劃器；

全域規劃引數和基本介紹
global_planner原始碼閱讀筆記
基于A在navugation中添加自己的演算法
淺談路徑規劃演算法（轉載）
ROS中Navigation功能包里路徑規劃A演算法詳解

全域規劃器的具體實作

基于之前的分析，我們大概能推斷出全域規劃器中各個類的呼叫順序：
即先啟動GlobalPlanner這個核心類；

在完成初始化后再該類內呼叫AStarExpansion或DijkstraExpansion這個類來實作節點檢索的工能，這兩個類繼承Expander這個類，同時Expander類的拷貝建構式初始化引數時傳入PotentialCalculator這個類作為成員變數；

最后呼叫兩個路徑搜索類來實作節點串列陣列到路點陣列的轉換；

各個類之間存在著極為復雜的繼承與呼叫關系，因此如果想基于GlobalPlanner的原始碼進行更改，最好不要動函式結構比如增改傳入引數之類的，建議還是按照原來的流程走哪怕你的演算法并不需要這么多步驟；如果實在需要更改傳入引數，建議做函式多載，這樣不會打亂繼承和呼叫的關系；

規劃器更改心得

1. 仿照Astar.h和Astar.cpp建立自己的規劃方式類（其實算不上完整的規劃，因為這個類只是在像素空間操作的），并更改Cmakelist；
2. 在GlobalPlanner類初始化部分將規劃器定向到自定義的規劃器；
3. 仿照GridPath類定義相應的路徑搜索類，并更改CmakeList；
4. 在GlobalPlanner類初始化部分將搜索器定向到自定義的；

照這個步驟做下來基本是沒問題的，反正最起碼我這是沒問題的，能不能做成就看命運了，另外除了嵌套關系復雜外函式傳參和各個類的全域變數定義也都不一樣，所以強烈不建議大幅度更改，就做一個擴展就得了

另一種全域規劃器的定義方法是參照carrot_planner，從頭到尾按照ros::Plugin 的定義流程做，但這樣做想避障還需要對Cost_map有深入了解
移動機器人——自定義規劃演算法

長大后，我們閉口不談夢想

夢想很貴也很難，就像你對著一道題啃了一下午只識訓了焦頭爛額；
也像那個價格離譜到讓你望而生畏的驅動板或者電機；
或是一次次很累但什么都沒有做到的嘗試，

后來我們不再談及夢想，只談近況；不想太遠，只是一步一步向前走，