Navigation源码阅读之dwa_local_planner（DWA动态窗口法）

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

整个流程非常漫长、繁杂，很令人抓狂。但是把函数调用流程梳理一遍，可以准确定位错误来源，也很方便改写～