ROS关于movebase的局部路径规划代码解析

终于做完了机器人movebase路径规划算法的修改工作，现在把工作时写的笔记分享出来，希望可以起到搞清楚算法步骤的作用（建议配合代码一起食用）

---------------------------------------------------------------------------
trajectory_planner_ros.cpp
TrajectoryPlannerROS::computeVelocityCommands：
局部规划和速度信息计算

1.获取机器人位置
2.将全局规划的结果 转化为相对于当前位置的坐标系
3.根据机器人位置修剪这个计划
4.根据odom获取当前机器人的移动速度
5.获取全局规划的最终目标位置
6.判断是否已经到达目标位置
    1）已经到达目标位置，判断是否偏仰角在可接受范围
        1-偏仰角可接受，返回零速度
        2-偏仰角不可接受，调用findBestPath进行局部规划得到drive_cmd和最佳path
        3-设置标记为转移到目标点（rotating_to_goal=true），然后调用rotateToGoal获取cmd_vel
    2)没有到达目标位置，转到7.
7.更新全局规划
8.调用findBestPath进行局部规划得到drive_cmd和最佳path
9.对cmd_vel进行赋值，将drive_cmd的结果赋值为cmd_vel
10.将local_plan的值设置为路径规划找到的最佳path
11.将局部路径规划local_plan和全局路径规划transformed_plan一起传入可视化工具
------------------------------------------------------------------------
trajectory_planner.cpp
Trajectory TrajectoryPlanner::findBestPath:
获取最优局部路径和速度
传入的信息：当前机器人的姿势，和运动信息

1.使用resetPathDist重新设置地图信息,path_map_,goal_map_
2.获取机器人的信号范围
3.在path_map_中对信号覆盖范围进行标记
4.在path_map_,goal_map_地图上进行更新：在全局路径规划下更新路径
5.使用createTrajectories函数求出最优轨迹（需要参数：当前位置，当期速度）
6.判断最优路径是否找到
    1）没有找到最优局部路径，将速度信息均设置为0
    2）找到最优路径，设置为最优路径的速度信息
------------------------------------------------------------------------
trajectory_planner.cpp
Trajectory TrajectoryPlanner::createTrajectories：
推出轨迹并且计算最小成本
传入的信息：当前的位置信息，当前的运动状态信息

1.根据设定的最大，最小加速度，局部规划的周期时间，当前机器人的速度，计算最大速度和最小速度，最大角速度和最小角速度
2.根据设定的vx，vy，vtheta方向上的样本个数计算样本差（差速轮这里没有y方向，所以设为0）
3.获取全局规划下的路径障碍物cost：impossible_cost
4.如果是允许先进的，使用for循环，依次将每个sample的vx_sample，vtheta_sample放入generateTrajectory函数，求得路径
5.在所有samples的路径中得到一个cost最小的路径作为最佳路径保存
6.判断是否是完整机器人
    1）如果是完整机器人，需要发送扫射速度，发射vy_sample=0.1的扫射速度，generateTrajectory函数求得路径
    2)发射vy_sample=-0.1的扫射速度，generateTrajectory函数求得路径
7.如果只进行旋转，从机器人的基座最小原位旋转角速度开始，进行不同角速度的样本计算
8.路径规划成功，且当前距离较小时，判断转向与角速度方向关系是否一致
    1）如果一致：（角速度小于0且右转 或者 角速度大于0且左转）更新最佳路径
9.判断是否找到了一条合法的路径
    1）如果找到了，则判断是否局部路径的规划结果是倒行
        1-如果是倒行，判断运动状态，设置约束状态
        2-根据规划的运动结果判断是否满足震荡条件
    2）如果没有找到，使机器人的速度样本为向后退一步
10.返回局部路径规划结果
------------------------------------------------------------------------------
trajectory_planner.cpp
void TrajectoryPlanner::generateTrajectory
生成轨迹
传入的信息：
机器人当前位置x, y, theta，当前运动状态vx, vy, vtheta，运动速度样本vxsample, vysample, vthetasample，设置的最大加速度 accx,accy，全局障碍物cost

1.封锁配置
2.计算总体速度
3.以当前速度移动的话，一个周期（一次局部规划）需要运行多少个点，即需要走多少步
4.每一步所需要的时间
5.resetPoints函数创造一个新的轨道，轨道速度设置为sample，cost设置为-1
6.使用for循环模拟每一步的走法
    1）使用footprintCost计算出footprint
        1-footprint为负，即在这一步遇到了障碍物，规划失败
    2）计算occcost
    3）计算goaldist和pathdist
    4）判断是否需要根据朝向打分
        1-如果是，headingDiff函数极端heading_diff
    5）将当前位置信息点加入轨道
    6）计算新的位置信息，当前的速度信息
7.各个cost做加权值和，得到的cost为当前路径的cost
---------------------------------------------------------------------------------
计算信息：
1）footprint的计算：在可以探测到的区域内障碍物的cost
2）occcost的计算：沿着规划路径所走的最大障碍物成本
3）goaldist和pathdist的计算target_dist：与全局规划路径和目标位置的距离
4）heading_diff的计算：与目标位置的偏仰角cost
-----------------------------------------------------------------------------------
footprint的计算:
footprintCost(x_i, y_i, theta_i)
传入的参数：
    当前x，y位置，当前角度theta
当计算障碍物的cost时，需要把该位置信号覆盖区的也算上
    world_model_.footprintCost(x_i, y_i, theta_i, footprint_spec_, inscribed_radius_, circumscribed_radius_)