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,该类会进行所有轨迹的计算,轨迹中会保存轨迹点、速度、评分等各种因素,最后下发的速度指令也是在这里选择确定的