DWA算法分析

DWA Local Planner这部分是属于Local planner，在Navigation中有两个包：
dwa_local_planner 和base_local_planner 查看了dwa_local_planner ，发现其实际是调用的base_local_planner 中的函数，而base_local_planner 中包含了两种planner ：DWA 或者Trajectory Rollout approach 。所以结论就是，只需要搞清楚base_local_planner 的执行就OK。
base_local_planner package 实际是继承于BaseLocalPlanner ：

class TrajectoryPlannerROS : public nav_core::BaseLocalPlanner 

对于基类接口定义：

namespace nav_core {
  class BaseLocalPlanner{
    public:
      virtual bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel) = 0;
      virtual bool isGoalReached() = 0;
      virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped>& plan) = 0;
      virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
      virtual ~BaseLocalPlanner(){}
    protected:
      BaseLocalPlanner(){}
  };
};   

因此，派生类必须实现基类的接口。类TrajectoryPlannerROS 的方法：

在成员变量中，重要的几个变量如下：

WorldModel* world_model_; ///< @brief The world model that the controller will use
TrajectoryPlanner* tc_; ///< @brief The trajectory controller
costmap_2d::Costmap2DROS* costmap_ros_; ///< @brief The ROS wrapper for the costmap the controller will use
costmap_2d::Costmap2D* costmap_; ///< @brief The costmap the controller will use
std::vector<geometry_msgs::PoseStamped> global_plan_;

其实最重要的就是去找到代价最小代价的velocity。通过在三个维度（x，y，theta）的速度，加速度的采样，得到候选velocity，然后对这些velocity做cost 评判，评判的标准是：

if (!heading_scoring_) { cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost; } else { cost = pdist_scale_ * path_dist + goal_dist * gdist_scale_ + occdist_scale_ * occ_cost + 0.3 * heading_diff; }
traj.cost_ = cost;

那么，这里面的path_dist，goal_dist，occ_cost，heading_diff 是怎么计算的呢？
occ_cost的计算很简单，直接通过costmap就能得到这个值:

double footprint_cost = footprintCost(x_i, y_i, theta_i);
occ_cost = std::max( std::max(occ_cost, footprint_cost),
                   double(costmap_.getCost(cell_x, cell_y)));

计算path_dist，goal_dist：

    path_dist = path_map_(cell_x, cell_y).target_dist;
    goal_dist = goal_map_(cell_x, cell_y).target_dist;

那么如何更新path_dist，goal_dist: 需要costmap_ , global_plan_

    path_map_.setTargetCells(costmap_, global_plan_);
    goal_map_.setLocalGoal(costmap_, global_plan_);

首先看 成员函数setTargetCells：

//update what map cells are considered path based on the global_plan
void MapGrid::setTargetCells(const costmap_2d::Costmap2D& costmap,const std::vector<geometry_msgs::PoseStamped>& global_plan) 

这个函数会根据global_plan更新costmap，如何做到的呢？

//将adjusted_global_plan的信息，打包成MapCell类型，塞到path_dist_queue
 for (i = 0; i < adjusted_global_plan.size(); ++i) {
      double g_x = adjusted_global_plan[i].pose.position.x;
      double g_y = adjusted_global_plan[i].pose.position.y;
      unsigned int map_x, map_y;
      if (costmap.worldToMap(g_x, g_y, map_x, map_y) && costmap.getCost(map_x, map_y) != costmap_2d::NO_INFORMATION) {
        MapCell& current = getCell(map_x, map_y);
        current.target_dist = 0.0;
        current.target_mark = true;
        path_dist_queue.push(&current);
        started_path = true;
      } else if (started_path) {
          break;
      }
    }
    if (!started_path) {
      ROS_ERROR("None of the %d first of %zu (%zu) points of the global plan were in the local costmap and free",
          i, adjusted_global_plan.size(), global_plan.size());
      return;
    }

    computeTargetDistance(path_dist_queue, costmap);

于是重点来了，最后一行computeTargetDistance(path_dist_queue, costmap);，这个函数的实际算法实现：

 void MapGrid::computeTargetDistance(queue<MapCell*>& dist_queue, const costmap_2d::Costmap2D& costmap){
    MapCell* current_cell;
    MapCell* check_cell;
    unsigned int last_col = size_x_ - 1;
    unsigned int last_row = size_y_ - 1;
    while(!dist_queue.empty()){
      current_cell = dist_queue.front();
      dist_queue.pop();

      if(current_cell->cx > 0){
        check_cell = current_cell - 1;
        if(!check_cell->target_mark){
          //mark the cell as visisted
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }

      if(current_cell->cx < last_col){
        check_cell = current_cell + 1;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }

      if(current_cell->cy > 0){
        check_cell = current_cell - size_x_;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }

      if(current_cell->cy < last_row){
        check_cell = current_cell + size_x_;
        if(!check_cell->target_mark){
          check_cell->target_mark = true;
          if(updatePathCell(current_cell, check_cell, costmap)) {
            dist_queue.push(check_cell);
          }
        }
      }
    }
  }

上述代码检查current_cell 的四个邻接cell， 然后不断的扩散，每个cell相对于之前的cell，更新target_dist :

double new_target_dist = current_cell->target_dist + 1;
    if (new_target_dist < check_cell->target_dist) {
      check_cell->target_dist = new_target_dist;
    }

  inline bool MapGrid::updatePathCell(MapCell* current_cell, MapCell* check_cell,
      const costmap_2d::Costmap2D& costmap){

    //if the cell is an obstacle set the max path distance
    unsigned char cost = costmap.getCost(check_cell->cx, check_cell->cy);
    if(! getCell(check_cell->cx, check_cell->cy).within_robot &&
        (cost == costmap_2d::LETHAL_OBSTACLE ||
         cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||
         cost == costmap_2d::NO_INFORMATION)){
      check_cell->target_dist = obstacleCosts();
      return false;
    }

    double new_target_dist = current_cell->target_dist + 1;
    if (new_target_dist < check_cell->target_dist) {
      check_cell->target_dist = new_target_dist;
    }
    return true;
  }