理解ROS Navigation Stack，看完这篇你就知道啦！
ROS Navigation Stack是ROS提供的一个二维的导航功能包集合，通过输入里程计、传感器信息和目标位姿，输出控制机器人到达目标状态的安全速度指令。
这三个部分都是以插件的形式实现的，通过插件机制可以方便地切换不同算法实现的规划器。恢复行为会在机器人移动过程中出现了异常状态时被触发，目的是帮助机器人摆脱异常状态。另外，move_base还包括了global_costmap（全局代价地图）和local_costmap（局部代价地图），规划器需要在代价地图上进行导航规划。下面我们来详细看一下上面提到的各部分内容。
简单来说，里程计的作用就是估计机器人运动的距离和速度。通过阅读源码可以得知，在ROS Navigation Stack中，里程计信息有两个作用，一个作用是提供给局部规划器，当局部规划器选取最优路径和判断机器人是否停止的时候会使用到里程计的速度信息，另一个作用就是将估计位姿信息用于定位。
nav_msgs/Odometry.msg
激光雷达也可以感知到环境中障碍物的位置，通过将障碍物加入到代价地图中，实现避障。具体使用哪几种传感器，依赖于使用的机器人平台。理论上来说，使用的传感器种类越多，定位和避障的效果更好。
tf是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓存并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。
点击可查看大图
从上面这张图我们就可以直观地理解什么是tf变换了，由tf树帮助我们管理激光雷达与机器人底盘之间的坐标变换关系，当激光雷达感知到某个位置存在障碍物时，通过tf变换就能得到障碍物与机器人底盘之间的距离。
map_server在ROS Navigation Stack中是可选的，其主要的作用就是给机器人导航提供地图。提供地图有两种方式，一种方式是通过SLAM提供实时地图，另一种方式是提供SLAM提前建立并保存或者通过其他方式制作的地图，常用的SLAM算法有gmapping和hector_slam等。
一般在比较规则的场景，可以制作高精度地图提供给机器人，定位和规划都会有比较好的效果。通过SLAM提供实时地图，需要把实时地图以话题的形式发布。通过map_server提供的地图为pgm格式，通过加载yaml配置文件，将地图以话题的形式加载到系统中。在yaml文件中可以配置地图的分辨率，原点以及表示占据/空闲的概率。下面为yaml配置文件的内容：
·地图的默认参考系为map。占据的概率occ = (255 – color_avg) / 255.0，其中color_avg为像素RGB的平均值。
amcl是ROS Navigation Stack中唯一指定的定位算法，全称为自适应蒙特卡洛定位，它是一种用于在二维环境中移动的机器人的概率定位系统。简单概括一下它的原理就是通过在全局地图中撒粒子，粒子可以理解为机器人的可能位姿。
按照评价标准，例如激光雷达数据与地图的匹配程度给粒子打分，分数越高代表机器人在这个位置的可能性越大，经过粒子滤波器以后留下的就是分数高的粒子了。
上图中红色的粒子簇即为amcl在全局地图中撒下的粒子，可以看到一开始的粒子簇集中在给定的起始位姿周围，此时的粒子簇还是比较分散的，随着机器人的移动，粒子簇逐渐收敛，定位效果还是不错的。
点击可查看大图
amcl在ROS Navigation Stack中的作用就是输出map->odom的tf变换，来弥补里程计的漂移误差，它要求在机器人的定位系统中要存在里程计位姿估计，即odom->base_link的tf变换，并给定起始位姿和输入传感器数据。
costmap_2d功能包提供了一种二维代价地图的实现方案，该方案从实际环境中获取传感器数据，构建二维或三维的栅格占用地图，以及基于占用栅格地图和用户定义膨胀半径的二维代价地图。该包也支持基于map_server初始化代价地图，基于滚动窗口的代价地图，以及订阅和配置传感器话题。
点击可查看大图
上面这张图为ROS wiki上的一个示例，可以看到图中灰色部分即为静态地图，红色部分为传感器感知到的障碍物，蓝色部分为膨胀层。红色多边形表示机器人的形状，为了避免碰撞，机器人形状不应该和红色部分相交，机器人中心点不应该和蓝色部分相交。
move_base是整个ROS Navigation Stack的最顶层，它将各个功能模块组合起来，通过SimpleActionServer接收目标点并完成导航任务。move_base支持任何遵循在nav_core包中指定的nav_core::BaseGlobalPlanner接口的全局规划器和任何遵循在nav_core包中指定的 nav_core::BaseLocalPlanner 接口的局部规划器。
move_base执行的流程是通过状态机来控制是执行规划行为还是恢复行为，当规划失败就会执行恢复行为，如果所有恢复行为也都失败就会退出导航并报告问题。规划的流程是通过回调函数接收目标点，同时会有一个线程不断地跑全局规划器，然后局部规划器以一定频率对规划出来的路径进行跟踪，计算出速度指令，最终到达目标点就重置规划状态。
nav_core功能包为用于导航的机器人行为提供了通用的接口，其中定义了三个抽象类，分别是BaseGlobalPlanner、BaseLocalPlanner和RecoveryBehavior。
点击可查看大图
BaseLocalPlanner中定义了纯虚函数setPlan()、isGoalReached()和computeVelocityCommands()，setPlan()用于实现设定局部规划器跟踪的路径，isGoalReached()用于实现判断是否到达目标点，computeVelocityCommands()用于实现计算安全的速度指令。RecoveryBehavior定义了纯虚函数runBehavior()，runBehavior()用于实现执行恢复行为。
global_planner功能包的GlobalPlanner继承了nav_core:: BaseGlobalPlanner，实现了makePlan()函数。makePlan()函数的基本流程就是输入起点和终点，读取代价地图，使用A*或者dijkstra算法进行规划，调用OrientationFilter类中的方法对路径进行优化，将规划好的路径以话题的形式发布。
局部规划器是move_base中最复杂的部分，实现局部规划器需要继承nav_core包的BaseLocalPlanner类。常用的局部规划器是dwa_local_planner，dwa_local_planner提供了在二维平面进行局部路径规划的动态窗口法的实现。动态窗口法主要是在速度空间中采样多组速度，并模拟机器人在这些速度下一定时间内的轨迹。在得到多组轨迹以后，对这些轨迹进行评价，选取最优轨迹所对应的速度来驱动机器人运动。
局部规划器的一个基本流程就是读取局部代价地图，设定要跟踪的全局路径，对全局路径进行分段，根据分段的全局路径的坐标进行局部规划，计算机器人每个周期（采样周期）内的线速度、角速度，使之尽量符合全局最优路径，并实现实时避障。
以DWA局部规划器为例，当规划全局路径的线程planThread在控制循环中输出新的路径时，新的路径将通过setPlan()传递给DWAPlannerROS，直接保存为global_plan_。如果isGoalReached()返回false，程序继续执行调用computeVelocityCommands()。
点击可查看大图
上图是move_base默认的恢复行为的执行流程，可以理解为机器人通过旋转底盘，来清除代价地图中的障碍物，经过清除以后如果可以找到可走的路径，就继续导航，否则认为目标点不可达并报告终止导航任务。
从源码来看，move_base中包括了clear_costmap_recovery、move_slow_and_clear和rotate_recovery这三个有关恢复行为的功能包。clear_costmap_recovery实现的是将给定区域内的障碍物从代价地图中清除。
move_slow_and_clear实现的是清除代价地图中的障碍物，并在用户给定的限制速度下移动一定的距离，这可能会导致机器人发生碰撞。rotate_recovery实现的是通过使机器人旋转一周来清除代价地图中的障碍物。
本文介绍了ROS Navigation Stack的整体设计思路和各个功能包的作用以及它们之间的联系，希望能够帮助大家更好地理解ROS Navigation Stack，感谢大家的阅读。
ROS navigation wiki：
http://wiki.ros.org/navigation
ROS move_base wiki：
http://wiki.ros.org/move_base
ROS Navigation Stack github：