base_local_planner是直接決策小車以何種軌跡執行、下發何種速度的大類，可能由於開發的歷史原因，下面的檔案全部放在這個包裡，顯得有些雜亂，咱們慢慢捋一捋。

首先看看navigation包的nav_core包，nav_core是一個純虛類，裡面對base_local_planner進行了父類的虛擬函式宣告，它使用預設建構函式，initialize函式接受名字、tf、代價地圖進行“構造”，另外三個介面分別是setPlan（用來將全域性路徑傳到區域性規劃器），computeVelocityCommands（計算下發速度），以及isGoalReached（判斷本次導航是否結束）。這就是base_local_planner虛類的宣告。那麼來繼承這個類的，預設是DWAPlannerROS和TrajectoryPlannerROS以pluginlib的形式繼承，而我們一般使用ＤＷＡ演算法進行速度計算而不是。dwa_local_planner已經作為一個獨立的包放在外面，這一塊就不再贅述。

我們從ＤＷＡ演算法開始，依次看看呼叫的base_local_planner的函式流程：

一、從DWAPlannerROS::computeVelocityCommands開始：

１．呼叫了getLocalPlan函式，這個函式是LocalPlannerUtil::getLocalPlan，這個類的作用大概是對發過來的全域性路徑進行tf上的轉化以及裁剪，它依次呼叫的兩個函式均位於goal_functions.cpp中，這個檔案下全是工具函式，不從屬於類。下列均為工具函式：

（１）publishPlan這個函式是將區域性路徑發給rviz，我們在rviz中可以同時觀看兩個nav_msgs/Path格式的topic（全域性、區域性）。

（２）prunePlan將全域性路徑裁剪成前方一小段區域性路徑，採用的方法是用迭代器依次判斷與當前位置的距離，小於某距離時跳出迭代器迴圈，將路徑其他部分剔除。這個方法非常暴力，預設的前方距離是１m，這個數字不是可配置的rosparam，讓人費解。

（３）transformGlobalPlan是利用計算區域性代價地圖的範圍，將範圍之外的剔除，在區域性代價地圖內的路徑點的frame轉化為global_frame。

double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
                                       costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
 

（４）getGoalPose是將全域性路徑的最後一個元素取出，利用tf中的poseStampedMsgToTF獲取goal_pose的各引數，tf的具體操作方法我們之後再分析。

（５）isGoalReached是判斷兩個因素，一個是位置是否足夠接近終點（容忍度之內），一個是車是否停下（按照里程計反饋）。

２．回到computeVelocityCommands，它接著呼叫了DWAPlanner::updatePlanAndLocalCosts函式，而這個函式是依靠那幾個打分項活著的，檢查一下就可以發現該函式中出現的打分項有：path_costs_，goal_costs_，goal_front_costs_，alignment_costs_，一直在圍繞這些打分項做文章。這幾個打分項，其實都屬於base_local_planner::MapGridCostFunction類。轉到map_grid_cost_function.cpp，這裡詳細講述了打分前的準備工作。

（１）在updatePlanAndLocalCosts函式中呼叫的setTargetPoses介面其實就是將外部處理好的區域性路徑傳到成員變數而已，其他什麼都不做。

（２）其他成員函式都呼叫了MapGrid的物件，先轉到map_grid.cpp看看它的實現方式：

①MapGrid反覆使用了裝有MapCell的vector，再轉到map_cell.cpp（這呼叫層級有點厚）～

private:

  std::vector<MapCell> map_; ///< @brief Storage for the MapCells

②MapCell這個類是真的皮，預設建構函式和複製建構函式啥都不做，但是初始化列表裡說明了地圖體素距小車無限遠，暫時先這麼初始化吧。

MapCell::MapCell()
    : cx(0), cy(0),
      target_dist(DBL_MAX),
      target_mark(false),
      within_robot(false)
  {}

③回到MapGrid類，commonInit函式是將整個柵格地圖初始化（其實就是將每個體素塞進vector都標好位置），而getIndex函式就正好是找到vector的下標。sizeCheck函式是檢查柵格地圖是否填充正確，不對就再來一遍。

④MapGrid::updatePathCell是為了檢查小車前方的check_cell和目前位於的current_cell的距離，如果檢查出這個cell是障礙物那麼距離設為障礙物的型別。

unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
   (cost == costmap_2d::LETHAL_OBSTACLE ||
    cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
    cost == costmap_2d::NO_INFORMATION))
{
  check_cell->target_dist = obstacleCosts();
  return false;
}

⑤MapGrid::adjustPlanResolution是為了根據地圖以及代價地圖的解析度對全域性路徑進行填充，防止路徑點過於稀疏。雖然函式看起來很花哨，但是仔細看就很容易發現意圖。

⑥MapGrid::setTargetCells是為了將global_plan經過的cells儲存佇列path_dist_queue中，這個佇列每當經過一個體素（隊頭）就刪除之，把新的體素塞在隊尾。MapGrid::setLocalGoal函式也類似，用來判斷小車下一時刻將通過哪個體素。

⑦MapGrid::computeTargetDistance函式是這個類的關鍵，它把區域性地圖裡的所有體素均和一個佇列中的元素（其實是路徑點）進行距離計算，但是沒有返回任何物件，有點費解。。。

（３）回到map_grid_cost_function.cpp，很多函式就容易理解了。MapGridCostFunction::scoreTrajectory是為了判斷路徑點前方的體素的評分，如果有錯誤返回負數。一個需要注意的點是，aggregationType_是一個列舉，它分別代表返回最後一點的值（如果沒有處於碰撞危險中）、所有點的和，或者所有非零點的乘積。

３．那麼再回到DWAPlannerROS::computeVelocityCommands，接下來呼叫了LatchedStopRotateController的物件，轉到latched_stop_rotate_controller.cpp分析它的實現：

（１）LatchedStopRotateController::stopWithAccLimits以及rotateToGoal中，加速度的上下限是可配置引數，它們是在一個模擬時間週期內計算出所需的線速度與角速度。

（２）至於具體實現的computeVelocityCommandsStopRotate，是通過里程計讀數的類物件odom_helper_的幫助來判斷小車是否停止旋轉。

①轉到odom_helper_ros.cpp，可以看到它是很簡單的通過接收base_controller的里程計讀數，值得注意的是訊息佇列只有一位，這意味著收一個就扔一個～

odom_sub_ = gn.subscribe<nav_msgs::Odometry>( odom_topic_, 1, boost::bind( &OdometryHelperRos::odomCallback, this, _1 ));

二、接著從computeVelocityCommands呼叫DWAPlannerROS::dwaComputeVelocityCommands，再接著呼叫DWAPlanner::findBestPath：

（１）obstacle_costs_是ObstacleCostFunction的物件，進入obstacle_cost_function.cpp，發現它的建構函式上來就要構建一個costmapModel，不明覺厲啊，趕緊轉到costmap_model.cpp：

①在標頭檔案中，我們可以看到footprintCost函式，看過costmap層級定義的朋友明白這些形參其實就是指小車位置、車輪數（圓形或方形）、內切圓半徑、外接圓半徑。

virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
          double inscribed_radius, double circumscribed_radius);

 而這個函式作用是判斷小車模型（圓或者方）最大可能的地圖代價。

②CostmapModel::lineCost是用一條線來檢查小車輪廓邊緣是否侵犯了代價地圖。

（２）ObstacleCostFunction::scoreTrajectory和前面的MapGrid類似，都是錯誤返回負數，並且正確情況分為最後一點的值與所有點的和。getScalingFactor是為了限制小車速度太大。

（３）回到DWAPlanner::findBestPath，設定好obstacle_costs之後，用一個SimpleTrajectoryGenerator初始化計算函式，轉到simple_trajectory_generator.cpp：

①SimpleTrajectoryGenerator::initialise其實就是藉助可配置引數，設定一個可選速度區間，用std::vector<Eigen::Vector3f>儲存。

②SimpleTrajectoryGenerator::generateTrajectory創造了一個時間微分，通過位置與速度計算函式，得到新的軌跡。這時候看一下trajectory.cpp，發現base_local_planner::Trajectory其實就是由三個vector組成，分別儲存X，Y和點之間的夾角。

③SimpleTrajectoryGenerator::computeNewPositions和computeNewVelocities函式均是通過速度區間求解下一個速度與位置。

（４）回到DWAPlanner::findBestPath，接著是SimpleScoredSamplingPlanner類物件scored_sampling_planner_，轉到simple_scored_sampling_planner.cpp，findBestTrajectory是對外呼叫的介面，它通過遍歷all_explored這個儲存Trajectory的vector來尋找最優解，那麼評分的標準在SimpleScoredSamplingPlanner::scoreTrajectory中：

①scoreTrajectory在軌跡上執行所有評分函式，建立正成本的加權和，一旦發現負成本或負成本之和大於正的._traj_cost累積值，就中止。

三、執行到這裡，結果就很明確了，由於result_traj_是由三個vector組成，那分別取其中的xv_，yv_，thetav_，就可以得到本次週期所需要的速度了。當然用一個tf::Pose來返回這個cmd_vel的數值，也是很費解了，明明可以用一個matrix更便於理解，當然可能穩定性稍差。

這就是move_base——dwa_local_planner——base_local_planner的流程，用多層級、分散式的軟體架構實現了導航演算法～