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中的函數。

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

總結

以上是生活随笔為你收集整理的Navigation源码阅读之dwa_local_planner（DWA动态窗口法）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。