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_pose
• compute_path_through_poses

发布消息

• plan

controller_server机器人控制

服务器，接受客户端开始跟随当前路径的请求，然后发布控制机器人运动指令。
接收Action

• follow_path

接收消息

• odom （实际上是定位消息）
• speed_limit

发布控制指令

• cmd_vel

recoveries_server故障恢复服务器，在机器人运动异常时进行处理。
接收Action

• spin
• backup
• wait

发布控制指令

• cmd_vel

根据上述三个模块功能的描述，可以大致的想到导航功能的实现分为首先调用路径规划

算法得到一个可行路线，然后调用跟随路径接口，在跟随的同时监控机器人是否出现异常，若出现异常则打断机器人跟随路径并执行故障恢复，要么后退要么旋转或者等待动态障碍物走开。而Navigation2中的实现则更为具体和详细。

算法调用逻辑客户端

Navigation2 的导航控制逻辑采用行为树实现，该方法常被用在游戏开发中，类似于状态机

的功能但更为强大。

行为树

Navigation2中实现了四种行为树插件，分别为，动作，条件，控制，修饰。接下来简单介绍各自的功能，详细的行为树功能可参考BehaviorTree.CPP库的描述 。

动作行为树的叶子节点，调用一个动作就代表执行一段算法或者控制机器人运动。

条件判断子节点是否执行的条件

控制子节点执行的流程控制，顺序执行

，或循环执行

修饰修饰当前子节点，比如设置子节点循环执行5次，或者修改子节点

返回值

在Navigation2中实现许多的行为树插件，主要有

• 动作：
• 动作：

bt_navigator

将上述行为树

插件组合起来，便可以得到完整的导航控制逻辑，采用xml文件配置得到。
具体采用流程图

的形式表达如下

至此，我们便大致理清了导航包的逻辑。目前对于整体的导航框架，还剩下代价地图

功能没有进行描述。

代价地图

代价地图分为局部代价地图

和全局代价地图，分别在全局路径规划和局部路径规划中新建单独的线程进行计算。代价地图采用多图层的结构，用来确定可行区域以及手动设置限速区域，手动设置静止入内区域等。