ros Navigation DWA学习

参考博客
ROS 源码阅读——局部路径规划之DWAPlannerROS分析

dwa_planner_ros.cpp中的computeVelocityCommands()函数是计算最后的速度指令的。在函数中，首先得到机器人位置，然后得到全局路径映射后的局部路径，然后由dp_.updatePlanAndLocalCosts()更新打分，然后判断是否到达目标附近，是的话调用停止的函数发布速度，否则调用dwaComputeVelocityCommands计算速度。

dp_.updatePlanAndLocalCosts()函数更新打分应该是更新局部路径产生的路径点那些内容。

在dwaComouteVelocityCommands函数中，主要的是findBestPath()函数。

findBestPath()函数在dwa_planner.cpp文件中定义。
首先利用generator_.initialise()产生轨迹。该函数为base_local_planner功能包中的simpleTrajectorygenerator.cpp中的类。

generator_.initialise(pos,
        vel,
        goal,
        &limits,
        vsamples_);

    result_traj_.cost_ = -7;

然后就是findBestTrajectory()函数

std::vector<base_local_planner::Trajectory> all_explored;
scored_sampling_planner_.findBestTrajectory(result_traj_, &all_explored);

该函数在base_local_planner功能包中的simpleScoredSamplingPlanner.cpp中定义.。
首先进行打分项的prepare。打分项那部分，每个都是critics,这个在dwa_planner.cpp中构造函数中会把不同的costfunction压入。分别是

 // see constructor body for explanations
      base_local_planner::SimpleTrajectoryGenerator generator_;
      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_;
      base_local_planner::TwirlingCostFunction twirling_costs_;

打分项prepare后，就是simple_scored_sampling_planner中的scoreTrajectory函数，而在其中，cost计算如下

double cost = score_function_p->scoreTrajectory(traj);

这一部分在base_local_planner功能包中的Trajectory_cost_function.h中声明，是一个虚函数，其定义根据之前score_function_p的不同，即critics中类型的不同，分别到不同的costfunction中。

以obstacle_cost_function为例。
其cost为f_cost和cost的最大值，f_cost计算如下：

 double f_cost = footprintCost(px, py, pth,
        scale, footprint_spec_,
        costmap_, world_model_);

footprintCost在obstacle_cost_function.cpp中。
其计算如下：

 double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);

world_model.h文件中footprintCost函数也是虚函数，因为用的是costmap,所以为costmap_model.cpp的footprintCost函数。

在其中会返回足迹点之间线段上所有的点中的最大代价，或者求和。
点的代价参考博客 costmap_2D包的理解

应该是就不包含距离障碍的代价，就是碰撞或不碰撞。