Navigation2 算法流程
转自 https://zhuanlan.zhihu.com/p/405670882
此文仅作学习笔记
启动流程
在仿真环境中启动导航包的示例程序，执行nav2_bringup/bringup/launch/tb3_simulation_launch.py文件。ROS2的launch文件支持采用python语言来编写以支持更加复杂的功能，本文件的主要功能如下：
- 声明各参数变量赋值
- 启动gazebo
- 并加载模型
- 启动robot_state_publisher ros包，加载URDF文件发布静态机器人坐标变换关系
- 启动rviz
- 包含
bringup_launch.py文件并传递参数
在bringup_launch.py文件中包含了导航相关的算法启动内容。在文件中实现的主要功能为
- 加载参数
- 如果采用SLAM构建地图则启动SLAM算法文件
slam_launch.py - 不使用SLAM算法，则运行定位算法启动文件
localization_launch.py。 - 运行导航算法启动文件
navigation_launch.py
这里我们仅看导航和定位两个文件。
在location_launch.py文件中启动两个节点，nav2_map_server和nav2_amcl。在navigation_launch.py文件中则启动controller_server、planner_server、recoveries_server、bt_navigator和nav2_waypoint_follower算法包。至于nav2_lifecycle_manager包则为其他所有包的节点管理功能，可以暂时不理会。
因此，Navigation2启动流程如下
定位部分
定位部分代码较为简单，具体功能主要是
- 发布地图信息（
nav2_map_server） - 发布机器人位置信息（
nav2_amcl）
这一部分需要注意的仅仅是AMCL定位时的坐标变换关系，在单纯的里程计中发布的是/odom_frame到\base_frame的坐标关系，而acml确定的是地图坐标系/map_frame和/odom_frame的关系，即可以认为里程计
确定从起始位置出发的局部定位信息，而amcl算法确定局部定位信息和全局地图之间的坐标关系。
导航部分
调用接口流程
Action 机制
Navigation2 高度依赖于ROS的Action通信机制
，因此有必要对其有个大致了解。
基本的通信流程为
- 客户端发布启动消息给服务端
- 服务端响应启动消息，并执行动作
- 服务端在执行动作时返回当前执行动作进度
- 服务端执行动作结束，返回执行结果
- 客户端响应执行结果
根据action消息的特点，将导航节点分为两个部分，一是具体的算法实现作为服务端，二是算法调用逻辑控制
实现作为客户端
算法执行服务端
planner_server路径规划服务器，接受客户端的路径规划请求后计算路径并发布。
接收Action
compute_path_to_posecompute_path_through_poses
发布消息
plan
controller_server机器人控制
服务器，接受客户端开始跟随当前路径的请求，然后发布控制机器人运动指令。
接收Action
follow_path
接收消息
odom（实际上是定位消息）speed_limit
发布控制指令
cmd_vel
recoveries_server故障恢复服务器，在机器人运动异常时进行处理。
接收Action
spinbackupwait
发布控制指令
cmd_vel
根据上述三个模块功能的描述，可以大致的想到导航功能的实现分为首先调用路径规划
算法得到一个可行路线，然后调用跟随路径接口，在跟随的同时监控机器人是否出现异常，若出现异常则打断机器人跟随路径并执行故障恢复，要么后退要么旋转或者等待动态障碍物走开。而Navigation2中的实现则更为具体和详细。
算法调用逻辑客户端
Navigation2 的导航控制逻辑采用行为树实现，该方法常被用在游戏开发中，类似于状态机
的功能但更为强大。
行为树
Navigation2中实现了四种行为树插件，分别为，动作，条件，控制，修饰。接下来简单介绍各自的功能，详细的行为树功能可参考BehaviorTree.CPP库的描述 。
动作行为树的叶子节点，调用一个动作就代表执行一段算法或者控制机器人运动。
条件判断子节点是否执行的条件
控制子节点执行的流程控制，顺序执行
，或循环执行
修饰修饰当前子节点，比如设置子节点循环执行5次，或者修改子节点
返回值
在Navigation2中实现许多的行为树插件，主要有
- 动作：
- 动作：
bt_navigator
将上述行为树
插件组合起来，便可以得到完整的导航控制逻辑，采用xml文件配置得到。
具体采用流程图
的形式表达如下
至此，我们便大致理清了导航包的逻辑。目前对于整体的导航框架，还剩下代价地图
功能没有进行描述。
代价地图
代价地图分为局部代价地图
和全局代价地图，分别在全局路径规划和局部路径规划中新建单独的线程进行计算。代价地图采用多图层的结构，用来确定可行区域以及手动设置限速区域，手动设置静止入内区域等。