Navigation源码阅读之dwa_local_planner（DWA动态窗⼝
法）
DWAPlannerROS是封装类，提供了与move_ba的接⼝，⽽DWAPlanner是具体实现类，它⾮常依赖costmap（当然不指望让⼩车动态避障的话就⽆所谓了），因此我们在使⽤时需要保证代价地图的膨胀度以及实时更新频率。btw：在此类代码中，基本上下反复使⽤的变量在函数形参中都是引⽤，实在放⼼不下还是看声明吧～
move_ba是规划路径与速度的⼤类，DWAPlannerROS提供给它的接⼝有两个，tPlan与computeVelocityCommands。
⼀．tPlan负责获取全局路径，DWAPlannerROS::tPlan获取后，转发给DWAPlanner::tPlan，恢复振荡标志位再转发给LocalPlannerUtil::tPlan，这样层层叠叠的调⽤很有层次感，这样每当产⽣⼀个新的全局路径都第⼀时间提供给全局——局部转化以及剪裁功能使⽤。
⼆．computeVelocityCommands这个函数负责本次循环的下发速度的解算，它⼀上来就先确定⼩车当前在全局中位置。
１．Costmap2DROS::getRobotPo获取⼩车全局坐标与姿态。
if ( ! costmap_ros_->getRobotPo(current_po_))
{
ROS_ERROR("Could not get robot po");
return fal;
}
这个错误其实会出现得⽐较频繁，导致⼩车不动或者抽搐。具体我们先看这个函数，虽说它位于Costmap2DROS类中，但是和costmap 并没有什么关系，负责将⼩车⾃⾝的位姿⽤tf转化为全局位姿，在这⾥出错的原因很可能是transform_tolerance_设得太⼩，⼩车⾃⾝性能跟不上被迫报错。try中抛出的三个错误犯的概率不是太⼤。
２．LocalPlannerUtil::getLocalPlan将全局路径转化到局部路径，位于ba_local_planner包local_planner_util.cpp中。
楮实子的功效与作用该函数先调⽤ba_local_planner::transformGlobalPlan，这是ba_local_planner包中goal_functions.cpp中的函数，它将全局路径转化为ba_link下的路径，这个⽂件包括的是⼯具性的函数，DWA与trajectoryRollOut⽅法均会调⽤该⽂件。
if(limits_.prune_plan)
{
ba_local_planner::prunePlan(global_po, transformed_plan, global_plan_);
}
这是在根据⼩车当前位置裁剪前⽅⼀⼩段路径，依然存放在transformed_plan中。limits_.prune_plan是可配置的参数，默认为true。
３．调⽤DWAPlanner::updatePlanAndLocalCosts，这个函数调⽤了MapGridCostFunction即根据栅格地图产⽣⼀系列打分项，它们均调⽤了各⾃的接⼝tTargetPos：
void apGridCostFunction::tTargetPos(std::vector<geometry_msgs::PoStamped> target_pos) {
target_pos_ = target_pos;
}
如何算胎儿体重
４．调⽤LatchedStopRotateController::isPositionReached判断是否到终点了，这也是ba_local_planner中的功能包，这只是通过判断终点和当前位置的算术距离，xy_goal_tolerance是可设置的参数，⼩于这个数就可认为到达了。若到终点了，调⽤LatchedStopRotateController::computeVelocityCommandsStopRotate函数，使⼩车旋转⾄最终姿态；否则继续调⽤dwaComputeVelocityCommands函数计算下发速度。如果有报错：
ROS_WARN_NAMED("dwa_local_planner", "DWA planner failed to produce path.");
这⼤概率是dwaComputeVelocityCommands中代价地图出现异常，导致函数返回fal：
指南学习心得
st_ < 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 clo to the robot."); local_plan.clear();
publishLocalPlan(local_plan);
三单
return fal;
}
围棋的规则如果该函数没有问题，则move_ba将会得到计算后的cmd_vel。
寒冬里的温暖三．dwaComputeVelocityCommands这个函数很有意思，它的形参是⼀个tf变换和⼀个待处理的速度cmd_vel，这个速度是从computeVelocityCommands函数中⼀脉相承过来的。
１．⽤ba_local_planner::OdometryHelperRos的对象odom_helper_来读取⾥程计读取的⽬前⼩车位姿global_po；
（１）预备知识１：ba_local_planner::OdometryHelperRos位于ba_local_planner包内，接收ba_controller传出的odom话 题，将话题中的twist部分转化为tf变换（robot_vel），这⼀步是为了在dwa中的findBestPath函数中使⽤global_vel形参。
２．dp_是指向DWAPlanner类的shared_ptr，调⽤DWAPlanner::findBestPath函数，在这个函数⾥变量后缀是costs_的，都是打分项，例如该函数第⼀句：
obstacle_costs_.tFootprint(footprint_spec);
地榆升白片说明书这就是调⽤了打分项——避障打分，这个函数调⽤ObstacleCostFunction::tFootprint函数，具体在对ba_local_planner的阅读中再分析过，在这⾥对避障打分类进⾏初始化。接着是对当前位姿、速度与终点位姿转化为矩阵，并⽤SimpleTrajectoryGenerator对generator_初始化，创造路径的采样空间。接下来：
std::vector<ba_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);
这是⽤ba_local_planner::SimpleScoredSamplingPlanner的对象，对所有可能轨迹进⾏遍历。dwa算法的实现与ba_local_planner 包紧密相关，所以两个包需要结合⼀起阅读～接着在all_explored这个vector中遍历，若找到cost_>=0的轨迹(即我们需要的轨迹），则将pt（ba_local_planner::MapGridCostPoint的对象）放⼊traj_cloud_中。（这是要发布⼀个由
ba_local_planner::MapGridCostPoint组成的topic？再发布⼀个可视化的代价给rviz？）
随后将drive_velocities设置好（这是关键点，因为实际上cmd_vel的数值来源就是该参数），并返回⼀个最佳路径传回dwaComputeVelocityCommands。
３．把drive_cmds（即findBestPath中的drive_velocities）参数赋予cmd_vel，这就是我们需要的下发
速度。并且把
脚内侧传接球教案ba_local_planner::Trajectory格式的path转化成nav_msgs/Path，调⽤publishLocalPlan函数。在此需要注意：
costmap_ros_->getGlobalFrameID()
局部轨迹必须与代价地图处于同⼀frameID下。
publishLocalPlan函数也是goal_functions.cpp中的函数。
整个流程⾮常漫长、繁杂，很令⼈抓狂。但是把函数调⽤流程梳理⼀遍，可以准确定位错误来源，也很⽅便改写～
