1 體系結構
（1）主要成員
base_local_planner::LocalPlannerUtil planner_util_; 用來存儲運動控制參數以及costmap2d、tf等，會被傳入dp_
costmap_2d::Costmap2DROS* costmap_ros_;
base_local_planner::OdometryHelperRos odom_helper_; 用來輔助獲取odom信息，會被傳入dp_
boost::shared_ptr dp_; 正常的dwa運動控制類
base_local_planner::LatchedStopRotateController latchedStopRotateController_; 到達目標點後的停止旋轉運動控制類
（2）主要接口
void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)—初始化
bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)—設置全局路徑
bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel)—計算控制速度
bool isGoalReached()—判斷是否達到目標點
2 總體流程
在move base控制循環中會在規劃出新的路徑時，將新的全局路徑利用setPlan傳給DWAPlannerROS，然後調用DWAPlanner::setPlan將路徑傳遞給DWAPlanner（同時復位latchedStopRotateController_中的鎖存相關的標誌位）。
在運動規劃之前，move_base先利用DWAPlannerROS::isGoalReached（內部利用LatchedStopRotateController::isGoalReached函數實現）判斷是否已經到達了目標點（即位置在xy_goal_tolerance以內，朝向在yaw_goal_tolerance以內，且速度小於trans_stopped_velocity，rot_stopped_velocity），如果是則控制結束，即發送0速，且復位move_base的相關控制標記，並返回action成功的結果。否則，利用DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)計算局部規劃速度。
在DWAPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)中，會先將全局路徑映射到局部地圖中，並更新對應的打分項（參考DWAPlanner::updatePlanAndLocalCosts函數，和具體的打分項更新）。然後，利用LatchedStopRotateController::isPostionReached函數判斷是否已經到達目標位置，如果是，則利用LatchedStopRotateController::computeVelocityCommandsStopRotate函數計算對應的減速停止或者旋轉至目標朝向的速度指令（取決於是否機器人已經停穩，進而決定實現減速停止或者旋轉至目標朝向），否則利用DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped< tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel)計算DWA局部路徑速度。
對於停止時的處理邏輯，即LatchedStopRotateController的所有邏輯，參考“停止”章節。
對於DWAPlannerROS::dwaComputeVelocityCommands(tf::Stamped< tf::Pose> &global_pose, geometry_msgs::Twist& cmd_vel)函數，直接利用DWAPlanner::findBestPath(
tf::Stamped < tf::Pose > global_pose,
tf::Stamped< tf::Pose> global_vel,
tf::Stamped< tf::Pose>& drive_velocities,
std::vector<geometry_msgs::Point> footprint_spec)獲取最優局部路徑對應的速度指令。
在DWAPlanner::findBestPath函數中，先利用SimpleTrajectoryGenerator::initialise(
const Eigen::Vector3f& pos,
const Eigen::Vector3f& vel,
const Eigen::Vector3f& goal,
base_local_planner::LocalPlannerLimits* limits,
const Eigen::Vector3f& vsamples,
bool discretize_by_time = false)初始化軌跡產生器，即產生速度空間。
然後，利用SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector* all_explored = 0)查找最優的局部路徑。在該函數中，先調用每個打分項的prepare函數進行準備，參考“打分”章節。然後，利用SimpleScoredSamplingPlanner裏面存儲的TrajectorySampleGenerator軌跡產生器列表（目前，只有SimpleTrajectoryGenerator一個產生器）分別進行軌跡產生和打分，並根據打分選出最優軌跡。對於每個軌跡的打分，調用SimpleScoredSamplingPlanner::scoreTrajectory執行，在該函數中，遍歷各個打分項進行打分並進行求和，如果某個打分項打分小於0，則將該負值作爲路徑打分項進行立刻返回。在對每個打分項打分之前，先獲取對應的scale參數，如果scale爲0證明該打分項無效，則跳過。如果獲取到正常的打分項打分後（利用打分項的scoreTrajectory函數），將打分項乘以對應的scale作爲最終的打分項打分。
具體的速度採樣和軌跡規劃參考“運動規劃”章節。
接着在DWAPlanner::findBestPath中，根據使能參數，選擇發佈軌跡相關的可視化數據。
然後，如果最優軌跡有效，則利用最優軌跡更新擺動打分項的標誌位。
最後，在返回最優規劃軌跡之前，判斷最優軌跡代價，如果代價小於0，則代表最優軌跡無效，即沒有有效軌跡，則將返回速度指令，設置爲0。
最後，如果DWAPlannerROS::dwaComputeVelocityCommands會判斷得到的最優軌跡代價值，如果小於0，則該函數會返回false，從而引起move_base進行進一步的處理，如重新規劃、recovery等。
至此，完成了整個dwa local planner的流程。
3 運動規劃
在每個控制週期內，DWAPlanner::findBestPath會查找最優局部軌跡。
在DWAPlanner::findBestPath中，首先調用SimpleTrajectoryGenerator::initialise進行速度採樣，然後利用SimpleScoredSamplingPlanner::findBestTrajectory根據速度採樣空間進行軌跡產生、打分、篩選，從而得到最優局部軌跡。
（1）採樣
速度採樣在SimpleTrajectoryGenerator::initialise中進行，總體思路是先獲取速度空間邊界，然後根據採樣個數參數在空間內進行採樣。
在獲取空間邊界時，根據use_dwa參數，採用兩套策略。
如果use_dwa == false，首先用當前位置與目標位置的距離處理仿真時間sim_time（模擬仿真時間內勻減速到0，剛好到達目標點的情景）得到max_vel_x和max_vel_y邊界，然後基於acc_lim * sim_time得到一種邊界（上邊界），還有設置的速度極限參數（max_vel_xxx，min_vel_xxx）作爲一種邊界，然後選取三種邊界中空間較小的邊界。這種策略，能夠獲得較大的採樣空間（因爲用了sim_time）。
如果use_dwa==true，則直接用acc_lim * sim_period得到邊界，然後還有設置的速度極限參數作爲邊界，然後選取兩種邊界中空間較小的邊界。
得到速度空間邊界後，根據x，y，theta三個採樣個數進行插補，進而組合出整個速度採樣空間。
（2）獲取最優軌跡
獲取最優軌跡在SimpleScoredSamplingPlanner::findBestTrajectory中進行，在該函數中，首先調用各個打分項的prepare函數進行準備。然後遍歷std::vector<TrajectorySampleGenerator*>軌跡產生器列表（目前只有一個）進行打分，並且一旦某個產生器發現了最優軌跡則返回（該做法較爲粗略，即使有多個產生器也無法得到全局最優）。
對於每個打分器，由於上一步已經進行了速度採樣，因此可以逐個速度產生軌跡，並進行打分，同時記錄最優打分和最優軌跡。
其中，由速度產生軌跡由SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj)函數實現。
在該函數中，先判斷速度是否滿足“平移速度不小於min_trans_vel且旋轉速度不小於min_rot_vel”和“平移速度不大於max_trans_vel”兩個條件，如果滿足，則直接返回false，此時，traj爲空路徑。否則，繼續正常的軌跡產生流程。
爲了產生軌跡，需要由pos，vel，sample_target_vel三個參數離散仿真出軌跡中的所有點位姿。首先，需要確定仿真步數（即多少個點）（雖然由參數discretize_by_time_確定是由sim_time/sim_granularity計算仿真步數，還是由std::max(sim_time_distance / sim_granularity_, sim_time_angle / angular_sim_granularity_)計算，但該參數目前保持爲false，即由後者計算仿真步數）。有了仿真步數num_steps，可以由sim_time/num_steps計算出每步的時間間隔dt。然後，就可以根據具體策略計算軌跡點。
如果use_dwa== false，則採用連續加速的策略，即仿真出的軌跡中不同點對應的速度是變化的，此時將軌跡中保存的對應速度設爲基於當前速度第一次加速出的速度。否則，軌跡中的各個點爲同樣的速度，即sample_target_vel，此時軌跡中保存的速度也是該速度。
然後，開始計算各個軌跡點，如果use_dwa==false，則邊計算軌跡點位姿，邊變化速度，否則用恆速計算軌跡點位姿。
這樣，就完成了軌跡的生成。
每個速度對應軌跡產生後，調用SimpleScoredSamplingPlanner::scoreTrajectory進行打分，在該函數中，遍歷各個打分項進行打分並進行求和，如果某個打分項打分小於0，則將該負值作爲路徑打分項進行立刻返回。在對每個打分項打分之前，先獲取對應的scale參數，如果scale爲0證明該打分項無效，則跳過。如果獲取到正常的打分項打分後（利用打分項的scoreTrajectory函數），將打分項乘以對應的scale作爲最終的打分項打分。
至此，就完成了速度採樣、軌跡生成、軌跡打分和選優。
4 路徑
（1）存儲全局路徑
路徑類型爲std::vector<geometry_msgs::PoseStamped>。
在move base控制循環中會在規劃出新的路徑時，將新的全局路徑利用setPlan傳給DWAPlannerROS，然後調用DWAPlanner::setPlann將路徑傳遞給DWAPlanner（同時復位latchedStopRotateController_中的鎖存相關的標誌位）。
在DWAPlanner::setPlann中，會先將OscillationCostFunction中的擺動標誌位復位，然後利用planner_util_->setPlan將路徑傳入planner_util_，直接保存爲global_plan_。
此時的路徑時相對於全局地圖的全局座標系的（通常爲"map")。
（2）局部路徑映射及存儲
在computeVelocityCommands計算速度前，會先將全局路徑映射到局部地圖座標系下（通常爲“odom”），通過LocalPlannerUtil::getLocalPlan(tf::Stampedtf::Pose& global_pose, std::vector<geometry_msgs::PoseStamped>& transformed_plan)實現。
在getLocalPlan中，先將較長的全局路徑映射並截斷到局部地圖內（即座標系轉換爲局部地圖，且範圍完全在局部地圖內，超出地圖的則拋棄）。然後，裁減全局路徑和局部路徑（與機器人當前位置距離超過1m的舊的路徑會被裁減掉）。
裁減過的全局路徑還保存在planner_util_中，映射並經過裁減後的局部路徑會在computeVelocityCommands函數中傳入dp_中（利用DWAPlanner::updatePlanAndLocalCosts函數）。
在updatePlanAndLocalCosts中，會將局部路徑保存到dp_的global_plan_中（對於dp_來說，局部地圖中映射的全局路徑就是全局的，而dp_自己規劃的路徑是局部的）。然後，將該路徑信息傳入各個打分對象，具體參考打分章節。
5 打分
打分對象一共6個，分別爲：
base_local_planner::OscillationCostFunction oscillation_costs_（擺動打分）
base_local_planner::ObstacleCostFunction obstacle_costs_（避障打分）
base_local_planner::MapGridCostFunction path_costs_（路徑跟隨打分）
base_local_planner::MapGridCostFunction goal_costs_（指向目標打分）
base_local_planner::MapGridCostFunction goal_front_costs_（前向點指向目標打分）
base_local_planner::MapGridCostFunction alignment_costs_（對齊打分）
其中，擺動打分和避障打分項是獨立的類型，後四個打分項是同一類型。
打分計算過程中出現的負的值可以認爲是錯誤代碼（用於指示具體的出錯原因），而不是得分，該說明對下面所有的打分描述有效。
如果軌跡爲空（在產生軌跡時出錯，例如不滿足最大最小速度要去），則各個打分項對應的得分都爲0。
（1）打分對象初始化及更新
oscillation_costs_
在DWAPlanner的構造函數中，利用oscillation_costs_.resetOscillationFlags()復位擺動標誌位（側移、旋轉、前進方向的相關參數strafe_pos_only_，strafe_neg_only_，strafing_pos_，strafing_neg_，rot_pos_only_，rot_neg_only_，rotating_pos_，rotating_neg_，forward_pos_only_，forward_neg_only_，forward_pos_，forward_neg_）。
然後將oscillation_costs傳入打分項列表（也會加入其它打分項），並將打分項列表std::vector<base_local_planner::TrajectoryCostFunction*>和採樣規劃器列表std::vector<base_local_planner::TrajectorySampleGenerator*>傳入打分採樣規劃器base_local_planner::SimpleScoredSamplingPlanner。
在DWAPlannerROS的動態配置回調函數中，會將動態配置參數再傳入DWAPlanner::reconfigure函數，在該函數中利用oscillation_costs_.setOscillationResetDist(config.oscillation_reset_dist, config.oscillation_reset_angle)初始化該打分項。
在正常行走過程中（沒有到達目標點範圍），DWAPlannerROS會調用dwaComputeVelocityCommands實現computeVelocityCommands函數，在dwaComputeVelocityCommands中，會調用DWAPlanner::findBestPath方法，在該方法中會先採樣計算出最優路徑，然後利用oscillation_costs_.updateOscillationFlags(pos, &result_traj_, planner_util_->getCurrentLimits().min_trans_vel)更新擺動打分項的標誌位。在打分項更新標誌位的函數中，如果判斷最優路徑的代價也小於0（即無效），則不更新，否則，則根據最優路徑和最小平移速度限制設置打分標誌setOscillationFlags(base_local_planner::Trajectory* t, double min_vel_trans)。然後，如果在某些方向上有了限制，則檢查是否能夠復位標誌位resetOscillationFlagsIfPossible(const Eigen::Vector3f& pos, const Eigen::Vector3f& prev)。
對於setOscillationFlags函數，先設置x方向的擺動：一旦規劃的速度與當前的方向不同，則設置forward_xxx_only標誌位，表示後續只能沿規劃的速度方向運行。對於側向擺動和旋轉擺動，必須在x方向速度小於min_vel_trans時，纔會判斷，判斷邏輯和x方向一致。
由於前面可能變過方向，即設置過xxx_xxx_only標誌位，短時間內不能變向。此時，需要利用resetOscillationFlagsIfPossible判斷是否可以取消標誌位，即是否可以開始變向。通過判斷當前位置相對於上次換向的位置的平移和旋轉位置是否超過oscillation_reset_dist_和oscillation_reset_angle_，如果是則復位所有標誌位。
上面兩個函數實現了xxx_xxx_only標誌位的置位和復位。
obstacle_costs_
在DWAPlanner的構造函數中，利用local costmap初始化避障打分項，然後通過sum_scores參數（默認爲false）設置避障打分項的打分彙總方式。
然後，將obstacle_costs_傳入打分項列表，同上。
在DWAPlanner::reconfigure函數中，利用obstacle_costs_.setScale函數設置比例參數爲resolution * occdist_scale（碰撞參數），並在obstacle_costs中記錄爲scale_。然後利用obstacle_costs_.setParams函數設置max_trans_vel, max_scaling_factor, scaling_speed（這些參數可以引起避障打分項中的footprint變化，從而根據不同的速度膨脹不同的大小）。
在每一次通過DWAPlanner的findBestPath函數計算最優局部路徑時，都會利用obstacle_costs_.setFootprint爲obstacle_costs_設置一次footprint參數（帶padding），會被obstacle_costs_的footprintr_spec_（std::vector<geometry_msgs::Point>類型）參數記錄。
至此，就記錄了所有參數，並且更新了碰撞打分相關的footprint。
path_costs_
path_costs_，goal_costs，goal_front_costs_，alignment_costs_四個打分項都是MapGridCostFunction類型，因此打分思路都一致，只是通過設置不同參數即可實現不同的打分方式。MapGridCostFunction類的構造函數爲
MapGridCostFunction(costmap_2d::Costmap2D* costmap
double xshift = 0.0,
double yshift = 0.0,
bool is_local_goal_function = false,
CostAggregationType aggregationType = Last);
其中，path_costs_和alignment_costs_都是參考整個路徑信息，因此is_local_goal_function參數爲false，同理goal_costs_，goal_front_costs_對應的該參數爲true。
另外，由於goal_front_costs_和alignment_costs_兩個打分項都是參考前向打分點，因此還是利用MapGridCostFunction::setXShift函數記錄了forward_point_distance_參數，而另外兩個打分項對應的該參數爲0。
另外，goal_front_costs_和alignment_costs_兩個打分項還利用MapGridCostFunction::setStopOnFailure函數設置了stop_on_failure參數爲false。
對於path_costs_的初始化，在DWAPlanner的構造函數中，利用local costmap初始化路徑打分項。
在DWAPlanenr::reconfigure函數中，利用path_costs_.setScale函數設置比例參數爲resolution * pdist_scale_ * 0.5，並在path_costs_中記錄爲scale_。scale_參數會在打分採樣規劃器的計算軌跡代價時用到，即用打分項得到的得分，乘以打分項記錄的scale_。
在DWAPlanner::updatePlanAndLocalCosts函數（每個控制週期都會將映射到local costmap中的路徑信息在這裏更寫到相關的打分項或者其他部件中）中，會利用path_costs_.setTargetPoses函數將全局路徑在局部地圖中的映射傳入到path_costs中，作爲局部的打分參考路徑，MapGridCostFunction::setTargetPoses函數將路徑信息存儲爲target_poses_。
至此，就完成了初始化和初步的更新。
goal_costs_
對於goal_costs_的初始化，在DWAPlanner的構造函數中，利用local costmap初始化該打分項，同時也設置了is_local_goal_function標誌位true。參考path_costs_打分項初始化說明。
在DWAPlanenr::reconfigure函數中，利用goal_costs_.setScale函數設置比例參數爲resolution * gdist_scale_ * 0.5，並在path_costs_中記錄爲scale_。
在DWAPlanner::updatePlanAndLocalCosts函數（每個控制週期都會將映射到local costmap中的路徑信息在這裏更寫到相關的打分項或者其他部件中）中，會利用path_costs_.setTargetPoses函數將全局路徑在局部地圖中的映射傳入到path_costs中，作爲局部的打分參考路徑，MapGridCostFunction::setTargetPoses函數將路徑信息存儲爲target_poses_。
至此，就完成了初始化和初步的更新。
goal_front_costs_
對於goal_front_costs_的初始化，在DWAPlanner的構造函數中，利用local costmap初始化該打分項，同時也設置了is_local_goal_function標誌位爲true。另外，設置了stop_on_failure標誌位。參考path_costs_打分過程的說明。
在DWAPlanner::reconfigure函數中，利用goal_front_costs_.setScale函數設置比例參數爲resolution * gdist_scale_ * 0.5，並在goal_front_costs_中記錄爲scale_。然後，利用goal_front_costs_.setXShift函數將forward_point_distance_設爲該打分項的xshift_參數（即打分點相對於機器人原點的偏移）。
在DWAPlanner::updatePlanAndLocalCosts函數（每個控制週期都會將映射到local costmap中的路徑信息在這裏更寫到相關的打分項或者其他部件中）中，爲了給前向打分點確定它的“goal”，採用的方法是先計算當前位置和局部路徑末端點之間的連線朝向，然後在局部路徑末端點的基礎上沿該方向延伸forward_point_distance_距離，並將這個”goal“替換掉局部路徑末端點。然後利用path_costs_.setTargetPoses函數將全局路徑在局部地圖中的映射傳入到path_costs中，作爲局部的打分參考路徑，MapGridCostFunction::setTargetPoses函數將路徑信息存儲爲target_poses_。
至此，就完成了初始化和初步的更新。
alignment_costs_
對於alignment_costs_的初始化，在DWAPlanner的構造函數中，利用local costmap初始化該打分項。同時設置了stop_on_failure標誌位。
在DWAPlanenr::reconfigure函數中，利用path_costs_.setScale函數設置比例參數爲resolution * pdist_scale_ * 0.5，並在path_costs_中記錄爲scale_。然後，利用alignment_costs_.setXShift函數將forward_point_distance_設爲該打分項的xshift_參數（即打分點相對於機器人原點的偏移）。
在DWAPlanner::updatePlanAndLocalCosts函數（每個控制週期都會將映射到local costmap中的路徑信息在這裏更寫到相關的打分項或者其他部件中）中，爲了讓前向打分點合理的靠近路徑，需要針對不同的情況考慮。如果，當前位置和局部路徑末端點之間的距離比forward_point_distance_大時，則仍然alignment_costs_.setTargetPoses(global_plan_)，即不做特殊處理，打分時參考前向打分點和局部地圖路徑的距離。否則，設置alignment_costs_的scale爲0，從而忽略alignment_costs_這一代價項。
（2）代價打分
所有的打分都是基於各個打分項的scoreTrajectory(Trajectory &traj)函數實現。
oscillation_costs_
該打分項主要利用了軌跡中的xv_, yv_, thetav_三個參數進行擺動判斷。擺動打分較爲簡單，如果xxx_xxx_only設置爲了true，但是對應的規劃路徑速度方向相反，則返回代價值-5，否則返回0。
obstacle_cost_
碰撞打分的總思路是：
對軌跡上的每一點計算出對應的footprint（利用了x, y ,theta, footprint四個信息計算）進行footprintCost計算，最後彙總得分時，如果sum_scores_參數爲true，則加和所有得分，否則只取最後一個點的得分（覆蓋計算的方式）。但是，如果某個點的footprintCost爲負，則直接返回該負的得分。
在進行對軌跡各個點計算footprintCost之前，會先計算縮放因子。如果當前平移速度小於scaling_speed，則縮放因子爲1.0，否則，縮放因子爲(vmag - scaling_speed) / (max_trans_vel - scaling_speed) * max_scaling_factor + 1.0。然後，該縮放因子會被用於計算軌跡中各個點的footprintCost。
對於footprintCost，其傳入參數爲 static double footprintCost(
const double& x,
const double& y,
const double& th,
double scale,
std::vector<geometry_msgs::Point> footprint_spec,
costmap_2d::Costmap2D* costmap,
base_local_planner::WorldModel* world_model);
但scale並沒有使用，即目前還不能根據速度改變打分效果。
在footprintCost中，首先計算利用world_model->footprintCost計算footprint的代價值，如果footprint的代價值小於0，則footprintCost直接返回-6。接着，將x，y轉換爲地圖座標，轉化時如果失敗則footprintCost返回-7。得到地圖座標後，會對比footprint的代價值和機器人中心點（x，y）的代價值（可通過costmap直接獲取），並以較大的值作爲返回值。
對於world_model->footprintCost的計算，是對footprint中的每個線段進行獨立計算代價的。首先嚐試獲取線段的地圖座標，如果獲取失敗則返回-1。然後，計算利用lineCost函數計算線段代價值，並用footprint_cost參數保存最大的線段代價值，最終如果正常，則返回的也是footprint_cost。但是，如果某個線段的代價值小於0，則world_model->footprintCost直接返回-1。
對於線段的代價計算函數lineCost，其實是對線段中的所有像素點（通過bresenham算法得到）進行代價計算，如果像素點的代價爲LETHAL_OBSTACLE或者NO_INFORMATION，則該點代價爲-1。如果某個點的代價爲-1，則該線段的代價也爲-1，否則則取所有點中最大的代價作爲線段的代價。
總結下來，obstacle_cost_的打分，可能會返回-6，-7，或者具體的代價值。並且，速度scale參數沒有使用，後續可以自己嘗試實現。另外，最後彙總得分的方式兩種都不好。如果是加和，則越長的軌跡越容易積累較大的代價，因此速度大的軌跡容易被拋棄。如果是最後一個點，則中間點如果發生碰撞就不會考慮，這種策略顯得太粗略。可以嘗試增加一種返回最大footprintCost的策略，即考慮軌跡中最大footprintCost的那個點。
path_costs_
在每個findBestPath週期時，會對各個打分項進行prepare操作，其中oscillation_costs_和obstacle_costs_的prepare不執行具體的動作，而剩餘四個打分項在prepare中會對局部地圖進行預計算，從而方便後邊進行打分。
預計算的思想在於維護一個跟局部地圖尺寸對應的路徑計算地圖（MapGrid類型），該地圖中的每個元素爲MapCell類型，該類型記錄了該元素距離目標點的距離信息或者距離路徑的距離信息。在每次打分前，先利用局部路徑更新該地圖信息，並根據is_local_goal_function參數決定記錄距離目標點的距離信息還是距離路徑的距離信息，這些信息在後面打分時可以直接使用。
在prepare函數中，首先復位整個路徑計算地圖。然後，根據is_local_goal_function參數，分別利用MapGrid::setLocalGoal(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan)和MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap, const std::vector<geometry_msgs::PoseStamped>& global_plan)兩個函數分別計算對應的預處理信息。
對於setLocalGoal函數，是用來計算軌跡路徑計算地圖中所有點到局部地圖路徑末端點的最短距離。在該函數中，首先對映射到局部地圖中的路徑進行像素級的插補，從而使得整個路徑是像素連續的。然後，查找路徑在局部地圖中的最後一個點，並將路徑計算地圖中的該點標記爲已訪問（標記爲已訪問的點可以進行最短路徑計算），然後調用computeTargetDistance函數採用類似dijkstra算法的逐步探索的方式，計算出路徑計算地圖中所有點（像素級）相對於與標記點的最短距離。
對於setTargetCells函數，是用來計算軌跡路徑計算地圖中所有點到局部地圖路徑的最短距離。與setLocalGoal函數的區別主要在於，首先將局部地圖路徑中所有的點標記爲已訪問，然後調用computeTargetDistance函數就會計算路徑計算地圖中所有點到整個局部地圖路徑的最短距離，具體算法需要進一步瞭解。
因此，在經過預計算後，對於path_costs_和alignment_costs這兩個考慮與路徑距離的打分項，其內部的路徑計算地圖中存儲的是所有點到路徑的最短距離；對於goal_costs_和goal_front_costs_這兩個考慮與局部目標點距離的打分項，其內部的路徑計算地圖中存儲的是所有點到局部目標點的最短距離。
至此，完成了path，goal相關的打分項的預計算，下一步就是利用scoreTrajectory(Trajectory &traj)進行具體的打分。
在MapGridCostFunction::scoreTrajectory函數中，依次獲取路徑中的各個點座標，並嘗試將座標轉爲地圖座標，如果轉化失敗，則scoreTrajecotry函數直接返回-4。否則，從路徑計算地圖中找到距離局部地圖路徑的距離，並將該距離（像素級）值作爲該點的得分。
另外，對於path_costs_和goal_costs_兩個打分項的stop_on_failure參數爲true，即如果軌跡中的某個點未障礙物或者未再路徑計算地圖中探索到，則表示查找該點的得分失敗，這會造成scoreTrajectory函數直接返回對應的負的得分。例如，如果軌跡中的某個點本身就是障礙物（在路徑計算地圖中會有標記），則scoreTrajectory直接返回-3作爲軌跡得分；如果軌跡中某個點不可達（在路徑計算地圖中沒有計算到），則scoreTrajectory直接返回-2作爲軌跡得分。
對於goal_front_costs和alignment_costs_兩個打分項對應的stop_on_failure參數爲false，這是因爲前向打分點有可能會出現計算距離出錯的情況，例如超出地圖範圍，但由於前向打分點的打分失敗不會帶來危險，因此可以不立刻返回負得分。
軌跡所有點得分的彙總方式有Last、Sum、Product三種方式，默認爲Last（且沒有對應的配置參數，除非自己修改源碼實現），即只考慮軌跡最後一個點的打分。
總的來說，path_costs_打分項只考慮最後一個點的得分，該得分可能會取-4（如果是path_costs_和goal_costs_，還有可能返回-2，-3），或者最後一個點與局部地圖路徑之間的像素距離。
goal_costs_
該打分項的思路在path_costs_的打分過程中已經描述，可以參考。
goal_front_costs_
該打分項和goal_costs_基本一樣，只是參考的是機器人前向點和局部目標點的距離，而不是機器人原點和局部目標點的距離。
alignment_costs_
該打分項和path_costs_基本一樣啊，只是參考的是機器人前向點和局部地圖路徑的距離，而不是機器人原點和局部地圖路徑的距離。
6 停止
對於DWAPlannerROS的停止處理邏輯，由LatchedStopRotateController類完成，主要包括了停止判斷、加速停止、旋轉至目標朝向三個大的部分。
（1）停止判斷
在move_base的每個控制週期，都會利用利用DWAPlannerROS::isGoalReached（內部利用LatchedStopRotateController::isGoalReached函數實現）判斷是否已經到達了目標點（即位置在xy_goal_tolerance以內，朝向在yaw_goal_tolerance以內，且速度小於trans_stopped_velocity，rot_stopped_velocity）。
對於LatchedStopRotateController::isGoalReached函數，分爲鎖存目標和不鎖存目標兩種處理邏輯。鎖存目標，即如果機器人在行駛過程中出現過位置滿足xy_goal_tolerance的條件時，則設置xy_tolerance_latch_標誌位，代表已經達到過目標，不用考慮最終是否超出目標點，直接進行停止、旋轉至目標朝向即可。
如果不鎖存，則必須始終滿足機器人當前位置是否滿足xy_goal_tolerance的條件，滿足則代表到達了目標位置。
在到達目標位置的前提下，還要判斷機器人朝向是否滿足目標朝向yaw_goal_tolerance的需求，如果也滿足，則判斷當前速度是否滿足停止條件，即x和y的速度小於trans_stopped_velocity，theta速度小於rot_stopped_velocity。
只有到達位置、達到朝向、速度滿足停止三個條件都滿足的情況下，纔算機器人到達了目標位姿，isGoalReached函數纔會返回true。
（2）加速停止
在DWAPlannerROS::computeVelocityCommands的每個計算週期內，都會先利用LatchedStopRotateController::isPostionReached函數判斷是否已經到達目標位置，如果是，則利用LatchedStopRotateController::computeVelocityCommandsStopRotate函數計算對應的減速停止或者旋轉至目標朝向的速度指令，否則纔會使用DWAPlanner計算最優軌跡對應的速度命令。
對於isPostionReached的判斷，和isGoalReached中判斷達到位置的邏輯一樣，即如果啓動了鎖存，且xy_tolerance_latch_標誌位被標記（即滿足過xy_goal_tolerance條件），則認爲已經到達；如果未啓用鎖存，則需要基於當前位置是否滿足xy_goal_tolerance確定是否到達了位置。
在LatchedStopRotateController::computeVelocityCommandsStopRotate函數中，會假設用戶想要進行旋轉停止了，即已經到達位置了，因此會先判斷當前朝向是否滿足yaw_goal_tolerance要求，如果滿足，則將速度指令設置爲0（這種做法太過粗略）。
如果朝向沒有滿足要求，則會判斷是要進行加速停止還是旋轉至目標點。
如果機器人還未處於旋轉停止狀態（通過rotating_to_goal標記），並且速度沒有滿足停止要求，則會調用stopWithAccLimits進行加速停止處理，否則代表應進入了旋轉停止狀態，則會調用rotateToGoal進行旋轉至目標朝向處理，同時設置rotating_to_goal標誌位true。
對於stopWithAccLimits函數，對於利用acc_lim參數進行最快的減速命令計算，然後利用DWAPlanner::checkTrajectory函數計算出軌跡，並進行軌跡打分。在計算軌跡時利用了SimpleTrajectoryGenerator::generateTrajectory(
Eigen::Vector3f pos,
Eigen::Vector3f vel,
Eigen::Vector3f sample_target_vel,
base_local_planner::Trajectory& traj)，如果速度不滿足最小速度要求（min_rot_vel、min_trans_vel）則軌跡爲空。
在對軌跡進行打分時，利用了SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost)，如果軌跡爲空，則打分爲0，此時雖然checkTrajectory返回true，但對應的速度可能過小（目前判斷沒有太大影響）。
如果checkTrajecotry返回true（軌跡打分大於等於0）則代表路徑有效，stopWithAccLimits計算出的加速停止速度命令有效，否則設置速度命令爲0。
（3）旋轉至目標朝向
旋轉至目標朝向有rotateToGoal函數計算，首先將x和y速度設置爲0，對於theta速度，利用當前朝向與目標朝向的差的比例控制（係數爲1）產生，然後用加速能力（acc_lim*sim_period）、速度限幅（max_rot_vel，min_rot_vel）、減速能力（v^2=2as公式）進行速度限幅。接着利用DWAPlanner::checkTrajectory進行軌跡打分檢測，邏輯同前。
版權聲明：本文爲博主原創文章，轉載請註明出處：http://www.cnblogs.com/sakabatou/p/8297479.html