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

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

框架

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

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

1. TrajectorySampleGenerator产生一系列轨迹
2. 然后TrajectoryCostFunction遍历轨迹打分
3. TrajectorySearch找到最好的轨迹拿来给小车导航
4. 由于小车不是一个质点，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_ 优先选择前向行驶；