DWAPlannerROS是封裝類，提供了與move_base的介面，而DWAPlanner是具體實現類，它非常依賴costmap（當然不指望讓小車動態避障的話就無所謂了），因此我們在使用時需要保證代價地圖的膨脹度以及實時更新頻率。btw：在此類程式碼中，基本上下反覆使用的變數在函式形參中都是引用，實在放心不下還是看宣告吧～

move_base是規劃路徑與速度的大類，DWAPlannerROS提供給它的介面有兩個，setPlan與computeVelocityCommands。

一．setPlan負責獲取全域性路徑，DWAPlannerROS::setPlan獲取後，轉發給DWAPlanner::setPlan，恢復振盪標誌位再轉發給LocalPlannerUtil::setPlan，這樣層層疊疊的呼叫很有層次感，這樣每當產生一個新的全域性路徑都第一時間提供給全域性——區域性轉化以及剪裁功能使用。

二．computeVelocityCommands這個函式負責本次迴圈的下發速度的解算，它一上來就先確定小車當前在全域性中位置。

１．Costmap2DROS::getRobotPose獲取小車全域性座標與姿態。

if ( ! costmap_ros_->getRobotPose(current_pose_))
{
  ROS_ERROR("Could not get robot pose");
  return false;
}

 這個錯誤其實會出現得比較頻繁，導致小車不動或者抽搐。具體我們先看這個函式，雖說它位於Costmap2DROS類中，但是和costmap並沒有什麼關係，負責將小車自身的位姿用tf轉化為全域性位姿，在這裡出錯的原因很可能是transform_tolerance_設得太小，小車自身效能跟不上被迫報錯。try中丟擲的三個錯誤犯的概率不是太大。

２．LocalPlannerUtil::getLocalPlan將全域性路徑轉化到區域性路徑，位於base_local_planner包local_planner_util.cpp中。

該函式先呼叫base_local_planner::transformGlobalPlan，這是base_local_planner包中goal_functions.cpp中的函式，它將全域性路徑轉化為base_link下的路徑，這個檔案包括的是工具性的函式，DWA與trajectoryRollOut方法均會呼叫該檔案。

if(limits_.prune_plan) 
{
  base_local_planner::prunePlan(global_pose, transformed_plan, global_plan_);
}
 

 這是在根據小車當前位置裁剪前方一小段路徑，依然存放在transformed_plan中。limits_.prune_plan是可配置的引數，預設為true。

３．呼叫DWAPlanner::updatePlanAndLocalCosts，這個函式呼叫了MapGridCostFunction即根據柵格地圖產生一系列打分項，它們均呼叫了各自的介面setTargetPoses：

void apGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
  target_poses_ = target_poses;
}

４．呼叫LatchedStopRotateController::isPositionReached判斷是否到終點了，這也是base_local_planner中的功能包，這只是通過判斷終點和當前位置的算術距離，xy_goal_tolerance是可設定的引數，小於這個數就可認為到達了。若到終點了，呼叫LatchedStopRotateController::computeVelocityCommandsStopRotate函式，使小車旋轉至最終姿態；否則繼續呼叫dwaComputeVelocityCommands函式計算下發速度。如果有報錯：

ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");

 這大概率是dwaComputeVelocityCommands中代價地圖出現異常，導致函式返回false：

if(path.cost_ < 0)
{
  ROS_DEBUG_NAMED("dwa_local_planner",
     "The dwa local planner failed to find a valid plan, cost functions discarded all candidates. This can mean there is an obstacle too close to the robot.");
  local_plan.clear();
  publishLocalPlan(local_plan);
  return false;  
}

如果該函式沒有問題，則move_base將會得到計算後的cmd_vel。

三．dwaComputeVelocityCommands這個函式很有意思，它的形參是一個tf變換和一個待處理的速度cmd_vel，這個速度是從computeVelocityCommands函式中一脈相承過來的。

１．用base_local_planner::OdometryHelperRos的物件odom_helper_來讀取里程計讀取的目前小車位姿global_pose；

（１）預備知識１：base_local_planner::OdometryHelperRos位於base_local_planner包內，接收base_controller傳出的odom話　　　題，將話題中的twist部分轉化為tf變換（robot_vel），這一步是為了在dwa中的findBestPath函式中使用global_vel形參。

２．dp_是指向DWAPlanner類的shared_ptr，呼叫DWAPlanner::findBestPath函式，在這個函式裡變數字尾是costs_的，都是打分項，例如該函式第一句：

obstacle_costs_.setFootprint(footprint_spec);

這就是呼叫了打分項——避障打分，這個函式呼叫ObstacleCostFunction::setFootprint函式，具體在對base_local_planner的閱讀中再分析過，在這裡對避障打分類進行初始化。接著是對當前位姿、速度與終點位姿轉化為矩陣，並用SimpleTrajectoryGenerator對generator_初始化，創造路徑的取樣空間。接下來：

std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

這是用base_local_planner::SimpleScoredSamplingPlanner的物件，對所有可能軌跡進行遍歷。dwa演算法的實現與base_local_planner包緊密相關，所以兩個包需要結合一起閱讀～接著在all_explored這個vector中遍歷，若找到cost_>=0的軌跡(即我們需要的軌跡），則將pt（base_local_planner::MapGridCostPoint的物件）放入traj_cloud_中。（這是要釋出一個由base_local_planner::MapGridCostPoint組成的topic？再發佈一個視覺化的代價給rviz？）

隨後將drive_velocities設定好（這是關鍵點，因為實際上cmd_vel的數值來源就是該引數），並返回一個最佳路徑傳回dwaComputeVelocityCommands。

３．把drive_cmds（即findBestPath中的drive_velocities）引數賦予cmd_vel，這就是我們需要的下發速度。並且把base_local_planner::Trajectory格式的path轉化成nav_msgs/Path，呼叫publishLocalPlan函式。在此需要注意：

costmap_ros_->getGlobalFrameID()

區域性軌跡必須與代價地圖處於同一frameID下。

publishLocalPlan函式也是goal_functions.cpp中的函式。

整個流程非常漫長、繁雜，很令人抓狂。但是把函式呼叫流程梳理一遍，可以準確定位錯誤來源，也很方便改寫～