ros导航路径规划算法

调试了很长时间的机器人自动导航，一直以为路径规划是个很高大上的东东。直到看完路径生成及选择代码，结果好无语。

可能这是最简单的路径规划算法，毕竟叫做simple trajectory generator。

路径规划分成两部分：1.样本路径生成。2.最优路径选择

第一部分 样本路径生成

1.获取当前的x轴速度vx，y轴速度vy，和z轴转向角速度th。这部分数据一般从odom获取，并且在2d地图上，y轴速度一般是0。

2.获取下发控制命令的频率H，算出每个控制命令的周期  1/H = sim_period_

3.以x轴速度vx为例，根据命令周期长度，加速度，减速度，最大速度，最小速度，算出下个周期时，vx的最大值max_vx和最小值min_vx。

max_vx = min(最大速度，vx + 加速度×命令周期);

min_vx = max(最小速度，vx - 减速度×命令周期)

4.获取每个速度的采样率，例如vx的最大速度为1,最小速度为0,采样率为6，那么针对vx的采样标本是0,0.2,0.4,0.6,0.8,1。

5.穷举法，针对vx，vy，th的所有标本进行全组合，一共就有（vx采样率×vy采样率×th采样率）个样本速度（vx，vy，th）。

6.针对每个样本速度，在 sim_period_时间内，可以生成一条路径（由一系列个点组成）。

需要注意是否使用dwa算法，dwa是非持续加速算法，即在sim_period_时间样本速度就是实际速度不会改变。而持续加速算法，最开始实际速度就是当前速度，在sim_period_时间内不断加减速，最终实际速度变成样本速度。

代码可见：这两个方法

void SimpleTrajectoryGenerator::initialise(
    const Eigen::Vector3f& pos,
    const Eigen::Vector3f& vel,
    const Eigen::Vector3