点击下方卡片，关注「计算机视觉工坊」公众号
选择星标，干货第一时间送达

来源：计算机视觉工坊

「3D视觉从入门到精通」知识星球(点开有惊喜) ！星球内新增20多门3D视觉系统课程、入门环境配置教程、多场顶会直播、顶会论文最新解读、3D视觉算法源码、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

1、引言

ROS2 Navigation2（简称Nav2）是ROS2中用于移动机器人导航的全新框架，相较于ROS1中的Navigation栈，Nav2可谓是一次“华丽转身”。它不仅重构了整个架构，还引入了行为树（Behavior Tree）等现代技术，BT的引入解决了原先FSM（finite state machine）处理状态切换和逻辑过于复杂和难以复用的问题（详见Behavior Trees的论文，阅读前三章即可完成对BT的深度理解，大概耗时30mins^[1]）。Nav2还提供了更丰富的插件接口（servers & plugins），开发者可以根据需求自定义行为逻辑，为支持用户根据具体业务场景的快速自定义开发提供了有效的解决方案，真正做到“随心所欲而不逾矩”。

另外，Nav2的最大优势在于其对多线程和实时性的支持，这得益于ROS2本身的特性。它摆脱了ROS1中单线程和中心master节点控制的局限，使得复杂任务可以并行处理，效率大幅提升。

网上有开发者调侃道：“以前用ROS1导航，调试参数就像调钢琴，稍有不慎就跑偏；现在用Nav2，参数调节工具更直观，机器人跑得又快又稳，简直是‘导航界的天花板’。” 总结来说，Nav2不仅功能强大，还更加易用，是移动机器人开发者的“新宠”。

而本文致于对Nav2中的框架和核心概念进行解析（内容在第2、3章节），并分享Nav2源码编译安装过程中的避坑（内容在第4、5、6章节），在第7章节进行一些Nav2中关键流程的理解分享，最后的参考文档与网站是本人在学习Nav2和进行本文总结时所参考的理论、知识分享和经验总结。大家可以根据自己的需要，选择合适的章节完成阅读。

注意本人尝试的Nav2源码编译安装是基于ubuntu20.04系统搭配ROs2 Foxy版本，其他系统与ROS版本可基于版本做适当的依赖库准备

2、Nav2的整体框架

话不多说，先上Nav1和Nav2系统框架图：图1:Nav1系统框图图2:Nav2系统框图

由上可见，Nav2完成了去Master中心节点，服务模块BT Navigator Server、Recovery Server、Controller Server和Planner Server组成了Nav2的核心业务，通过Servers的功能集成方案使得系统框架的模块化更加完善。其中，BehaviorTree Navigator Server是Nav2引入的一个“全新”的概念，正是通过BT的所有的Plugins插件，Nav2才优雅地实现了对用户自定义的插件化设计支持，并帮助用户根据业务场景的不同快速完成解决方案的算法更新与集成，避免了在使用Nav1时修改核心逻辑时的开发成本问题。

特性	Nav1	Nav2
架构基础	ROS1，单线程执行	ROS2，支持多线程与分布式通信
任务调度	有限状态机(FSM)，逻辑复杂难扩展	行为树（BT），逻辑模块化且易扩展
容错机制	简单恢复策略，容错能力有限	动态恢复服务器(Recovery Server)，支持多级恢复行为（如清除地图、重规划）
环境建模	单一全局地图，动态障碍处理较弱	全局+局部代价地图分层管理，实时融合传感器数据
多机器人支持	无原生支持	内置多机协作接口，适应仓储、物流等场景
算法扩展	修改核心代码成本高	插件化设计 ，支持快速集成新算法

3、Nav2的核心概念

看过Nav2的系统框架后，接下来就是具体各个Servers的概念和设计目标了，本章节挑选出来的Concepts都是核心中的核心，不仅使用频率高，而且是作为后期用户自定义开发时必须得清楚的关键概念。除此之外，若是大家对其他的模块概念也感兴趣，可以自行跳转Nav2的官方文档网站查询：https://docs.nav2.org/getting_started/index.html

3.1、BT Navigator Server（行为树）

First of First，核心中的核心！BT Navigator Server（行为树）的概念可以说是学习Nav2时不可能绕开的话题，行为树（BT）在复杂机器人任务中占据着重要地位且不可能被撼动。它是一种树形结构，用于表示待完成的任务。与有限状态机（FSM）相比，行为树提供了一个更具可扩展性和人类可理解性的框架，用于定义多步骤或多状态的应用程序。FSM可能包含数十个状态和数百个状态转换，导致复杂性和错误风险增加。例如，在机器人足球比赛中，将比赛逻辑嵌入FSM会面临许多可能的状态和规则，建模选择（如从左侧、右侧或中间射门）可能不清晰。相比之下，行为树允许创建和重用基本原语，例如“kick”“walk”“go to ball”等行为。这不仅简化了任务建模，还提高了代码的复用性。更多关于行为树的详细信息可以参考相关文献，详见Behavior Trees的论文，阅读前三章即可完成对BT的深度理解，大概耗时30mins^[2]。

Nav2项目中的BT的具体实现是采用了BehaviorTree CPP V3作为其行为树库。在BT Navigator中，Nav2创建了可构建为行为树的节点插件（Plugins），这些插件在解析行为树的XML文件时被加载并关联到注册名称，从而实现通过行为树进行导航的功能。使用此库的一个关键原因是其支持子树加载的能力，这意味着Nav2行为树可以作为节点插件集成到更高级别的行为树中。例如，在机器人足球比赛中，Nav2行为树可以用作“go to ball”节点，而足球检测则作为更大任务的一部分。此外，Nav2还提供了一个NavigateToPoseAction插件，允许客户端应用程序通过标准动作接口调用Nav2软件堆栈。

3.2、Lifecycle Nodes and Bond（生命周期节点和绑定）

除了BT之外，Nav2的另一个全新的亮点就来自于Lifeycle：节点生命周期的管理。生命周期节点是ROS2独有的，它们是包含状态机转换的用于加载和卸载ROS 2服务器的节点。这有助于确定ROS系统启动和关闭的状态是否正常。它还帮助用户根据实际用途（商业或调试）以合理的方式构建他们的程序，当一个节点启动时，它处于未配置状态，只处理节点的构造函数，该构造函数不应包含任何 ROS 网络设置或参数读取。通过启动系统或提供的生命周期管理器，需要通过配置将节点转换为非活动状态。之后，可以通过激活阶段的转换来激活节点。

这种状态将允许节点处理信息，并完全设置为运行状态。在配置阶段，触发 on_configure() 方法，将设置所有参数、ROS网络接口，以及安全系统，所有动态内存的分配。在激活阶段、触发 on_activate() 方法的将激活ROS网络接口，并设置程序中的任何状态以开始处理信息。要关闭（该节点）即过渡到停用需要清理、关闭，并以最终状态结束。网络接口分别在这些阶段被停用并停止处理、释放内存、干净地退出。生命周期节点框架在整个项目中被广泛使用，所有服务器都使用它。如果可能的话，所有ROS系统最好使用生命周期节点。

在Nav2中，我们使用 nav2_util LifecycleNode 的包装器。这种包装包装了典型应用的生命周期节点的许多复杂性。它还包括生命周期管理器的连接 bond ，以确保服务器转换后，它也保持活动状态。如果服务器崩溃，它会让生命周期管理器知道并向下过渡系统，以防止严重故障。

3.3、Action Server（动作服务器）

动作服务器是Nav2中另一个关键概念，它允许客户端以异步方式请求复杂任务的执行，并在任务执行期间接收反馈。Nav2中的许多功能，例如导航到目标点（NavigateToPose）、跟随路径（FollowPath）和回环（Spin），都是通过动作服务器实现的。

动作服务器的核心思想是将任务分解为多个阶段，并在每个阶段提供反馈。例如，在导航到目标点的过程中，动作服务器会持续更新机器人的当前位置、目标距离以及路径规划的状态。这种机制不仅增强了任务的透明度，还允许客户端根据反馈动态调整任务参数。

Nav2的动作服务器基于ROS2的动作框架构建，支持多线程处理，能够同时处理多个任务请求。此外，Nav2还提供了多种预定义的动作类型，开发者可以通过插件机制轻松扩展自定义动作。

在实际应用中，动作服务器的使用非常灵活。例如，开发者可以创建一个自定义动作，用于执行特定的机器人行为（如抓取物体或避开动态障碍）。通过将动作服务器（Action Server）是Nav2中另一个核心概念，它允许客户端与服务器之间进行长时间的任务交互。与服务（Service）不同，动作服务器支持任务的反馈和状态更新，非常适合导航任务这种需要持续监控进度的场景。

在Nav2中，NavigateToPose 是最常用的动作之一，它允许机器人从当前位置导航到目标位置。动作服务器通过目标（Goal）、反馈（Feedback）和结果（Result）三个主要部分与客户端交互。客户端发送目标请求后，服务器会持续更新任务状态，并在任务完成后返回结果。此外，Nav2还提供了多种内置动作，如 FollowPath、Spin 和 BackUp，开发者可以根据需求选择或自定义动作。通过插件机制，用户可以轻松扩展动作服务器的功能，满足特定场景下的导航需求。

动作服务器的引入不仅增强了Nav2的灵活性，还为复杂任务的实现提供了强有力的支撑。例如，在多机器人协作场景中，动作服务器可以协调多个机器人之间的任务分配与执行，确保任务高效完成。

3.4、Navigation Servers（导航服务器）

介绍完上述的三个Nav2中的全新概念后，我们就可以来到较为熟悉的规划器、控制器与恢复器了。与ROS1 Navigation1一样，规划器、控制器与恢复器仍是作为导航任务中的三大核心组件（由Nav2的系统框图可见其分量），与Nav1有所区别的是，这三大服务都是通过Plugin的形式在BT的XML进行配置和加载，加载后的这三大服务则在行为树（BT）中作为单独的action servers被控制。除此之外，导航的三大核心服务的功能上与Nav1中相似：

• 规划器负责生成全局路径；

• 控制器用于实时跟踪路径并调整机器人运动；

• 恢复器则在机器人陷入困境或遇到问题时提供解决方案，确保系统具备容错能力；

如上文所说的一样，在Nav2中这三个组件以动作服务器（Action Server）的形式实现，分别为规划器服务器、控制器服务器和恢复器服务器。这些服务器不仅托管了各自的核心算法插件，还管理了算法运行所需的环境表达（如代价地图）。通过这种方式，Nav2能够灵活地支持多种算法插件，并在运行时动态配置（这就是与Nav1的核心区别所在）。

规划器和控制器服务器支持在启动时通过名称（别名）和算法类型进行配置。例如，DWB控制器可以被命名为FollowPath，其所有参数将被放置在对应的规划器、控制器和恢复器是导航任务中的三大核心组件。规划器负责生成全局路径，控制器用于实时跟踪路径，而恢复器则在机器人陷入困境或遇到问题时提供解决方案，从而增强系统的容错能力。在Nav2项目中，这些组件以动作服务器的形式存在，分别称为规划器服务器、控制器服务器和恢复器服务器。

这些服务器不仅托管具体的算法插件，还管理算法运行所需的环境表达（如代价地图）。通过配置，用户可以为每个服务器指定别名及对应的算法类型，例如使用DWB控制器时，可将其命名为FollowPath，所有相关参数则存储在该命名空间下（如FollowPath.<param>）。

规划器和控制器服务器对外暴露动作接口，行为树（BT）中的节点可通过调用这些接口来执行任务。服务器内部通过映射算法名称（如FollowPath）调用具体算法，从而实现行为树中算法的抽象化。例如，用户可以定义多种插件控制器，分别用于路径跟随、对接充电器、避障或与工具交互。这种设计避免了环境表达对象的重复创建，降低了计算开销。

恢复器服务器同样包含插件化的恢复动作，但每个恢复插件会公开独立的操作服务器，因为恢复动作通常无法共享统一接口。此外，恢复器服务器订阅本地代价地图，以接收来自控制器服务器的实时更新，从而避免多次创建代价地图实例，进一步优化性能。如果需要扩展新的动作服务器，建议优先复用现有框架，仅在必要时创建新插件和BT节点，以保持Nav2代码库的简洁与高效。

3.4.1、Planner Server（规划服务器）

规划器的任务是计算完成一些目标函数的路径。根据所选的命名法和算法，该路径也可以称为路线。两个典型示例是计算一个到达目标位姿的规划（例如从当前位置到达一个目标位姿）或者完全覆盖（例如覆盖所有空闲空间的规划）。规划器可以访问全局环境表达和缓存在其中的传感器数据。规划器可以被编写为具有以下功能的工具：

• Compute shortest path

• Compute complete coverage path

• Compute paths along sparse or predefined routes

Nav2中规划器的一般任务是计算从当前位置到达目标位姿的一个有效且可能是最佳的路径。但是，有许多受支持的规划类和路线类。

3.4.2、Controller Server （控制服务器）

控制器，在ROS 1中也被称为局部规划器，是我们跟随全局计算路径或完成局部任务的方法。控制器有权访问局部环境表达，以尝试计算要跟随的基准路径的可行控制工作。许多控制器会将机器人向前投射到空间中，并在每次更新迭代时计算局部可行路径。控制器可以被编写为具有以下功能的工具：

• Follow a path

• Dock with a charging station using detectors in the odometric frame

• Board an elevator

• Interface with a tool 在Nav2中，控制器的一般任务是计算一个有效的控制工作以跟随全局规划路径。然而，有多个控制器类和局部规划器类。Nav2项目的目标就是所有控制器算法都可以作为此服务器中的插件，以用于一般研究和产业任务中。

3.4.3、Behavior & Recovery Server（行为和恢复服务器）

恢复器是容错系统的支柱。恢复器的目标是处理系统的未知状况或故障状况并自主处理这些状况。例子包括感知系统中会导致环境表达充满假障碍物的故障。这样就会触发清除成本地图恢复以允许机器人移动。

另一个例子就是机器人由于动态障碍物或控制不佳而卡住。在允许的情况下，倒退或原地旋转会允许机器人从卡住的位置移动到可以成功进行导航的自由空间中。

最后，在完全故障的情况下，可以实施恢复以引起操作员的注意以寻求帮助。这可以通过电子邮件、短信、Slack、Matrix等来完成。

需要注意的是，行为服务器不仅能够持有恢复行为，还可以共享访问昂贵的资源，例如代价地图或TF缓冲区等。这意味着行为服务器的功能不仅仅局限于恢复行为，它可以适用于更广泛的场景。每个行为可能拥有自己的API，这为开发者提供了更大的灵活性和定制空间。

在Nav2中，这种设计使得行为服务器能够更好地管理不同类型的资源和任务逻辑。例如，某些行为可能需要频繁访问代价地图以进行路径规划或障碍物检测，而另一些行为则可能依赖TF缓冲区来处理坐标变换。通过将这些资源的访问权限集中管理，行为服务器不仅提高了资源利用率，还简化了插件开发的复杂度。

此外，由于每个行为可以拥有独立的API，开发者可以根据具体需求设计行为逻辑，而无需修改核心代码。这种插件化的设计理念极大地增强了Nav2的可扩展性和适应性，使其能够满足不同应用场景下的导航需求。

4、Nav2源码编译环境准备

介绍了那么多，现在终于到了可以动手操作，在实践中感受Nav2魅力的时刻了。本章主要是先将ubuntu中的环境一次性搭建完成，为下一章节的Nav2源码编译安装做准备。

4.1、依赖库的基本安装操作

本次的源码编译，不使用基于rosdep自动查询进行依赖库的安装：

rosdep install -y --from-paths src --ignore-src --rosdistro foxy

这样是为了在CI/CD搭建时和作业机器人系统搭建时的方便性，可直接将完整依赖的系统镜像一次性部署，部署的基本条件是完成了ros-foxy-desktop（桌面版）的安装

sudo apt update
sudo apt install ros-foxy-desktop

随后，安装Nav2所需的所有依赖库：

sudo apt install libgeographic-dev ros-foxy-geographic-msgs ros-foxy-behaviortree-cpp-v3 libgraphicsmagick++-dev graphicsmagick-libmagick-dev-compat libompl-dev ros-foxy-gazebo-ros-pkgs

4.2、behaviortree_cpp_v3库的二进制安装问题

behaviortree_cpp_v3库是Nav2编译时的必须依赖项，若是在上述的依赖库安装中报错找不到behaviortree_cpp_v3的话，可以先尝试 sudo apt update 更新下镜像源，若还是不行的话就需要通过源码安装 BehaviorTree.CPP v3了：

# 进入工作空间src目录
mkdir -p ~/nav2_ws/src
cd ~/nav2_ws/src

# 克隆 BehaviorTree.CPP 仓库
git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git
cd BehaviorTree.CPP
git checkout 3.8.0  # 使用与Nav2兼容的版本

# 返回工作空间根目录并编译
cd ~/nav2_ws
colcon build --symlink-install --packages-select behaviortree_cpp_v3

需要注意的是上述代码块中的~/nav2_ws/src也是接下来nav2源码下载的路径位置。

5、Nav2编译安装

5.1、Nav2的基本编译安装流程

完成上述的依赖库安装后，我们就可以进行Nav2的编译安装了：

mkdir -p ~/nav2_ws/src //若是路径已经存在，则跳过文件夹生成这步
cd ~/nav2_ws/src

//注意，由于本人测试的系统是ubuntu20.04系统搭配ROs2 Foxy版本，所以下载的Nav2源码版本也是 foxy，-b 的命令就是选择具体的代码分支，不然默认就是最新的main分支
git clone https://github.com/ros-planning/navigation2.git -b foxy

cd ~/nav2_ws
colcon build

以上，若是没有出现任何报错则完成了Nav2的源码编译工作了。

5.2、一定要注意注意版本：

• 确保所有仓库都切换到foxy分支

• 如果使用其他ROS2发行版，需要调整分支名称

6、Nav2仿真测试

若是上述的依赖库和源码编译安装都顺利完成了，接下来就可以进行仿真的测试了。但朋友们，先别欢呼，一般情况下仿真测试的问题和坑是最多的，但愿大家能直接仿真测试成功，同时也希望我的趟坑经历能帮助到大家。

本章的结构：4.1是基本的测试操作，4.2中是一些常见测试问题的解决方案。

6.1、Nav2的导航仿真测试

6.1.1、Nav2相关的环境变量配置

ROS2 Foxy的仿真测试，一定离不开 turtlebot，所以我们需要在这里二进制安装 turtlebot 的系统默认库：

sudo apt install ros-foxy-turtlebot3*

接下来就是配置一些仿真所需的环境变量了：

source /opt/ros/foxy/setup.bash //该命令一般都会配置到~/.bashrc中，若已配置则不需要额外输入

cd ~/nav2_ws
source install/setup.bash //每次新开终端中，执行Nav2的相关进程都需要输入这个命令
export TURTLEBOT3_MODEL=burger  //不建议使用waffle，foxy中waffle的模型有些小问题
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/foxy/share/turtlebot3_gazebo/models

上面中配置的环境变量只是临时生效，若是想要将 TURTLEBOT3_MODEL 和 GAZEBO_MODEL_PATH 的配置长期生效则可以在终端中输入一下命令：

echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc
echo "export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/foxy/share/turtlebot3_gazebo/models" >> ~/.bashrc

6.1.2、运行Nav2仿真示例

输入和执行以下命令测试上述中Nav2是否安装成功，注意需要保证上述环境的配置正确：

ros2 launch nav2_bringup tb3_simulation_launch.py

运行上述命令后，正常的情况是能启动rviz2、gazebo，且终端输出的日志中没有error等级的报错，详见下图：可以看见此时rviz2中还没有显示完整的地图、传感器数据和机器人数据，需要点击rviz2中左下角处的“Startup”按钮，按下后则可以让rviz2完成初始化和正常显示相关信息了，见下图（但也有可能不需要点击“Startup”按钮，rviz2直接就能显示信息）。以上所有的节点进程被正确启动，接下来就可以进行turtlebot的导航主流程了，首先点击rviz2中显示界面上方标签栏“2D Pose Estimate”按钮，然后在地图中设置机器人的初始位置和朝向，可以多次下发机器人初始位姿直到机器人的定位与实际位姿一致，见下图：一旦下发了正确的“2D Pose Estimate”，tf tree将变得完整，Nav2也将完全激活并准备好运行。此时应该能看到机器人和粒子云了，如下图：随后，点击“Navigaton2 Goal”按钮并选择goal的位姿，此时Nav2将调用 BT navigator 通过 action server 前往该目标点。在导航过程中，可以通过 rviz2 左下角处的插件“Pause”或“Reset”来影响导航动作。以上，就是最简单的Nav2仿真测试的全部流程了。

6.2 Nav2仿真测试常见问题及解决方案

6.2.1 Gazebo启动失败

首先确认 Gazebo 能否单独启动 gazebo --verbose，若是不能单独启动成功则说明 Gazebo 未正确安装，需重新安装：

sudo apt install gazebo11 libgazebo11-dev ros-foxy-gazebo-ros-pkgs

如果能正常单独启动 Gazebo，且能打开空白世界，问题可能出在 ROS2 与 Gazebo 的集成，可跳转至4.2.2，查看问题处理办法。

6.2.2 启动Gazebo时一直在preparing

如果在 ROS2 Foxy 中使用 Gazebo 仿真时地图加载缓慢或无法打开，可以通过预加载地图缓存 来优化体验。一般情况下，Gazebo 会在 ~/.gazebo 下自动缓存模型，但可以手动加速，也是解压至~/.gazebo/models的目录下：

# 下载常用模型（如TurtleBot3环境）
wget -P ~/.gazebo/models http://models.gazebosim.org/turtlebot3_worlds/model.tar.gz
tar -xzf ~/.gazebo/models/model.tar.gz -C ~/.gazebo/models

如果上述出现下载失败的问题，可以先完成下面的步骤，然后跳转至4.2.3章节处理。

# 下载离线模型包
wget https://github.com/osrf/gazebo_models/archive/refs/heads/master.zip
unzip master.zip -d ~/.gazebo/
mv ~/.gazebo/gazebo_models-master ~/.gazebo/models

完成离线地图包的下载后，需要调整下 Gazebo 模型的路径配置：

echo "export GAZEBO_MODEL_PATH=$HOME/.gazebo/models:/usr/share/gazebo-11/models" >> ~/.bashrc
source ~/.bashrc

如此，若是都能执行成功，则说明 gazebo models 的问题已经被解决，接下来再根据6.1章节进行测试即可。

6.2.3 turtlebot3_worlds的gazebo model下载失败

如果下载失败，多半是git的链接被墙或者直接失效404了。但不要担心，有最无脑的解决方案，那就是先安装安装 TurtleBot3 相关软件包，再将 model 的文件复制粘贴至我们之前配置好的 GAZEBO_MODEL_PATH 路径下即可。

安装 TurtleBot3 相关软件包，TurtleBot3 的 Gazebo 仿真包需要手动安装：

# 安装核心包
sudo apt install ros-foxy-turtlebot3* ros-foxy-gazebo-ros-pkgs

# 安装额外的依赖（如果后续步骤报错）
sudo apt install ros-foxy-cartographer ros-foxy-navigation2

随后，将二进制安装的turtlebot3_gazebo中/opt/ros/foxy/share/turtlebot3_gazebo/models的文件复制粘贴到~/.gazebo/models下，即可解决 gazebo model 的问题。

接下来再根据6.1章节进行测试即可。

7、Nav2示例中的关键流程拆分

本章节的展开将采取一种新的方式，考虑到全文篇幅，文章中不再贴上完整的文件代码，而是直接给出源码仓库中的具体文件和具体行数，然后进行说明，因此有条件的小伙伴建议打开Nav2源码后配合本章节的内容，“消化和吸收”的效果更佳喔。 在章节6.1.2中，我们通过输入命令：

ros2 launch nav2_bringup tb3_simulation_launch.py

启动了turtlebot3的仿真，现在我们就从这条命令行开始，逐步地由外至内，剖析关键语句的含义与作用。tb3_simulation_launch.py文件在nav2的仓库中是/navigation2/nav2_bringup/bringup/launch/tb3_simulation_launch.py，进入到launch文件后，直接转到第180行，可见：

ld = LaunchDescription()
//中间有部分省略
...
    ld.add_action(declare_slam_cmd) //加载 slam 动作，但argument设置为false，即不激活
...
    ld.add_action(declare_params_file_cmd) // nav2的参数读取动作，例如加载定位节点（amcl）、map-server地图加载服务、bt_navigator服务配置和插件名声明、控制服务器和规划服务器等
    ld.add_action(declare_bt_xml_cmd) //通过xml加载用户自定义的行为树板块
...
    ld.add_action(start_robot_state_publisher_cmd) //启动加载机器人urdf和发布关节静态tf
    ld.add_action(rviz_cmd) //启动rviz2和参数
    ld.add_action(bringup_cmd) //启动bringup.py的子启动脚本，并加载相关参数

其中变量名ld是launch description的缩写，通过ld的实例化对象加载一个一个的action servers，这里就跟上文中3.1和3.3对应上了。通过对需要加载的每个action arguments进行配置，实现了action servers在启动时的自定义传参。 随后，在6.1.2中完成启动后，我们会先点击rviz2中显示界面上方标签栏的“2D Pose Estimate”按钮，rviz2由此下发定位模块中amcl所需的/initialpose话题，amcl定位节点则通过对/initialpose 话题的订阅完成粒子滤波器的初始化和机器人位姿的发布流程，详见navigation2-foxy-devel/nav2_amcl/src/amcl_node.cpp中的 initialPoseReceived回调函数和handleInitialPose粒子滤波位姿初始化函数：

void AmclNode::initPubSub()
{
...
// initial_pose_sub_为订阅"initialpose"话题的订阅器，回调函数为initialPoseReceived
  initial_pose_sub_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
    "initialpose", rclcpp::SystemDefaultsQoS(),
    std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1));
...

具体的amcl算法理论和实现细节在本文中不是重点，就不展开说明了，大家只要明白通过Rviz下发的/initialpose话题可以帮助到amcl定位模块完成初始化，并支持下一步的导航流程，这样就足够了。

当完成了定位节点的位姿初始化后，根据6.1.2中的仿真测试流程，接下来就是在 RViz2 中点击 【Navigation2 Goal】 的按钮了，并且可以发现在点击了该 goal 的按钮后，机器人会从当前位姿跟随着一条trajectory轨迹逐步走到当初下发goal所在的位姿。那么当在 RViz2 中点击 Navigation2 Goal 按钮时，RViz2 发送一个消息类型为 geometry_msgs/msg/PoseStamped（包含目标位置和朝向（x, y, z 和四元数姿态））的话题：/goal_pose。在nav2源码仓库中搜索 goal_pose 话题的订阅器就可以找到其话题是由Nav2 的 bt_navigator 接收，文件所在路径为/nav2_ws/src/navigation2/nav2_bt_navigator/src/bt_navigator.cpp，

nav2_util::CallbackReturn
BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
...
// Support for handling the topic-based goal pose from rviz
  client_node_ = std::make_shared<rclcpp::Node>("_", options);
  self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(
    client_node_, "navigate_to_pose");
...
  goal_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
    "goal_pose",
    rclcpp::SystemDefaultsQoS(),
    std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));
...

//该函数负责：
//  - 将目标传递给行为树（BT）。
//  - 启动导航任务（如全局规划、局部控制等）。
void
BtNavigator::onGoalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr pose)
{
  nav2_msgs::action::NavigateToPose::Goal goal;
  goal.pose = *pose;
  self_client_->async_send_goal(goal);
}

由此可见，BtNavigator类在被触发了 goal_pose 的订阅器后是将目标点位姿转成了action 的对象类型 NavigateToPose：：Goal 用于导航到该位姿，其中 self_client_ 是 rclcpp_action::Client类型的客户端，功能是异步发送导航目标到 Action Server，如 nav2_bt_navigator。 分析到这里的时候，我们需要再进一步分析代码中 BtNavigator::on_configure 的内容：

nav2_util::CallbackReturn
BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
...
// Step 1 Communication Setup
  action_server_ = std::make_unique<ActionServer>(
    get_node_base_interface(),
    get_node_clock_interface(),
    get_node_logging_interface(),
    get_node_waitables_interface(),
    "navigate_to_pose", std::bind(&BtNavigator::navigateToPose, this), false);

// Step 2 Loads plugin libraries for custom BT nodes
// Get the libraries to pull plugins from
  plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array();
  global_frame_ = get_parameter("global_frame").as_string();
  robot_frame_ = get_parameter("robot_base_frame").as_string();
  transform_tolerance_ = get_parameter("transform_tolerance").as_double();
// Create the class that registers our custom nodes and executes the BT
  bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);

...
// Step 3 Get the BT filename to use from the node parameter, essential navigation parameters like frames, timeout, etc.
  get_parameter("default_bt_xml_filename", default_bt_xml_filename_);

// Step 4 Loads default Behavior Tree XML file
if (!loadBehaviorTree(default_bt_xml_filename_)) {
    RCLCPP_ERROR(get_logger(), "Error loading XML file: %s", default_bt_xml_filename_.c_str());
    return nav2_util::CallbackReturn::FAILURE;
  }

return nav2_util::CallbackReturn::SUCCESS;
}

上述代码中需要特别注意的就是 action_server_ 这个对象，之前我们讲到在BT中所有任务的管理和执行都是通过 action aerver，而在本例中 action_server_ 是被创建的一个名为 navigate_to_pose 的 action服务器，并将其绑定到BtNavigator类中的navigateToPose成员函数，当有客户端发送navigate_to_pose类型的行动请求时，行动服务器将调用BtNavigator::navigateToPose函数来处理请求，navigateToPose() 是 BtNavigator 的核心方法，用于执行基于行为树（BT）的导航任务，我们最后再进入该关键类函数，看看它实现了哪些关键的功能：

void BtNavigator::navigateToPose()
{
// Step 1 进行初始化检查：
//  1# initializeGoalPose() 初始化目标位姿 
//  2# is_canceling lambda 检查动作服务器状态和取消请求
  initializeGoalPose();
auto is_canceling = [this]() {
    ...  // 省略
    };

std::string bt_xml_filename = action_server_->get_current_goal()->behavior_tree;
// Empty id in request is default for backward compatibility
  bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename;

// Step 2 关键函数：加载指定的行为树文件，失败则终止任务
if (!loadBehaviorTree(bt_xml_filename)) {
    RCLCPP_ERROR(
      get_logger(), "BT file not found: %s. Navigation canceled.",
      bt_xml_filename.c_str());
    action_server_->terminate_current();
    return;
  }

... 

// Step 3 反馈处理 on_loop lamda表达式实时更新导航反馈：
// -当前位姿
// -剩余距离
// -恢复次数
// -导航耗时
auto on_loop = [&]() {
      ... // 省略
      action_server_->publish_feedback(feedback_msg);
    };

// Step 4 核心语句，执行行为树：按照加载的行为树规则，逐个执行行为树的各个节点
// Execute the BT that was previously created in the configure step
  nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling);
// 资源管理和清理：确保行为树停止所有正在执行的动作
  bt_->haltAllActions(tree_.rootNode());

// Step 5 结果处理：根据行为树返回状态（SUCCEEDED/FAILED/CANCELED）触发对应动作服务器响应。
switch (rc) {
    ...  // 省略
  }
}

自此，BtNavigator::on_configure主要工作就是准备好配合 RVIZ 的 /goal_pose 订阅器，加载基于 BT 的 action-based navigation。BT 处理导航的逻辑是bt_navigator 使用预定义的行为树（此次测试中是 navigate_w_replanning_and_recovery.xml）执行导航任务，实现导航任务则是主要调用BT加载的：ComputePathToPose（全局规划）和FollowPath（局部跟踪）这两大动作服务器。介绍到这里，我们不难得出当前的导航任务下的关键节点与函数总结：

节点	关键函数	作用
RViz2	发布 PoseStamped 到 /goal_pose	发送用户点击的目标
bt_navigator	onGoalReceived()	接收目标并触发行为树
controller_server	FollowPath()	执行局部路径跟踪
planner_server	computePathToPose()	生成全局路径

最后再不妨结合下 navigate_w_replanning_and_recovery.xml中配置的规则，看看BtNavigator类中加载完成后的行为树的结构到底长啥样：

对于上面的行为树的整个执行流程，首先解释一下每个节点对应的含义：

1. Recovery Node：RecoveryNode 是一种用于恢复故障的行为树控制节点。它首先尝试执行 Child1，如果 Child1 成功，那么整个节点成功。如果 Child1 失败，它会执行 Child2，如果 Child2 成功，那么再次执行 Child1。如果任何一个子节点返回 RUNNING，整个节点将保持 RUNNING 状态。只有当 Child2 也失败时，整个 RecoveryNode 才会返回 FAILURE，表示无法解决问题。

2. PipelineSequence：也是行为树控制节点，可以有多个孩子，这里假设由两个孩子，左边的孩子是child1，右边的孩子是child2，如果child1执行成功，则会执行child2，如果child2返回RUNNING，则又会去执行child1，主要特点是在某个子节点返回 RUNNING 时，会重新触发之前的所有子节点。

3. RateController：属于控制节点，他表示按照多少频率去执行他的孩子。

4. SequenceStar：也属于控制节点，可以有多个孩子，会依次从左到右执行孩子节点。

上面的绿色节点都是控制类节点，起到一个流程控制作用，蓝色节点是动作节点，也就是需要真正执行相关执行的任务。对于上述行为树，首先，程序以Main Tree为起点开始执行，当执行到NavigateRecovery(Recovery类型)节点时，会先执行他的左孩子节点NavigateWithReplanning(PipelineSequence类型)节点，这个节点又会先执行其左孩子节点RateController，RateController节点会按照参数hz频率执行他的子节点ComputePathToPose(Recovery类型)节点，进而按照规则，ComputePathToPose(Recovery类型)节点又会执行他的左孩子节点ComputePathToPose动作节点。当我们执行到叶子节点时，也就是具体最终执行的动作，比如ComputePathToPose节点，就是规划机器人起点到终点的全局路径。下图是在每个节点都返回成功状态时，行为树的执行流程（1->2->3->4->5->6）。（本小段参考的是Chen Jackie大佬的文章内容《深度梳理：ROS2 Navigation2导航程序框架（一） - Chen Jackie的文章：https://zhuanlan.zhihu.com/p/662085768》）

至此，本章节Nav2示例中的关键流程拆分就基本结束了，如果需要继续深入每个执行器的注册、加载，以及BT类对象的注册、加载、任务执行的实现流程和逻辑等，本文也不再讨论，感兴趣的小伙伴可以选择研读参考网站的【1】和【2】，Chen大佬分析地相当透彻了，大家可以直接食用。

参考网站

1. 深度梳理：ROS2 Navigation2导航程序框架（一） - Chen Jackie的文章：https://zhuanlan.zhihu.com/p/662085768

2. 深度梳理：ROS2 Navigation2导航程序框架（二） - Chen Jackie的文章：https://zhuanlan.zhihu.com/p/662142400

3. Navigation Concepts：https://docs.nav2.org/concepts/index.html#

4. 导航相关概念：http://dev.nav2.fishros.com/doc/concepts/index.html

5. ROS2极简总结-Nav2-概述（上）规划器：https://blog.51cto.com/u_12369060/4993707

6. ROS2极简总结-Nav2-概述（中）控制器：https://blog.51cto.com/u_12369060/3768323

7. ros1仿真导航机器人 navigation：https://blog.csdn.net/ohhjack/article/details/140102791

参考资料

[1] 

详见Behavior Trees的论文，阅读前三章即可完成对BT的深度理解，大概耗时30mins: https://arxiv.org/abs/1709.00084

[2] 

详见Behavior Trees的论文，阅读前三章即可完成对BT的深度理解，大概耗时30mins: https://arxiv.org/abs/1709.00084

3D视觉硬件

3D视觉学习圈子

「3D视觉从入门到精通」知识星球(点开有惊喜) ！星球内新增20多门3D视觉系统课程、入门环境配置教程、多场顶会直播、顶会论文最新解读、3D视觉算法源码、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎扫码加入！

3D视觉全栈学习课程：www.3dcver.com

3D视觉交流群成立啦

点这里👇关注我，记得标星哦～

一键三连「分享」、「点赞」和「在看」

3D视觉科技前沿进展日日相见 ~