ROS源码解读(一)--局部路径规划

包括Trajectory Rollout 和 Dynamic Window Approach (DWA)两种方法，理论上来说分别对应base_local_planner和dwa_local_planner两个包，但其实dwa的大部分代码都放在了base_local_planner包里面

框架

对于这种大工程，我们还是从类继承图来着手：



以上接口是局部规划的核心，简单来说:

TrajectorySampleGenerator产生一系列轨迹

然后TrajectoryCostFunction遍历轨迹打分

TrajectorySearch找到最好的轨迹拿来给小车导航

由于小车不是一个质点，worldModel会检查小车有没有碰到障碍物。

流程

1.初始化

包括参数，地图，车身数据，比例系数

2.更新局部地图和代价

将全局路径中第一个离开局部地图的点作为全局目标（或者使用最后一个在局部地图内的点作为全局目标）

更新局部地图中每个点的代价，具体是在computeTargetDistance中实现的，实现的方法是用队列实现了一格格膨胀的效果，eg 终点为0，相邻的点cost+1 ,同理路径上的点为0，膨胀+1，注意同一个点取最小值

3.寻找最优路径

然后TrajectoryCostFunction遍历轨迹打分

每条轨迹遍历代价函数，比例求和

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

OscillationCostFunction oscillation_costs_ 震荡代价 ;是否震荡

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::PreferForwardCostFunction prefer_forward_costs_ 优先选择前向行驶；