ROS局部规划模块分析

BaseLocalPlanner是所有局部路径规划器的基类，所有的局部路径规划器都是它的插件

BaseLocalPlanner接口：

computeVelocityCommands

isGoalReached

setPlan

Initialize

1、以DWAPlannerROS为例：

a) 外部首先调用的是initialize函数，塞入外面弄好的costmap

b) DWAPlannerROS是对DWAPlanner的外部封装，DWAPlanner是实际的DWA算法实现类

c) Costmap会被塞入plannerutils中，然后该util将会传入DWAPlanner的构造函数

d) 实际使用时，会通过一些判断，类似于一个状态机，进行具体方法选择。主要是在快到点以及一些特殊状态时候不用DWA进行计算。

2、ROS中DWA解析

a) DWA其实最重要的就是各种速度评分函数及其权重的设计以及选择

b) 本DWA中主要使用了如下几种评分函数：

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_;

c) 所有的CostFunction都是继承于TrajectoryCostFunction

d) TrajectoryCostFunction的主要接口如下：包含了评分函数所需要的一切接口，初始化，权重设定、获得，评分计算

i. Prepare

ii. scoreTrajectory

iii. setScale

iv. getScale

e) 所有的costfunction都塞在一个TrajectoryCostFunction*的容器中，塞入顺序需要注意，因为任何一个costfunction返回负评分都会让评价速度被抛弃，因此可以利用这个特性提高算法效率

除了costfunction是必须的外，还需要一个轨迹生成器，DWA中使用  base_local_planner::SimpleTrajectoryGenerator，塞在了base_local_planner::TrajectorySampleGenerator*的容器中

Costfunction和generator都初始化好之后，都塞在了scored_sampling_planner_中，该变量为 base_local_planner::SimpleScoredSamplingPlanner类型，继承于纯虚基类base_local_planner::TrajectorySearch

f) 实际供外部调用的接口为

findBestPath(

      tf::Stamped<tf::Pose> global_pose,

      tf::Stamped<tf::Pose> global_vel,

      tf::Stamped<tf::Pose>& drive_velocities,

      std::vector<geometry_msgs::Point> footprint_spec)

该函数中，首先进行generator_的初始化，然后调用scored_sampling_planner_的findBestTrajectory接口。findBestTrajectory的第一个参数为轨迹结果，第二个参数为遍历过得所有轨迹。在findBestTrajectory函数中，首先进行generator生成轨迹，之后利用costfunction进行评分。

g) 因此，马上要考虑的问题就出现了，那就是generator如何生成轨迹，以及costfuntion如何进行评分。

h) DWA中的generator如前所述，只有一个，是SimpleTrajectoryGenerator，该类会进行所有轨迹的计算，轨迹中会保存轨迹点、速度、评分等各种因素，最后下发的速度指令也是在这里选择确定的