Nav2 行为树（Behavior Tree）技术详解

摘要

Nav2（Navigation 2）是ROS 2生态中生产级的移动机器人导航框架，其最显著的设计变革在于采用行为树（Behavior Tree, BT） 作为导航任务的核心编排机制。行为树替代了ROS 1 Navigation中硬编码的状态机，将路径规划、运动控制、故障恢复等独立功能模块以插件化节点的形式灵活组合，实现如"搭乐高积木"般的导航行为定制。本文系统讲解行为树在Nav2架构中的定位、四类核心节点的功能与实现原理、XML构建语法、典型导航场景的树结构分析，以及自定义行为树节点的开发方法。

1. 行为树在Nav2架构中的定位与作用

1.1 整体架构概述

Nav2采用模块化服务器架构，将导航功能拆分为多个独立的ROS 2生命周期节点（Lifecycle Node），通过行为树统一编排与调度。核心架构可分为"一大三小"四个服务器：

BT Navigator Server（行为树导航服务器） ：高层协调层，负责加载并执行XML定义的行为树，调度下层各功能模块。它本身是一个ROS 2 Action Server，对外暴露 NavigateToPose、NavigateThroughPoses 等导航接口。
Planner Server（规划服务器）：响应行为树节点的请求，计算全局路径。支持多种规划器插件（如Smac、Theta*、NavFn）的动态切换。
Controller Server（控制服务器）：执行局部路径跟踪（ROS 1中称为局部规划器），输出速度指令驱动机器人沿全局路径运动。支持DWB、RPP、TEB等多种控制器插件。
Behavior Server（行为/恢复服务器）：管理故障恢复行为（如原地旋转Spin、后退BackUp、等待Wait等），在导航异常时接管控制以帮助机器人脱困。

此外，Nav2还包含Smoother Server （路径平滑器，优化路径的光滑性与运动学可行性）和Waypoint Follower（路点跟随器）等辅助服务器，均可通过行为树节点调用。

图1展示了Nav2的整体架构，清晰体现了行为树与各功能模块/服务器之间的交互关系：

图1 Nav2架构概览

1.2 行为树作为导航大脑

行为树在Nav2中扮演决策与任务编排的核心引擎 角色。Nav2使用基于 BehaviorTree.CPP v3/v4 开源库的 BehaviorTreeEngine 作为运行时执行环境，该引擎读取XML格式的行为树描述文件，动态加载并实例化BT节点插件，按树结构的层级逻辑执行导航任务。

行为树的运行机制是：每个节点执行后向其父节点返回三种状态之一------SUCCESS（成功）、FAILURE（失败）或 RUNNING（执行中）。父节点根据子节点的返回状态和自身类型（序列、回退、并行等）决定下一步执行哪个子节点。这一"tick"驱动的执行模型使复杂导航行为的切换逻辑清晰可预测。

行为树替代了ROS 1 Navigation中耦合严重的状态机实现，带来了以下关键优势：

模块化与可组合性：各功能单元（规划、控制、恢复等）作为独立的BT节点插件存在，可通过XML自由组合，修改导航行为无需改动C++源码。
高度可重构性：用户可在行为树、核心算法、状态检查器等多个层面选择或替换插件，实现针对特定机器人平台与应用场景的定制导航逻辑。
清晰的故障处理语义 ：通过 RecoveryNode、ReactiveFallback、RoundRobin 等控制节点，故障恢复逻辑可分层、分上下文地嵌入行为树。
运行时可视化与调试：行为树执行状态可通过Groot2工具实时监控，方便开发与调试。

2. 核心节点类型详解

Nav2行为树中的节点是构成导航逻辑的基本单元。每个节点在行为树中被"tick"（时钟驱动调用）时，执行其功能并向父节点返回状态。Nav2沿用了BehaviorTree.CPP的四类标准节点分类，并在此基础上扩展了大量导航专用的自定义节点。

根据Nav2的插件注册表统计，nav2_behavior_tree 包提供了80余个预置BT节点，涵盖：Action节点（50+） 、Condition节点（17个） 、Control节点（5个） 和 Decorator节点（7个）。本节按功能类别进行详细说明。

2.1 Action节点（Action Nodes）

概念与特征 ：Action节点执行需要一定时间完成的操作，如调用ROS 2 Action Server进行路径规划或路径跟踪。其特征为：执行过程中返回 RUNNING，完成后返回 SUCCESS 或 FAILURE；可与ROS 2 Action Server、Service Server或同步逻辑交互；支持执行过程中的取消（halt）操作。

实现原理 ：Nav2中的大部分Action节点继承自 nav2_behavior_tree::BtActionNode<T> 模板类，该模板封装了ROS 2 Action Client的完整生命周期------发送Goal、监控状态、处理结果/取消/中止等。开发者只需覆写 on_tick()、on_success() 等虚函数即可实现特定逻辑。

核心导航Action节点一览：

节点名称	功能描述	关键输入端口	关键输出端口	备注
`ComputePathToPose`	调用Planner Server计算到目标点的路径	`goal`（目标位姿）, `planner_id`（规划器ID）	`path`（计算出的路径）, `error_code_id`（错误码）	最重要的规划节点之一
`ComputePathThroughPoses`	计算通过多个中间路点的路径	`goals`（路点列表）, `planner_id`	`path`, `error_code_id`	用于多路点导航
`FollowPath`	调用Controller Server按路径控制机器人运动	`path`（路径）, `controller_id`（控制器ID）	`error_code_id`	跟踪执行节点
`SmoothPath`	调用Smoother Server优化路径的光滑性与运动学可行性	`unsmoothed_path`, `smoother_id`	`smoothed_path`, `was_completed`	可选但推荐
`Spin`	原地旋转指定角度	`spin_dist`（旋转弧度）	---	常用恢复动作
`BackUp`	后退指定距离	`backup_dist`, `backup_speed`	---	常用恢复动作
`Wait`	等待指定时间	`wait_duration`（秒）	---	时间触发等待
`DriveOnHeading`	沿指定方向直线行驶	`dist_to_travel`, `speed`, `time_allowance`	---	特殊恢复动作
`NavigateToPose`	递归调用子行为树导航到单个目标点	`goal`, `behavior_tree`	`error_code_id`	用于嵌套子BT
`NavigateThroughPoses`	递归调用子行为树导航通过多个路点	`goals`, `behavior_tree`	`error_code_id`	用于嵌套子BT
`ComputeRoute`	调用Route Server在预定义导航图上计算路线	`start_id`, `goal_id`, `use_poses`	`path`, `route`, `planning_time`	基于图导航
`ClearEntireCostmap`	清除全部代价地图	`service_name`	---	实际为服务调用节点

关键参数说明 （以 ComputePathToPose 为例）：

goal （输入端口）：接收目标位姿（geometry_msgs/PoseStamped），通常从行为树黑板（Blackboard）的 "{goal}" 变量获取。
planner_id （输入端口）：指定要使用的规划器插件ID，如 "GridBased" 对应Smac规划器。
path （输出端口）：规划成功后，将生成的 nav_msgs/Path 写入黑板，供后续的 FollowPath 节点使用。
error_code_id（输出端口）：规划失败时输出错误码，可用于驱动恢复行为的选择。

2.2 Condition节点（Condition Nodes）

概念与特征 ：Condition节点对系统状态进行检查并立即返回 SUCCESS 或 FAILURE。它从不返回 RUNNING，执行应该是瞬时、计算量小的。Condition节点不改变系统状态（无副作用），仅用于行为树中的决策分支。

实现原理 ：Condition节点继承自 BT::ConditionNode 基类，在其 tick() 虚函数中实现同步的检查逻辑。Nav2共内置17个Condition节点，常见如下：

节点名称	检查内容	典型用途
`GoalReached`	检查机器人是否已到达目标位姿（位置与角度容差内）	判断导航是否完成
`GoalUpdated`	检查导航目标是否已被新目标替代	抢占当前行为以响应新目标
`IsStuck`	检测机器人是否被卡住（速度低于阈值持续一定时间）	触发恢复行为
`IsBatteryLow`	检查电池电量是否低于阈值	触发充电/对接行为树
`IsPathValid`	检查已规划路径是否碰撞自由	判断是否需要重新规划
`TransformAvailable`	检查所需TF变换是否可用	流程前置检查
`InitialPoseReceived`	检查是否已收到初始位姿估计	导航启动条件
`TimeExpired`	检查指定时间是否已过	超时判断
`DistanceTraveled`	检查已行驶距离是否达到阈值	里程触发条件

典型用法 ：Condition节点常与 ReactiveFallback 控制节点配对使用，实现"条件发生时中断当前行为"的响应式模式。例如，在恢复过程中实时检查 GoalUpdated------一旦外部发送了新目标，立即中断当前恢复动作并返回 SUCCESS，让主导航子树重新执行，从而实现对新目标的高响应性。

2.3 Control节点（Control Nodes）

概念与特征 ：Control节点拥有1至N个子节点，其职责是决定子节点的执行顺序和调度策略。Control节点本身不执行具体任务，而是通过子节点返回的状态驱动逻辑流转。

Nav2在BehaviorTree.CPP标准控制节点（Sequence、Fallback、Parallel）基础上，扩展了5种导航专用控制节点：

(1) PipelineSequence（管道序列）

行为语义 ：类似于标准 Sequence，从左到右顺序执行子节点，任一子节点返回 FAILURE 则立即终止并返回 FAILURE。关键差异在于：当子节点返回 RUNNING 时，PipelineSequence 会持续重新tick该子节点 ，而非跳过。这一特性特别适合"规划→控制"流水线------当 FollowPath 正在执行时，PipelineSequence 会持续tick前序的 ComputePathToPose（若被装饰器触发重规划），实现路径的动态重规划 与无缝切换。

xml 复制代码

<PipelineSequence name="NavigateWithReplanning">
    <RateController hz="1.0">
        <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <!-- 规划失败时的上下文恢复 -->
            <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
        </RecoveryNode>
    </RateController>
    <RecoveryNode number_of_retries="1" name="FollowPath">
        <FollowPath path="{path}" controller_id="FollowPath"/>
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
    </RecoveryNode>
</PipelineSequence>

(2) RecoveryNode（恢复节点）

行为语义 ：RecoveryNode 拥有两个子节点，第一个为主任务节点 ，第二个为恢复动作节点 （可包含子树）。当第一个子节点返回 FAILURE 时，执行第二个子节点（恢复），之后重试第一个子节点。通过 number_of_retries 参数控制最大重试次数。若恢复子节点也返回 FAILURE 或重试次数耗尽，则返回 FAILURE。

xml 复制代码

<RecoveryNode number_of_retries="6" name="NavigateRecovery">
    <!-- 主导航子树 -->
    <PipelineSequence name="NavigateWithReplanning"> ... </PipelineSequence>
    <!-- 系统级恢复子树 -->
    <ReactiveFallback name="RecoveryFallback"> ... </ReactiveFallback>
</RecoveryNode>

参数：number_of_retries --- 最大重试次数（整型）。

(3) RoundRobin（轮转选择）

行为语义 ：依次tick子节点，若当前子节点返回 FAILURE 则切换到下一个子节点；若返回 RUNNING 则维持当前节点并返回 RUNNING。仅当所有子节点均返回 FAILURE 时，RoundRobin 才返回 FAILURE。在Nav2中，RoundRobin常用于系统级恢复序列------轮流尝试不同的恢复策略（如清理代价地图 → 原地旋转 → 等待 → 后退），只要任意一种恢复成功即退出：

xml 复制代码

<RoundRobin name="RecoveryActions">
    <Sequence name="ClearingActions">
        <ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
        <ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
    </Sequence>
    <Spin spin_dist="1.57"/>
    <Wait wait_duration="5"/>
    <BackUp backup_dist="0.15" backup_speed="0.025"/>
</RoundRobin>

(4) ReactiveFallback（响应式回退）

行为语义 ：是Nav2中最具特色的控制节点之一。类似于标准的 Fallback 节点（顺序执行子节点直到某个返回 SUCCESS），但增加了一个关键特性：当第一个子节点返回 RUNNING 时，ReactiveFallback 会持续异步tick后续的条件子节点 。若后续条件节点返回 SUCCESS，则立即中止第一个子节点并返回 SUCCESS。

这一特性专门用于实现"响应式中断"模式。例如，在恢复子树中嵌套 ReactiveFallback + GoalUpdated 条件，使恢复过程能随时被新导航目标中断，无需等待当前恢复动作完成。

(5) 其他Nav2控制节点

NonblockingSequence：非阻塞序列，在当前子节点返回 RUNNING 时仍继续tick后续节点，用于并发控制场景。
PersistentSequence：持久序列，记录各子节点的完成状态以支持跨tick的进度记忆。

2.4 Decorator节点（装饰器节点）

概念与特征 ：Decorator节点只有一个子节点，其职责是修改该子节点的行为或返回值------如控制调用频率、转换返回值、重试逻辑等。Nav2内置7个装饰器节点，其中最核心的有以下三个：

(1) RateController（频率控制器）

控制子节点的最大tick频率（Hz）。在导航行为树中，常用来限制 ComputePathToPose 的重规划频率：

xml 复制代码

<RateController hz="1.0">
    <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
</RateController>

参数：hz --- 最大tick频率（Hz，double类型），上例中限制为每秒最多1次重规划。

(2) DistanceController（距离控制器）

当机器人移动的累计距离超过指定阈值时，才允许重新tick子节点。适用于控制路径重规划的触发距离：

xml 复制代码

<DistanceController distance="1.0">
    <ComputePathToPose goal="{goal}" path="{path}"/>
</DistanceController>

参数：distance --- 触发距离阈值（米，double类型），上例中为每行驶1.0米触发一次重规划。

(3) SpeedController（速度控制器）

与DistanceController类似，但以机器人的实时速度作为子节点tick的触发条件，仅在速度超过（或低于）阈值时激活子节点。

(4) GoalUpdater（目标更新器）

每次tick时检查黑板中的 goal 是否已更新。若有新目标则返回 SUCCESS，常用于在子树的入口处检测目标变更，驱动响应式行为切换。

Parameterizable Action Node ：另有一种特殊的Decorator模式，允许在XML中通过 param 属性动态传入参数，修改子Action节点的运行时配置。

3. 行为树的构建流程与XML语法规则

3.1 行为树的XML结构基础

Nav2行为树使用XML格式描述树结构。一个完整的BT XML文件遵循以下基本骨架：

xml 复制代码

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <!-- 行为树节点层级结构 -->
    </BehaviorTree>
</root>

核心元素说明：

<root> ：根标签，main_tree_to_execute 属性指定要执行的子树ID。
<BehaviorTree ID="..."> ：定义一个（子）行为树，可以包含多个子树定义。
BTCPP_format="4" ：可选属性，指定BehaviorTree.CPP库的版本格式（v4）。

3.2 节点的XML表示与黑板变量

每个BT节点通过XML标签表示，节点类型名（如 ComputePathToPose、FollowPath）对应C++插件注册名，需与工厂注册的名称完全一致：

xml 复制代码

<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>

黑板（Blackboard）变量 ：{变量名} 语法表示对黑板中存储数据的引用。黑板是行为树节点间共享数据的键值存储，运行时由 BT::Blackboard 管理。常用黑板键包括：

黑板键	数据类型	说明
`goal`	`geometry_msgs::msg::PoseStamped`	导航目标位姿
`path`	`nav_msgs::msg::Path`	当前规划的路径
`goals`	`std::vector<PoseStamped>`	多路点导航的目标列表
`error_code_id`	`uint16_t`	错误码标识

关键说明：行为树中的"Node"与ROS 2中的"Node"是完全不同的概念。BT Node是行为树执行逻辑的基本单元（通常作为插件动态加载），而ROS 2 Node是分布式系统中通过话题（Topic）、服务（Service）、动作（Action）通信的执行单元。BT中的Action Node也不一定对应ROS 2的Action Server，但Nav2中大多数Action节点确实通过ROS Action接口与独立的功能服务器通信。

3.3 参数配置方式

行为树节点的参数通过XML属性（Attribute） 传递，主要分为三类：

输入端口（Input Ports） ：定义节点执行所需的输入数据，如 goal="{goal}" 从黑板读取目标位姿。
输出端口（Output Ports） ：定义节点执行后写入黑板的结果数据，如 path="{path}" 将计算的路径写入黑板。
硬编码常量 ：直接写入数字或字符串常量，如 hz="1.0"、planner_id="GridBased"。

每个BT节点的端口定义在C++代码的 providedPorts() 静态方法中声明。例如，Nav2中 FollowPath 的端口定义如下：

cpp 复制代码

static BT::PortsList providedPorts() {
    return {
        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
        BT::InputPort<std::string>("controller_id", ""),
    };
}

3.4 构建一个最小可用的行为树

以下是一个最小但可运行的Nav2导航行为树，它每1米重新规划一次路径并跟踪执行：

xml 复制代码

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <PipelineSequence name="NavigateWithReplanning">
            <DistanceController distance="1.0">
                <ComputePathToPose goal="{goal}" path="{path}"/>
            </DistanceController>
            <FollowPath path="{path}"/>
        </PipelineSequence>
    </BehaviorTree>
</root>

该树虽然功能基本，但不包含任何恢复行为、重试机制、算法选择等高级特性，Nav2官方文档以此强调行为树的可扩展性------所有这些高级功能均可通过添加节点与子树渐进增强。

3.5 行为树的运行时执行流程

BehaviorTreeEngine 的 run() 方法以指定的循环频率（默认10ms）执行行为树。每次循环迭代称为一个"tick周期"，引擎从根节点开始，沿着树结构向下递归tick各节点。其执行逻辑如下：

从XML文件或字符串加载BT定义，通过 BT::BehaviorTreeFactory 注册的节点名称解析为C++插件实例。
创建共享黑板实例（BT::Blackboard），所有节点通过黑板读写共享数据。
在每个tick周期中，从根节点开始递推执行。控制节点根据子节点返回的状态决定下一个tick哪个节点。
若检测到取消请求（如ROS Action被抢占），执行 haltAllActions() 终止所有正在运行的Action节点。
最终返回整体执行状态------SUCCEEDED、FAILED 或 CANCELED。

3.6 行为树的配置与加载

行为树导航器（bt_navigator）的配置参数如下所示，指定使用的XML文件路径及需加载的BT节点插件库：

yaml 复制代码

bt_navigator:
  ros__parameters:
    use_sim_time: False
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_back_up_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node
      - nav2_clear_costmap_service_bt_node
      - nav2_is_stuck_condition_bt_node
      - nav2_goal_reached_condition_bt_node
      - nav2_goal_updated_condition_bt_node
      - nav2_initial_pose_received_condition_bt_node
      - nav2_reinitialize_global_localization_service_bt_node
      - nav2_rate_controller_bt_node
      - nav2_distance_controller_bt_node
      - nav2_speed_controller_bt_node
      - nav2_recovery_node_bt_node
      - nav2_pipeline_sequence_bt_node
      - nav2_round_robin_node_bt_node
      - nav2_transform_available_condition_bt_node
      - nav2_time_expired_condition_bt_node
      - nav2_distance_traveled_condition_bt_node

plugin_lib_names 列出所有需要加载的BT节点插件库。插件以 .so 动态库形式编译，通过 BT_REGISTER_NODES() 宏注册，运行时由 BtNavigator 按需加载。若XML中使用了某节点但未在 plugin_lib_names 中加载其对应库，则运行时会抛出节点未注册异常。

4. 典型导航场景下的行为树示例分析

4.1 Navigate To Pose With Replanning and Recovery（默认主行为树）

这是Nav2最核心的默认行为树 navigate_to_pose_w_replanning_and_recovery.xml，用于实现带有持续重规划和完善故障恢复的单点导航。

核心设计思想：

分层恢复架构：行为树采用两级恢复机制------子任务级的上下文恢复（Contextual Recovery）和系统级的全局恢复（System-Level Recovery）。
持续重规划 ：RateController 以1Hz频率周期性触发 ComputePathToPose 重新规划全局路径，确保路径对动态环境变化做出响应。
6次重试保障 ：最外层 RecoveryNode 设置 number_of_retries="6"，为导航系统提供充足的恢复机会。

完整XML结构：

xml 复制代码

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RecoveryNode number_of_retries="6" name="NavigateRecovery">
            <!-- ===== 主导航子树 ===== -->
            <PipelineSequence name="NavigateWithReplanning">
                <RateController hz="1.0">
                    <RecoveryNode number_of_retries="1" name="ComputePathToPose">
                        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
                        <ReactiveFallback name="ComputePathToPoseRecoveryFallback">
                            <GoalUpdated/>
                            <ClearEntireCostmap name="ClearGlobalCostmap-Context"
                                service_name="global_costmap/clear_entirely_global_costmap"/>
                        </ReactiveFallback>
                    </RecoveryNode>
                </RateController>
                <RecoveryNode number_of_retries="1" name="FollowPath">
                    <FollowPath path="{path}" controller_id="FollowPath"/>
                    <ReactiveFallback name="FollowPathRecoveryFallback">
                        <GoalUpdated/>
                        <ClearEntireCostmap name="ClearLocalCostmap-Context"
                            service_name="local_costmap/clear_entirely_local_costmap"/>
                    </ReactiveFallback>
                </RecoveryNode>
            </PipelineSequence>
            <!-- ===== 系统级恢复子树 ===== -->
            <ReactiveFallback name="RecoveryFallback">
                <GoalUpdated/>
                <RoundRobin name="RecoveryActions">
                    <Sequence name="ClearingActions">
                        <ClearEntireCostmap name="ClearLocalCostmap-Subtree"
                            service_name="local_costmap/clear_entirely_local_costmap"/>
                        <ClearEntireCostmap name="ClearGlobalCostmap-Subtree"
                            service_name="global_costmap/clear_entirely_global_costmap"/>
                    </Sequence>
                    <Spin spin_dist="1.57"/>
                    <Wait wait_duration="5"/>
                    <BackUp backup_dist="0.15" backup_speed="0.025"/>
                </RoundRobin>
            </ReactiveFallback>
        </RecoveryNode>
    </BehaviorTree>
</root>

（1）最外层 RecoveryNode 允许最多6次全局重试；（2）PipelineSequence 先执行1Hz重规划（含上下文恢复），再执行路径跟踪（含上下文恢复）；（3）若主导航子树失败，进入系统级恢复子树------ReactiveFallback 先异步检查 GoalUpdated，再通过 RoundRobin 依次尝试清理代价地图、原地旋转、等待、后退四种策略；（4）任一恢复成功后重新尝试导航。

分层恢复策略的详细解析：

(a) 子任务级上下文恢复 ：ComputePathToPose 被其自己的 RecoveryNode 包裹（number_of_retries="1"）。若规划失败（如因全局代价地图中有动态障碍阻挡），触发上下文恢复------在 ReactiveFallback 内先检查是否有新目标（GoalUpdated），若无则清除全局代价地图（ClearEntireCostmap），之后重新尝试规划。同理，FollowPath 的上下文恢复清除的是局部代价地图------这体现了上下文感知的恢复策略：规划失败（全局问题）清理全局代价地图，控制失败（局部问题）清理局部代价地图。

(b) 系统级全局恢复 ：当上下文恢复无法解决故障（即主导航子树整体返回 FAILURE），行为树进入系统级恢复子树。ReactiveFallback 首先异步检查 GoalUpdated------如果导航目标在恢复过程中被外部更新，立即终止恢复并返回 SUCCESS，使主导航子树以新目标重新执行。否则，RoundRobin 顺序尝试四种恢复策略：先清理两层代价地图，若仍失败则原地旋转1.57弧度（约90°），若仍失败则等待5秒（让动态障碍物通过），最后尝试后退0.15米。

4.2 Navigate To Pose With Consistent Replanning And If Path Becomes Invalid（健壮版导航树）

这是Nav2提供的更成熟版本的行为树 nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml，除上述默认行为树的功能外，还额外支持以下特性：

智能重规划触发：不仅周期性重规划，还可在路径变为无效（碰撞不可行）时立即触发重规划。
条件分支优化 ：使用 Condition 节点（如 IsPathValid）在不同算法间动态切换，为特定场景选用最合适的规划器或控制器。
子树复用：将导航逻辑封装为子树（Subtree），方便在其他行为树中引用复用。
选择器节点应用 ：支持 PlannerSelector、ControllerSelector、GoalCheckerSelector 等节点，允许通过ROS话题动态切换算法。

4.3 基于预定义导航图的导航行为树（Navigate on Route Graph with Recovery）

Nav2还提供了基于导航图（Route Graph） 的行为树，适用于需要在预定义的道路/通道网络上导航的场景（如工厂AGV）。该树的核心区别在于使用 ComputeRoute + ComputePathThroughPoses 替代 ComputePathToPose：

ComputeRoute 调用Route Server在图（Graph）上搜索起点到终点的最优路线。
路线转换为一系列路点（Waypoints）。
ComputePathThroughPoses 依次计算各路点间的自由空间路径。
FollowPath 执行路径跟踪。

这种架构将全局路线规划 与局部路径规划解耦，适合结构化环境中的车规级应用。

5. 自定义行为树节点的开发方法

5.1 开发环境准备

自定义BT节点开发需要满足以下依赖条件：

ROS 2（Binary或源码安装）
Nav2及相关依赖（含 nav2_behavior_tree 包）
BehaviorTree.CPP v3/v4库
C++17编译器

自定义节点可以单独创建ROS 2包，无需修改Nav2源码。所有 nav2_behavior_tree 包提供的节点类型均可被外部项目引用和扩展。

5.2 Nav2提供的基础类体系

Nav2为自定义BT节点开发提供了三层基础类体系：

基础类	用途	适用场景
`BtActionNode<ActionT>`	模板类，封装ROS 2 Action Client的完整生命周期	自定义与Action Server交互的BT节点
`BtServiceNode<ServiceT>`	模板类，封装ROS 2 Service Client通信	自定义与Service Server交互的BT节点
`BtCancelActionNode<ActionT>`	模板类，用于取消正在执行的ROS Action	主动中断导航任务的节点
`BT::ConditionNode`	BehaviorTree.CPP原生基类，同步返回SUCCESS/FAILURE	自定义条件检查节点
`BT::ControlNode`	BehaviorTree.CPP原生基类，管理子节点执行流	自定义控制节点
`BT::DecoratorNode`	BehaviorTree.CPP原生基类，修改单个子节点行为	自定义装饰器节点
`BT::ActionNodeBase`	BehaviorTree.CPP原生基类，不依赖ROS Action接口	自定义同步动作节点

BtActionNode 的核心方法：

方法	调用时机	是否必须覆写	说明
构造函数	节点实例化	是	指定XML标签名、Action Server名称、BT配置
`providedPorts()`	工厂注册时调用	是	定义节点的输入/输出端口
`on_tick()`	每次节点被tick时	否	设置Action Goal、获取黑板值等
`on_wait_for_result()`	等待Action Server响应期间周期性调用	否	用于检查超时、抢占等
`on_success()`	Action成功完成时	否	处理返回数据，写入黑板
`on_aborted()`	Action被中止时	否	处理中止结果
`on_cancelled()`	Action被取消时	否	处理取消结果

5.3 开发实战：创建自定义Action节点

以Nav2内置的 Wait 节点为例，完整展示自定义Action BT节点的开发流程：

步骤1：包含头文件

cpp 复制代码

#include "nav2_behavior_tree/bt_action_node.hpp"
#include "nav2_msgs/action/wait.hpp"

步骤2：定义节点类

cpp 复制代码

namespace nav2_behavior_tree {

class WaitAction : public BtActionNode<nav2_msgs::action::Wait> {
public:
    // 构造函数：设置XML标签名、Action Server名称、BT配置
    WaitAction(
        const std::string & xml_tag_name,
        const std::string & action_name,
        const BT::NodeConfiguration & conf)
    : BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
    {}

    // 必须覆写：定义输入/输出端口
    static BT::PortsList providedPorts() {
        return {
            BT::InputPort<int>("wait_duration", 1, "Wait time in seconds")
        };
    }

    // 可选覆写：每次tick时调用，用于准备Goal
    void on_tick() override {
        // 从输入端口读取参数（若端口未设置则使用默认值1）
        getInput("wait_duration", goal_.time.sec);
        goal_.time.nanosec = 0;
    }

    // 可选覆写：等待结果期间调用，此处检查超时
    void on_wait_for_result() override {
        // 可在此检查是否需要抢占当前任务
        // 例如检查是否有新目标到达
    }

    // 可选覆写：成功完成时调用
    BT::NodeStatus on_success() override {
        RCLCPP_INFO(node_->get_logger(), "Wait completed successfully");
        return BT::NodeStatus::SUCCESS;
    }
};

}  // namespace nav2_behavior_tree

步骤3：注册节点到工厂

cpp 复制代码

// 在插件导出文件中，将自定义类注册给行为树工厂
#include "behaviortree_cpp/bt_factory.h"
#include "nav2_behavior_tree/plugins/action/wait_action.hpp"

BT_REGISTER_NODES(factory) {
    factory.registerNodeType<nav2_behavior_tree::WaitAction>("Wait");
}

步骤4：编译为动态库

在 CMakeLists.txt 中将节点编译为共享库，并确保在 plugin_lib_names 参数中指定该库，使 BtNavigator 运行时能够加载。

步骤5：在XML中使用自定义节点

xml 复制代码

<Wait wait_duration="5"/>

5.4 开发自定义Condition节点

Condition（条件）节点继承自 BT::ConditionNode，执行同步状态检查并立即返回结果。以下是创建电池电量检查节点的示例：

cpp 复制代码

#include "behaviortree_cpp/condition_node.h"

class IsBatteryLowCondition : public BT::ConditionNode {
public:
    IsBatteryLowCondition(const std::string & name, const BT::NodeConfiguration & conf)
    : BT::ConditionNode(name, conf)
    {}

    static BT::PortsList providedPorts() {
        return {
            BT::InputPort<double>("battery_threshold", 0.2, "Battery level threshold (0-1)")
        };
    }

    BT::NodeStatus tick() override {
        double threshold;
        getInput("battery_threshold", threshold);

        // 从黑板或ROS话题读取当前电量
        double battery_level;
        if (!getInput("battery_level", battery_level)) {
            return BT::NodeStatus::FAILURE;
        }
        return (battery_level < threshold) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
    }
};

BT_REGISTER_NODES(factory) {
    factory.registerNodeType<IsBatteryLowCondition>("IsBatteryLow");
}

5.5 高级开发主题

(a) 创建自定义控制/装饰器节点 ：继承自 BT::ControlNode 或 BT::DecoratorNode 基类，可以创建比标准节点更复杂的执行逻辑（如自定义循环策略、并发控制等）。

(b) Groot2可视化开发 ：Groot2（Graphical Editor for Behavior Trees）是BehaviorTree.CPP官方提供的可视化BT编辑器。开发者可在Groot2中拖拽式构建行为树，导出为XML后在Nav2中直接使用。Groot2还支持运行时连接 BtNavigator 的Groot2 Publisher端口，实现行为树执行状态的实时监控与调试。

© 子树（Subtree）封装与复用 ：通过在XML中定义多个 <BehaviorTree ID="..."> 子树，可将常用的功能逻辑模块化封装，并在多个行为树中通过 <SubTree ID="..."/> 标签引用，实现"一次编写，多处复用"。

(d) 动态行为树切换：Nav2支持在每次导航请求中动态指定要使用的行为树文件路径，无需重新启动导航系统即可切换导航行为策略。

总结

行为树是Nav2导航框架的核心竞争力，它将导航决策逻辑从代码中解耦至声明式XML配置，极大提升了机器人导航系统的灵活性、可定制性和可维护性。通过理解四类基本节点（Action、Condition、Control、Decorator）的功能语义，掌握XML构建语法与黑板变量机制，并借助 BtActionNode、BtServiceNode 等模板类快速开发自定义节点，开发者能够根据具体应用需求构建从简单到高度复杂的导航行为------如仓库AGV的图基导航、巡检机器人的多路点巡逻、服务机器人的动态避障与电梯集成等。行为树的模块化设计使得每一层逻辑都可独立调试、可视化监控和渐进增强，是构建生产级自主导航系统的理想范式。

参考资料

Nav2 官方文档 --- Nav2 project overview and behavior tree explanation
Nav2 Behavior Trees --- Example behavior tree XML files and usage guide
Behavior Tree XML Nodes Configuration Guide --- Complete reference of BT XML nodes with examples
Writing a New Behavior Tree Plugin (Tutorial) --- Step-by-step guide for creating custom BT nodes
nav2_behavior_tree Package API Documentation --- ROS 2 Humble API reference
DeepWiki: Behavior Tree Framework (Navigation2) --- Node types classification and execution model
DeepWiki: Behavior Tree Node Catalog --- Complete catalog of all BT nodes in Nav2
DeepWiki: Behavior Tree Navigator --- BT Navigator architecture and execution
DeepWiki: Recovery Behaviors --- Recovery system integration with behavior tree
Detailed Behavior Tree Walkthrough --- In-depth walkthrough of default BT
Nav2 Navigate To Pose With Consistent Replanning --- Advanced behavior tree example
BehaviorTree.CPP v3 Documentation --- Official BehaviorTree.CPP library tutorials and API reference
Macenski, S., et al., "The Marathon 2: A Navigation System," --- Academic paper describing Nav2 architecture
Nav2中文网：详细的行为树演练 --- 中文社区的行为树参考指南