Nav2 专用节点

警告：初学者容易对术语产生混淆：

• 在行为树（BT）语境中，Node 与 ROS 2 语境下的 Node 完全不同。
• BT 语境中的 ActionNode 不一定与 ROS 2 的 Action Server 相连（但通常确实如此）。

Nav2 提供了大量自定义 BT 节点，用于实现 Nav2 特有的功能。下面介绍一些常用节点。 完整的自定义 BT 节点列表可在 nav2_behavior_tree 插件文件夹 中查看， 配置指南 也很有参考价值。

动作节点（Action Nodes）

• ComputePathToPose —— ComputePathToPose Action Server 客户端（规划器接口）

• FollowPath —— FollowPath Action Server 客户端（控制器接口）

• Spin、Wait、Backup —— Behaviors Action Server 客户端

• ClearCostmapService —— ClearCostmapService 服务客户端

这些动作节点执行完毕后，如果动作服务器判定动作已完成则返回 SUCCESS，仍在运行则返回 RUNNING，否则返回 FAILURE。注意，上表中的 ClearCostmapService 并非动作服务器客户端，而是服务客户端。

条件节点（Condition Nodes）

• GoalUpdated —— 检查 goal 话题上的目标是否已更新

• GoalReached —— 检查是否已到达目标

• InitialPoseReceived —— 检查是否在 initial_pose 话题上收到了初始位姿

• isBatteryLow —— 监听电池话题，检查电池是否低电量

上述条件节点用于检测系统的特定状态。通常，条件为真时返回 SUCCESS，否则返回 FAILURE。 默认 Nav2 行为树中使用的关键条件节点是 GoalUpdated，它会在特定子树中异步检测。该节点实现的行为是：「如果目标已更新，则必须重新规划。」 条件节点通常与 ReactiveFallback 节点配合使用。

装饰节点（Decorator Nodes）

• DistanceController（距离控制器）—— 机器人每行进一定距离时 tick 一次子节点

• RateController（频率控制器）—— 以恒定频率 tick 子节点，频率通过端口（port）设置

• GoalUpdater（目标更新器）—— 通过 BT 端口更新子节点的目标

• SingleTrigger（单次触发）—— 只 tick 子节点一次，之后所有后续 tick 均返回 FAILURE

• SpeedController（速度控制器）—— 以与机器人速度成比例的频率 tick 子节点

控制节点：PipelineSequence

PipelineSequence 控制节点在子节点返回 RUNNING 时会重新 tick 之前的子节点。 该节点与 Sequence 类似，区别在于「当前」节点之前的子节点也会被重新 tick（类似管道中水的流动）。 如果任何子节点返回 FAILURE，所有子节点都会被暂停（halt），父节点也返回 FAILURE。当序列中最后一个节点返回 SUCCESS 时，该节点暂停所有子节点并返回 SUCCESS。

下面通过一个使用 PipelineSequence 的示例行为树来说明。

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <PipelineSequence>
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </PipelineSequence>
    </BehaviorTree>
</root>

1. Action_A、Action_B 和 Action_C 都处于 IDLE 状态。

2. 父节点 PipelineSequence 第一次被 tick 时，假设 Action_A 返回 RUNNING。父节点返回 RUNNING，其他节点不会被 tick。

1. 假设 Action_A 返回 SUCCESS，Action_B 将被 tick 并返回 RUNNING。Action_C 尚未被 tick，仍处于 IDLE。

1. Action_A 再次被 tick 并返回 RUNNING，Action_B 被重新 tick 并返回 SUCCESS，因此行为树继续 tick Action_C。假设 Action_C 返回 RUNNING。Action_A 被重新 tick，正是 PipelineSequence 的核心作用。

1. 序列中所有动作都会被重新 tick。假设 Action_A 仍返回 RUNNING，Action_B 再次返回 SUCCESS，Action_C 本次 tick 也返回 SUCCESS。此时序列已完成，Action_A 被暂停（halt），即使它仍处于 RUNNING 状态。

回顾一下：如果 Action_A、Action_B 或 Action_C 在任何时候返回 FAILURE，父节点都会返回 FAILURE 并暂停所有子节点。

关于 PipelineSequence 的更多详情，请参阅 PipelineSequence 配置指南。

控制节点：Recovery

Recovery 控制节点只有两个子节点，当且仅当第一个子节点返回 SUCCESS 时才返回 SUCCESS。 如果第一个子节点返回 FAILURE，则 tick 第二个子节点。该过程持续循环，直到出现以下情况之一：

• 第一个子节点返回 SUCCESS（父节点返回 SUCCESS）

• 第二个子节点返回 FAILURE（父节点返回 FAILURE）

• 重试次数超过 number_of_retries 参数限制

顾名思义，该节点通常用于将一个动作与一个恢复（recovery）动作搭配使用。第一个子节点通常是「主要」行为，第二个子节点是在主要行为返回 FAILURE 时执行的恢复动作。通常，执行第二个子节点会提高第一个动作成功的几率。

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RecoveryNode number_of_retries="1">
            <ComputePathToPose/>
            <ClearLocalCostmap/>
        </RecoveryNode>
    </BehaviorTree>
</root>

在上面的例子中，假设 ComputePathToPose 失败。作为响应，ClearLocalCostmap 会被 tick 并返回 SUCCESS。 局部代价地图已被清除，假设机器人此时能够正确计算路径，ComputePathToPose 返回 SUCCESS。随后父节点 RecoveryNode 也返回 SUCCESS，行为树执行完成。

关于 RecoveryNode 的更多详情，请参阅 RecoveryNode 配置指南。

控制节点：RoundRobin

RoundRobin 控制节点以轮询（round robin）方式依次 tick 子节点，直到某个子节点返回 SUCCESS，此时父节点也返回 SUCCESS。 如果所有子节点都返回 FAILURE，父节点 RoundRobin 也返回 FAILURE。

下面通过一个示例行为树来说明这个概念。

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <RoundRobin>
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </RoundRobin>
    </BehaviorTree>
</root>

1. 所有节点都从 IDLE 状态开始。

1. 父节点被 tick 时，第一个子节点（Action_A）被 tick。假设该子节点返回 RUNNING。 此时其他子节点不会被 tick，父节点也返回 RUNNING。

1. 下一次 tick 时，假设 Action_A 返回 FAILURE。 于是 Action_B 将被 tick，而 Action_C 保持未被 tick。 假设 Action_B 返回 RUNNING，父节点 RoundRobin 也返回 RUNNING。

1. 下一次 tick 时，假设 Action_B 返回 SUCCESS。父节点 RoundRobin 暂停所有子节点并返回 SUCCESS。 父节点会记住当前状态，在下一次 tick 时从 Action_C 开始，而不是像步骤 2 那样从 Action_A 开始。

1. 本次 tick 时，假设 Action_C 返回 RUNNING，父节点 RoundRobin 也返回 RUNNING，其他节点不会被 tick。

1. 下一次 tick 时，假设 Action_C 返回 FAILURE。父节点将循环回去再次 tick Action_A。Action_A 返回 RUNNING，父节点 RoundRobin 也返回 RUNNING。只要不是所有子节点都返回 FAILURE，这个循环就会一直持续。

关于 RoundRobin 的更多详情，请参阅 RoundRobin 配置指南。

控制节点：NonblockingSequence

NonblockingSequence 控制节点在子节点返回 SUCCESS 或 RUNNING 时，会 tick 所有子节点。该节点与 PipelineSequence 类似，区别在于：只要子节点返回 SUCCESS 或 RUNNING，所有子节点都会被重新 tick，而不会停在最近的 RUNNING 节点上。如果任何子节点返回 FAILURE，所有子节点都会被暂停，父节点也返回 FAILURE。当序列中所有节点都返回 SUCCESS 时，该节点暂停所有子节点并返回 SUCCESS。

注意，即使某个节点在前一次 tick 中返回了 SUCCESS，下一次 tick 时 NonblockingSequence 仍会重新 tick 该节点，可能会将其重新启动。这样设计的目的是确保已成功的节点在等待其他长时间运行的节点完成时，不会锁存（latch）过时的状态。

下面通过一个使用 NonblockingSequence 的示例行为树来说明。

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <NonblockingSequence>
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </NonblockingSequence>
    </BehaviorTree>
</root>

1. Action_A、Action_B 和 Action_C 都处于 IDLE 状态。

2. 父节点 NonblockingSequence 第一次被 tick 时，假设 Action_A 返回 RUNNING。接着 Action_B 被 tick，也返回 RUNNING。最后 Action_C 被 tick，也返回 RUNNING。三个子节点都返回 RUNNING，NonblockingSequence 返回 RUNNING。

1. 下一次 tick 父节点 NonblockingSequence 时，序列中所有动作都会被重新 tick。假设 Action_A 返回 SUCCESS，Action_B 和 Action_C 仍返回 RUNNING。此时 NonblockingSequence 仍返回 RUNNING，因为有两个子节点处于 RUNNING 状态。

1. 下一次重新 tick 时，假设 Action_A 和 Action_C 返回 SUCCESS，Action_B 返回 RUNNING。此时 NonblockingSequence 仍返回 RUNNING，因为仍有一个子节点处于 RUNNING 状态。注意 Action_A 被重新 tick 并再次返回 SUCCESS，它并没有因为之前返回过 SUCCESS 而被跳过。

1. 最后，假设 Action_A、Action_B 和 Action_C 都返回 SUCCESS。此时序列已完成，三个子节点都被暂停，NonblockingSequence 返回 SUCCESS。

回顾一下：如果 Action_A、Action_B 或 Action_C 在任何时候返回 FAILURE，父节点都会返回 FAILURE 并暂停所有子节点。

关于 NonblockingSequence 的更多详情，请参阅 NonblockingSequence 配置指南。

控制节点：PersistentSequence

PersistentSequence 与 Sequence 类似，区别在于它会将最后一个正在运行的子节点索引存储在黑板（blackboard）中（键名为 current_child_idx），并且在暂停（halt）时不会重置该索引。

更多信息请参阅 BT.CPP 中的 Sequence BT 节点。

<root main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <Script code="current_child_idx := 0" />
        <PersistentSequence current_child_idx="{current_child_idx}">
            <Action_A/>
            <Action_B/>
            <Action_C/>
        </PersistentSequence>
    </BehaviorTree>
</root>

控制节点：PauseResumeController

PauseResumeController 是一个控制节点，通过服务调用为行为树提供暂停和恢复功能。

它有一个必需子节点（对应 RESUMED 状态）和三个可选子节点（分别对应 PAUSED 状态、ON_PAUSE 事件和 ON_RESUME 事件）。 该节点有两个输入端口：

• pause_service_name：暂停服务的名称
• resume_service_name：恢复服务的名称

1. 控制器从 RESUMED 状态开始，持续 tick 该子节点直到返回成功。
2. 当暂停服务被调用时，ON_PAUSE 子节点会被 tick 直到完成，然后控制器切换到 PAUSED 状态。
3. 在 PAUSED 状态下，PAUSED 子节点被 tick，直到状态改变或返回失败。
4. 当恢复服务被调用时，ON_RESUME 子节点会被 tick 直到完成，然后控制器切换回 RESUMED 状态。

控制器只有在 RESUMED 子节点返回成功时才返回成功。如果任何子节点返回失败，控制器返回失败。其他情况下返回 RUNNING。

<PauseResumeController pause_service_name="/pause" resume_service_name="/resume">
    <!-- RESUMED branch -->

    <!-- PAUSED branch (optional) -->

    <!-- ON_PAUSE branch (optional) -->

    <!-- ON_RESUME branch (optional) -->
</PauseResumeController>

当 ON_PAUSE 和 ON_RESUME 分支失败时，控制器返回失败并被暂停（halt），状态重置为 RESUMED。在实际失败前可能需要重试几次状态转换，控制器节点本身不提供重试功能，但可以在行为树中添加重试节点来轻松实现：

<PauseResumeController pause_service_name="/pause" resume_service_name="/resume">
    <!-- RESUMED branch -->

    <!-- PAUSED branch -->

    <RetryUntilSuccessful num_attempts="3">
        <!-- ON_PAUSE branch -->
    </RetryUntilSuccessful>

    <RetryUntilSuccessful num_attempts="3">
        <!-- ON_RESUME branch -->
    </RetryUntilSuccessful>
</PauseResumeController>