Nav2行为树

 

目录

 

Nav2 Behavior Trees

Introduction To Nav2 Specific Nodes

Action Nodes动作节点

Condition Nodes条件节点

Decorator Nodes装饰器节点

Control: PipelineSequence

Control: Recovery恢复

Control: RoundRobin轮询    

Detailed Behavior Tree Walkthrough

Overview

Prerequisites

Navigate To Pose With Replanning and Recovery导航至姿态并进行重新规划与恢复

Navigation Subtree

Recovery Subtree

Navigate To Pose

Navigate Through Poses（这个其实和上面的差不多）

Navigate To Pose and Pause Near Goal-Obstacle

Navigate To Pose With Consistent Replanning And If Path Becomes Invalid

Follow Dynamic Point

Odometry Calibration

---

Nav2 Behavior Trees

        Nav2是一个极具可重构性的项目。它允许用户在行为树、核心算法、状态检查器等多个不同的插件类型之间进行设置！本节重点介绍了项目中默认提供的一些示例行为树xml文件，用于执行有趣的任务。值得注意的是，这些可以根据你的具体应用进行修改，或作为构建自己应用专用行为树的指南。这些示例展示了通过使用行为树，你可以显著地重新配置导航行为。Nav2在包中提供了其他行为树，但本节重点介绍了一些重要的，比如 nav2_bt_navigator 。

        下面是一个非常基本但功能性的导航器示例。

<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>

• <root main_tree_to_execute="MainTree">: 行为树的根节点，指定要执行的主行为树名称为 "MainTree"。
• <BehaviorTree ID="MainTree">: 主行为树的定义，其中包含一系列行为节点。
• <PipelineSequence name="NavigateWithReplanning">: 行为树中的管道序列节点，名为 "NavigateWithReplanning"，代表一系列顺序执行的行为节点。
• <DistanceController distance="1.0">: 距离控制器节点，设置规划新路径的距离为 1.0 米。
  • <ComputePathToPose goal="{goal}" path="{path}"/>: 在距离控制器内部，通过此节点计算从当前位置到目标位置的路径，并将路径存储在变量 {path} 中。
• <FollowPath path="{path}"/>: 跟随路径节点，通过此节点实现跟随存储在变量 {path} 中的路径。

        这个行为树简单地每1米（由参数设置）规划一条新路径，使用`goalDistanceController`。如果在黑板变量上计算出了新路径，将会使用默认算法`pathFollowPath`沿着这个新路径进行跟随。

这棵树包含：

- 没有恢复方法
- 失败时没有重试机制
- 没有选择规划器或控制器算法
- 没有节点以在上下文中更改设置以获得最佳性能
- 没有与自动门、电梯或其他API的集成
- 没有用户提供的自定义BT节点
- 没有用于其他行为（如对接、跟随等）的子树
- 没有使用其他类型的规划器，如完整覆盖（在适当时使用）

所有这些，以及更多内容，都可以在Nav2中为您定制的导航逻辑中设置和配置。

• Introduction To Nav2 Specific Nodes

在初学时，词汇可能是一个很大的困惑点。 在讨论行为树（BTs）时，Node（节点）与ROS 2上下文中的Node完全不同。 在BTs上下文中，ActionNode并不一定与ROS 2上下文中的Action Server连接（但通常会）。

        提供了许多定制的Nav2 BT节点，可以按照Nav2特定的方式使用。下面将描述一些常用的Nav2节点。所有定制BT节点的完整列表可以在nav2_behavior_tree插件文件夹 nav2_behavior_tree plugins folder 中找到。配置指南 configuration guide 也可能非常有用。

Action Nodes动作节点

ComputePathToPose - 计算路径到姿态的动作服务器客户端（规划器接口）

FollowPath - 跟随路径的动作服务器客户端（控制器接口）

Spin, Wait, Backup - 行为服务器客户端

ClearCostmapService - 清除成本地图服务的服务器客户端

        完成后，这些动作节点将根据动作服务器认为动作是否已正确完成返回SUCCESS，如果仍在运行则返回RUNNING，否则返回FAILURE。请注意，在上述列表中，ClearCostmapService动作节点不是动作服务器客户端，而是服务客户端（ClearCostmapService动作节点不是动作服务器客户端，而是服务客户端。它用于向服务请求清除成本地图，而不是像动作服务器一样执行动作）。

Condition Nodes条件节点

GoalUpdated - 检查目标话题上的目标是否已更新

GoalReached - 检查目标是否已达成

InitialPoseReceived - 检查是否接收到了initial_pose话题上的姿态信息

isBatteryLow - 通过监听battery话题检查电池是否低电量

        上述条件节点列表可以用于探测系统的特定方面。通常，如果条件为真，则它们会返回SUCCESS，否则返回FAILURE。在默认的Nav2 BT中使用的关键条件是GoalUpdated，它在特定子树中异步进行检查。该条件节点允许实现描述为“如果目标已更新，则我们必须重新规划”的行为。条件节点通常与ReactiveFallback节点配对使用（这段话描述了在默认的Nav2行为树（BT）中使用的一个重要条件节点，即GoalUpdated。这个节点在特定的子树中以异步方式进行检查。它允许实现一种行为，即“如果目标已更新，则需要重新规划”。通常，条件节点会与ReactiveFallback节点一起使用，以实现根据条件调整行为的目的。ReactiveFallback节点允许在检测到特定条件时改变或调整行为树的执行路径）。

Decorator Nodes装饰器节点

Distance Controller距离控制器 - 每当机器人行进一定距离时，将会执行子节点

Rate Controller频率控制器 - 控制其子节点以恒定频率进行执行。执行频率通过一个公开的端口控制

Goal Updater目标更新器 - 通过BT上的端口更新子节点的目标

Single Trigger单触发器 - 只执行其子节点一次，并对所有后续执行返回FAILURE

Speed Controller速度控制器 - 根据机器人速度的比例控制其子节点的执行频率

Control: PipelineSequence

        PipelineSequence 控制节点在子节点返回 RUNNING 时重新触发之前的子节点。这个节点类似于 Sequence 节点，但具有额外的特性，即“当前”节点之前的子节点会被重新触发（类似水流在管道中的流动）。如果任何子节点返回 FAILURE，则所有子节点将停止执行，并且父节点也将返回 FAILURE。在序列中最后一个节点返回 SUCCESS 后，此节点将停止执行并返回 SUCCESS。

        为了进一步解释这一点，以下是一个使用 PipelineSequence 的示例行为树（BT）。

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

Action_A、Action_B 和 Action_C 全部处于空闲状态。

当父 PipelineSequence 首次执行时，假设 Action_A 返回 RUNNING。父节点现在将返回 RUNNING，而其他节点不会被执行。

        现在，假设 Action_A 返回 SUCCESS，那么 Action_B 将会被执行并返回 RUNNING。而 Action_C 尚未被执行，因此将返回 IDLE。

        Action_A 再次被执行并返回 RUNNING，Action_B 被重新执行并返回 SUCCESS，因此 BT 继续执行 Action_C，这是 Action_C 第一次被执行。假设 Action_C 返回 RUNNING。Action_A 的重新触发是 PipelineSequence 有用的地方。

        序列中的所有动作都将被重新执行。假设 Action_A 仍然返回 RUNNING，而 Action_B 再次返回 SUCCESS，而 Action_C 在此次执行中返回 SUCCESS。现在序列已经完成，因此即使 Action_A 仍处于 RUNNING 状态，它也会被停止。

        请注意，如果在任何时候 Action_A、Action_B 或 Action_C 返回了 FAILURE，父节点将返回 FAILURE，并停止任何子节点的执行。

有关 PipelineSequence 的更多详细信息，请参阅 PipelineSequence 配置指南。

• 有三个动作：Action_A、Action_B 和 Action_C，它们的初始状态都是 IDLE（空闲）。
• 当父 PipelineSequence 节点首次被触发时，假设 Action_A 返回 RUNNING（运行中）。父节点也会返回 RUNNING，并且不会执行其他节点。
• 如果 Action_A 返回 SUCCESS，接下来 Action_B 会被触发并返回 RUNNING。由于 Action_C 还没有被触发过，它将返回 IDLE。
• 当 Action_A 再次被触发并返回 RUNNING，而 Action_B 被重新触发并返回 SUCCESS 时，BT（行为树）会执行 Action_C。假设 Action_C 返回 RUNNING。重新触发 Action_A 是 PipelineSequence 节点的特点之一，它使得在特定条件下可以重新执行先前的节点。
• 所有序列中的动作都将被重新触发。如果假设 Action_A 仍然返回 RUNNING，而 Action_B 再次返回 SUCCESS，然后 Action_C 也返回 SUCCESS。这样，序列就完成了，因此即使 Action_A 仍处于 RUNNING 状态，它也会被停止。如果任何时候 Action_A、Action_B 或 Action_C 返回 FAILURE，父节点也会返回 FAILURE 并停止所有子节点的执行。

Control: Recovery恢复

        Recovery（恢复）控制节点只有两个子节点，并且仅当第一个子节点返回 SUCCESS 时，它才会返回 SUCCESS。如果第一个子节点返回 FAILURE，则将触发第二个子节点。这个循环会持续直到以下情况之一发生：

- 第一个子节点返回 SUCCESS（导致父节点返回 SUCCESS）
- 第二个子节点返回 FAILURE（导致父节点返回 FAILURE）
- 违反了 number_of_retries 输入参数（number_of_retries 指的是 Recovery 控制节点在执行过程中尝试恢复的最大次数。当第一个子节点失败后，Recovery 控制节点会触发第二个子节点，并且在一定条件下重试执行失败的子节点，直到达到设定的最大重试次数或者某个节点成功返回）

        通常，这个节点用于连接一个操作和一个恢复性操作，正如其名称所示。第一个操作通常是“主”行为，第二个操作将是在主行为失败时执行的操作。通常，触发第二个子节点操作会增加第一个操作成功的机会。

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

        在上面的例子中，假设 ComputePathToPose 失败了。作为响应，ClearLocalCostmap 将被触发，并返回 SUCCESS。现在，我们已经清除了成本地图，假设机器人能够正确计算路径，ComputePathToPose 现在返回 SUCCESS。然后，父 RecoveryNode 也将返回 SUCCESS，BT 将完成执行。

        有关 RecoveryNode 的更多详细信息，请参阅 RecoveryNode 配置指南RecoveryNode configuration guide。

Control: RoundRobin轮询    

        RoundRobin控制节点按照轮询方式依次触发其子节点，直到一个子节点返回 SUCCESS 为止，此时父节点也将返回 SUCCESS。如果所有子节点都返回 FAILURE，父节点 RoundRobin 也将返回 FAILURE。

        以下是一个示例行为树（BT），我们将通过它来理解这个概念。

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

1. 所有节点的初始状态都是 IDLE

2. 当父节点被触发时，会触发第一个子节点（Action_A）。假设在触发后子节点返回 RUNNING。在这种情况下，不会触发其他子节点，并且父节点也将返回 RUNNING

3. 在下一次触发时，假设Action_A返回 FAILURE。这意味着接下来将触发Action_B，而Action_C保持未触发状态。假设这次Action_B返回 RUNNING。这意味着父节点RoundRobin也将返回 RUNNING

4. 在下一次触发中，假设Action_B返回 SUCCESS。父节点RoundRobin现在将停止所有子节点并返回 SUCCESS。父节点保留此状态信息，并在下一次触发时触发 Action_C，而不是像步骤2那样从 Action_A 开始。

5. 在这次触发中，假设Action_C返回 RUNNING，父节点 RoundRobin 也返回 RUNNING。没有其他节点被触发。

6. 在最后一次触发中，假设Action_C返回 FAILURE。父节点将重新循环并触发 Action_A。Action_A 返回 RUNNING，因此父节点 RoundRobin 也将返回 RUNNING。除非所有子节点返回 FAILURE，否则此模式将无限循环。

有关 RoundNode 的更多详细信息，请参阅RoundRobin configuration guide

RoundRobin节点，它是一种行为树控制节点。RoundRobin节点会按照轮询的方式逐个触发其子节点，直到其中一个子节点返回 SUCCESS。如果所有子节点都返回 FAILURE，那么RoundRobin节点也将返回 FAILURE。

具体来说，RoundRobin节点会依次触发每个子节点。如果第一个子节点返回 RUNNING，则不会触发其他子节点，而父节点也将返回 RUNNING。如果第一个子节点返回 FAILURE，则会触发下一个子节点。当某个子节点返回 SUCCESS 时，父节点也将返回 SUCCESS。若所有子节点都返回 FAILURE，则父节点也会返回 FAILURE。这种机制使得RoundRobin节点可以按照顺序轮流执行子节点，并在其中一个子节点成功时停止执行。

• Detailed Behavior Tree Walkthrough

Overview

        这份文档是Nav2中使用的主要行为树（BT）的参考指南。

        在nav2_bt_navigator/behavior_trees中提供了许多示例行为树，但有时需要根据机器人的应用重新配置它们。接下来的文档将详细介绍当前主要默认的 BT navigate_to_pose_w_replanning_and_recovery.xml。

Prerequisites

        在继续本教程之前，先熟悉一下行为树的概念：

        阅读关于导航概念的简要解释 navigation concepts。

        查看关于BehaviorTree CPP V3 BehaviorTree CPP V3 网站的通用教程和指南（不是Nav2特定的）。具体来说，BehaviorTree CPP V3网站中“学习基础知识”部分解释了将在本指南中构建的基本通用节点。

        熟悉自定义的Nav2特定行为树节点 Nav2 specific BT nodes。

Navigate To Pose With Replanning and Recovery导航至姿态并进行重新规划与恢复

以下部分将详细描述目前在 Nav2 中使用的主要默认 BT，即 navigate_to_pose_w_replanning_and_recovery.xml 的概念。此行为树会以 1 Hz 的频率周期性地重新规划全局路径，并具有恢复动作（全局路径指的是机器人从起始点到目标点的完整路径，考虑了环境中的障碍物和限制条件。这条路径经过全局规划，通常是基于地图和目标位置计算得出的路径，其目标是覆盖整个导航区域而不考虑局部障碍物的影响）。

BTs 主要用 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>

        这可能仍然有些复杂，但这棵树可以分成两个较小的子树，我们可以一次关注一个子树。这两个较小的子树是最顶层的 RecoveryNode 的子节点。从现在开始，NavigateWithReplanning子树将被称为导航子树，而RecoveryFallback子树将被称为恢复子树。可以用以下方式表示：

导航子树主要涉及实际的导航行为：

计算路径

跟随路径

针对上述主要导航行为的上下文恢复行为

恢复子树包括处理系统级故障或内部难以处理的问题的行为。

整个行为树（希望如此）大部分时间都将花在导航子树中。如果导航子树中的两个主要行为（路径计算或路径跟随）中的任何一个失败，将尝试上下文恢复。

如果上下文恢复仍然不够，导航子树将返回FAILURE。系统将转向恢复子树，尝试清除任何系统级导航故障。

这将持续进行，直到父级 RecoveryNode 的 number_of_retries 超过（默认为 6）。

<RecoveryNode number_of_retries="6" name="NavigateRecovery">

Navigation Subtree

        现在我们已经了解了导航子树和恢复子树之间的控制流程，让我们专注于导航子树。

这个子树的XML如下：

<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>

        这个子树包含两个主要的动作，即“ComputePathToPose”和“FollowPath”。如果这两个动作中的任何一个失败，它们将会尝试在上下文中清除失败。这棵树的核心可以用一个父节点和两个子节点来表示，如下所示：

        父级的PipelineSequence节点允许先执行ComputePathToPose，一旦成功，就会执行FollowPath。在执行FollowPath子树时，ComputePathToPose子树也会被执行。这样做允许在机器人移动时重新计算路径。

        无论是ComputePathToPose还是FollowPath，都遵循相同的一般结构：

•         执行动作
•         如果动作失败，尝试在上下文中进行恢复

        下面是ComputePathToPose子树：

        父级的RecoveryNode控制着动作与上下文恢复子树之间的流程。对于ComputePathToPose和FollowPath的上下文恢复，涉及检查目标是否已更新，并清除相关的成本地图。

        如果你的应用程序能够容忍更多的上下文恢复尝试，考虑更改父级RecoveryNode控制节点中的number_of_retries参数，再转向系统级别的恢复。

        ComputePathToPose和FollowPath BT子树的唯一差异如下：

•         子树中的动作节点：

        - ComputePathToPose子树围绕ComputePathToPose动作展开。
        - FollowPath子树围绕FollowPath动作展开。

•         RateController修饰ComputePathToPose子树：

        - RateController修饰ComputePathToPose子树，以保持在指定频率下进行规划。此BT的默认频率为1 Hz。这样做是为了防止BT以树更新速率（100Hz）向规划服务器发送过多无用的请求。考虑根据应用程序的需要和计算路径的计算成本，将此频率更改为更高或更低的值。还有其他可以替代RateController的修饰器。根据需要，考虑使用SpeedController或DistanceController修饰器。

•         上下文恢复中要清除的成本地图：

        - ComputePathToPose子树清除全局成本地图。在规划器（规划全局路径，要在全局成本地图里面）的上下文中，全局成本地图是相关的成本地图。
        - FollowPath子树清除局部成本地图。在控制器（控制机器人行动，在局部地图里面控制机器人行动）的上下文中，局部成本地图是相关的成本地图。

Recovery Subtree

        Recovery子树是Nav2默认的navigate_to_pose_w_replanning_and_recovery.xml树的第二个重要部分。简而言之，当Navigation子树返回FAILURE时，此子树被触发，控制系统级别的恢复（当Navigation子树中的上下文恢复不足时，当Navigation子树中的恢复措施无法解决问题时，Recovery子树将被触发以尝试在系统级别进行恢复）。

XML 片段：

<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>

        最顶层的父节点 ReactiveFallback 控制着系统范围恢复的流程，并异步检查是否接收到新目标。如果目标在任何时候更新，此子树将停止所有子节点并返回成功。这允许对新目标进行快速反应并抢占当前正在执行的恢复。这应该与 Navigation 子树中的上下文恢复部分相似。这是一个常见的行为树模式，用于处理“除非发生‘这种情况’，否则执行动作 A”的情况。

        这些条件节点非常强大，通常与 ReactiveFallback 配对使用。很容易想象将整个 navigate_to_pose_w_replanning_and_recovery 树包装在具有 isBatteryLow 条件的 ReactiveFallback 中 - 这意味着 navigate_to_pose_w_replanning_and_recovery 树将执行，除非电池电量变低（然后进入另一个用于对接充电的子树）。

如果目标从未更新，行为树将继续到 RoundRobin 节点。行为树中默认的四个系统级恢复是：

1. 清除两个成本地图（本地和全局）的序列
2. Spin 动作
3. Wait 动作
4. BackUp 动作

在父 RoundRobin 的四个子节点中任何一个返回成功后，机器人将尝试在 Navigation 子树中重新导航。如果这次重新导航不成功，则将会执行下一个 RoundRobin 的子节点。

例如，假设机器人被卡住，Navigation 子树返回失败：（为了本例假设，假设目标从未更新）

1. 尝试 Recovery 子树中的清除两个成本地图序列，并返回成功。机器人现在回到 Navigation 子树中。
2. 假设清除两个成本地图仍然不够，Navigation 子树再次返回失败。机器人现在在 Recovery 子树中。
3. 在 Recovery 子树中，Spin 动作将会被执行。如果返回成功，机器人将返回主 Navigation 子树，但假设 Spin 动作返回失败。在这种情况下，树将继续停留在 Recovery 子树中。
4. 假设下一个动作 Wait 返回成功。机器人将会移动到 Navigation 子树。
5. 假设 Navigation 子树再次返回失败（清除成本地图、尝试旋转和等待仍然无法恢复系统）。机器人将再次进入 Recovery 子树，并尝试 BackUp 动作。假设机器人尝试 BackUp 动作，并成功完成该动作。BackUp 动作节点返回成功，因此现在机器人再次进入 Navigation 子树。

在这种假设的情况下，假设 BackUp 动作使机器人成功解除困境，并到达目标。在这种情况下，整体行为树仍将返回成功。

如果 BackUp 动作不足以使机器人摆脱困境，以上逻辑将无限循环，直到 Navigation 子树和 Recovery 子树中的 number_of_retries 超出，或者 Recovery 子树中的所有系统级恢复都返回失败（这不太可能，通常指向其他一些系统故障）。

• Navigate To Pose

        这个行为树实现了 Nav2 行为树 Nav2 Behavior Trees 中更成熟的版本。它在自由空间中从起始点导航到单个目标点。它包含对特定子上下文中自定义恢复行为的使用，以及用于系统级失败的全局恢复子树。它还提供用户在返回失败状态之前多次重试任务的机会。

（

        特定子上下文指的是在导航子树（Navigation Subtree）中的两个子树（ComputePathToPose 和 FollowPath）中的恢复行为。这些子树针对特定的上下文或操作执行恢复行为，例如在计算路径或跟随路径时的失败情况下。而全局恢复子树（Recovery subtree）是针对系统级别的失败情况设计的，用于处理更普遍的问题，例如机器人卡住或处于不良位置的情况。

）

        ComputePathToPose 和 FollowPath BT 节点都指定了它们要使用的算法。按照惯例，我们将其命名为它们所代表的算法风格（例如，不是 DWB，而是 FollowPath），这样行为树或应用程序开发人员无需担心技术细节。他们只想使用一个路径跟随控制器。

        在这个行为树中，我们尝试在返回任务失败之前重试整个导航任务 6 次。这允许导航系统有充分的机会尝试从失败条件中恢复，或者等待瞬时问题解决，比如人员拥挤或临时传感器故障。

        在正常执行中，这将每秒（1HZ）重新规划路径并将其传递给控制器，类似于 Nav2 行为树中 Nav2 Behavior Trees 的行为树。但是，这次如果规划器失败，它将触发其子树中的上下文感知恢复行为，清除全局代价地图。 可以在此处添加额外的恢复行为以进行其他特定上下文的恢复，比如尝试另一个算法。

        同样，控制器也具有类似的逻辑。如果它失败，它也会尝试清除影响控制器的局部代价地图。值得注意的是反应式回退中的 GoalUpdated 节点。这允许我们在通过取消操作将新目标传递给导航系统时退出恢复条件。这确保了当发出新目标时，导航系统会立即做出响应，即使上一个目标处于尝试恢复状态。

        如果这些上下文恢复失败，该行为树将进入恢复子树。该子树用于解决系统级别的故障，以帮助解决机器人卡住或处于糟糕位置等问题。该子树还具有每次迭代都会触发的 GoalUpdated BT 节点，以确保对新目标的响应性。接下来，恢复子树将执行恢复操作：代价地图清除操作、旋转、等待和后退。在子树中的每次恢复操作之后，将重新尝试主导航子树。如果它继续失败，将会尝试恢复子树中的下一个恢复操作。

        虽然这个行为树没有使用，但 PlannerSelector、ControllerSelector 和 GoalCheckerSelector 行为树节点也可能很有帮助。与硬编码要使用的算法不同（GridBased 和 FollowPath），这些行为树节点允许用户通过 ROS 主题动态更改导航系统中使用的算法。但是，可能更明智的做法是使用带有指定算法的条件节点在最有用和独特的情况下创建不同的子树上下文。然而，选择器节点可以是从外部应用程序而不是通过内部行为树控制流逻辑来更改算法的有用方式。最好通过行为树方法实现更改，但我们知道许多专业用户有外部应用程序来动态更改其导航器的设置（这里提到了“选择器节点”，它是一种特殊的行为树节点，允许你通过外部应用程序（比如ROS主题）动态地更改导航系统中使用的算法或设置。通常情况下，更改系统设置最好通过行为树内部的逻辑来实现，但是这里提到了有些专业用户可能会使用外部应用程序来进行这些更改）。

<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>

• 这个其实就是上面提到的那棵行为树
• MainTree 是顶级节点，其下是一个用于恢复和故障处理的 RecoveryNode。
• 在 NavigateRecovery 节点下，有两个 RecoveryNode 子节点：NavigateWithReplanning（规划路径）和 RecoveryFallback（恢复行为）。
• NavigateWithReplanning 子节点使用 PipelineSequence 控制节点，包含了路径规划和跟随路径的操作，以及针对这些操作的恢复策略。RateController 控制路径规划的频率，而 ComputePathToPose 和 FollowPath 执行具体的路径规划和跟随操作，分别具有 ReactiveFallback 子节点用于恢复。
• RecoveryFallback 子节点包含了 GoalUpdated 和 RoundRobin 控制节点，用于系统级别的恢复。其中 RoundRobin 包含一系列恢复操作节点，如清除成本地图、旋转、等待和后退。

        整个行为树结构旨在实现对导航系统的恢复和故障处理，使得当导航任务遇到问题时，系统能够尝试各种恢复操作以解决问题。

1. Navigate To Pose（导航到姿势）：从起始点到达单个目标点。
2. Navigate Through Poses（穿越多个姿势导航）：依次穿越多个目标点形成的路径。
3. Navigate To Pose and Pause Near Goal-Obstacle（导航到姿势并在接近目标障碍物时暂停）：在接近目标附近遇到障碍物时，暂停导航以等待障碍物清除。
4. Navigate To Pose With Consistent Replanning And If Path Becomes Invalid（具有一致重新规划且路径无效时导航到姿势）：在路径无效时，具有重新规划功能的导航。
5. Follow Dynamic Point（跟随动态点）：导航系统跟随动态移动的目标点。
6. Odometry Calibration（里程计校准）：校准机器人的里程计，使其导航更准确。

• Navigate Through Poses（这个其实和上面的差不多）

        这个行为树实现了从一个起始点通过多个中间硬姿势约束到最终目标的导航行为。它包含了在特定子上下文中使用自定义行为进行恢复以及针对系统级别失败的全局恢复子树（这个地方说到的是大树，而不是子树）。此外，它还提供了在返回失败状态之前多次重试任务的机会。

        `ComputePathThroughPoses` 和 `FollowPath` 的 BT 节点都指定了它们要使用的算法。按照惯例，我们根据算法的类型进行命名（例如，不是 DWB，而是 FollowPath），以便行为树或应用程序开发人员不必担心技术细节。他们只想使用路径跟踪控制器。

        在这个行为树中，我们尝试在返回任务失败之前重试整个导航任务 6 次。这使得导航系统有充足的机会尝试从失败条件中恢复，或者等待短暂问题解决，比如人群拥挤或临时传感器故障。

        在正常执行中，这将每 3 秒重新规划路径<RateController hz="0.333">，并将该路径传递给控制器，类似于 Nav2 行为树 Nav2 Behavior Trees 中的行为。但是，规划器现在是 `ComputePathThroughPoses`，它接受一个目标向量，而不是单个姿势目标。`RemovePassedGoals` 节点用于移除机器人已经通过的目标。在这种情况下，当机器人距离目标在列表中的下一个目标点不到 0.5 时，该姿势将从目标列表中删除。这样实现的目的是在机器人经过一些中间姿势后重新规划路径，并避免将它们考虑到未来的重新规划中（描述了一个策略，当机器人接近目标列表中的下一个目标点，距离小于 0.5 时，就会将这个目标点从列表中删除。这个策略的目的在于优化路径规划的效率。机器人已经接近目标，而不再需要规划路径去到这个目标。删除已经接近的目标可以让机器人避免不必要的规划，提高导航效率）。如果规划器失败，它将触发其子树中的上下文感知恢复，清除全局成本地图。可以在此处添加额外的恢复行为，以进行其他上下文特定的恢复，例如尝试另一个算法。

        同样，控制器也具有类似的逻辑。如果控制器失败，它也会尝试清除影响控制器的局部成本地图。值得注意的是，`ReactiveFallback` 中的 `GoalUpdated` 节点。这使我们可以在通过优先处理传递给导航系统的新目标时退出恢复状态。这确保了导航系统在发出新目标时会立即做出响应，即使上一个目标在尝试恢复中也会得到响应。

        如果这些上下文恢复失败，此行为树将进入恢复子树。这个子树专门用于处理系统级别的故障，帮助解决机器人被卡住或处于不良位置的问题。这个子树还有一个 `GoalUpdated` 节点，每次迭代都会进行检查，以确保新目标的响应性。接下来，恢复子树将执行清除成本地图、旋转、等待和后退等恢复操作。在子树的每个恢复之后，将重新尝试主导航子树。如果持续失败，将执行恢复子树中的下一个恢复操作。        虽然这个行为树没有使用，但 `PlannerSelector`、`ControllerSelector` 和 `GoalCheckerSelector` 的行为树节点也可能很有用。这些节点允许用户通过 ROS 主题动态更改导航系统中使用的算法，而不是硬编码算法（如 GridBased 和 FollowPath）。可能更明智的做法是使用带有指定算法的条件节点在最有用和独特的情况下创建不同的子树上下文。但是，选择器节点是一种从外部应用程序而不是通过内部行为树控制流逻辑来更改算法的有用方式。最好通过行为树方法来实现更改，但我们了解许多专业用户有外部应用程序来动态更改其导航器的设置。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="0.333">
          <RecoveryNode number_of_retries="1" name="ComputePathThroughPoses">
            <ReactiveSequence>
              <RemovePassedGoals input_goals="{goals}" output_goals="{goals}" radius="0.5"/>
              <ComputePathThroughPoses goals="{goals}" path="{path}" planner_id="GridBased"/>
            </ReactiveSequence>
            <ReactiveFallback name="ComputePathThroughPosesRecoveryFallback">
              <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>

• Navigate To Pose and Pause Near Goal-Obstacle

作为先决条件，我们鼓励用户阅读行为树文档 Behavior Tree documentation ，该文档解释了行为树中使用的不同行为节点，例如ReactiveSequence、SequenceStar和RetryUntilSucessfull。

        这个行为树是对“导航到姿势 Navigate To Pose ”（“导航到姿势”指的是机器人在自主导航过程中，从当前位置导航到特定的目标位置和姿势的行为。这通常涉及机器人根据传感器数据和环境地图规划路径，并执行相应的动作来达到目标位置和方向）的一种软扩展。除了“导航到姿势 Navigate To Pose ”的功能外，这个行为树允许机器人高效地处理靠近目标的障碍物（例如叉车、人员或其他临时障碍物），暂停机器人的导航并等待用户指定的时间，以检查障碍物是否清除。如果在等待期间障碍物移动了，机器人将继续前往目标，采用较短的路径。如果在等待期间障碍物没有移动或等待时间到期，则机器人将绕过更长的路径到达最终目标位置。对于给定的任务，这个行为树有助于解决因靠近目标位置存在临时障碍物而生成长路径导致的长周期时间问题。

        下图显示了行为树的结构。从图中可以注意到导航子树中有一个额外的分支称为“MonitorAndFollowPath”。这个分支是为用户执行机器人应该展示的任何类型的监视行为而创建的。在这个特定的行为树中，监视分支专门由“PathLongerOnApproach”行为树节点使用，用于检查全局规划器是否决定在机器人接近用户指定的目标接近区域时为其规划了明显更长的路径。如果没有明显更长的路径，则监视节点进入“FollowPath”恢复节点，然后生成必要的控制命令（简单来说，这个分支是用来确保机器人在接近目标时走的路径是最优的，如果不是，就会调整路径并继续执行任务）。

         当存在明显更长的路径时，“PathLongerOnApproach”节点的子节点将被触发。子节点是一个“RetryUntilSuccesfull”修饰器节点，它又有一个“SequenceStar”节点作为其子节点。首先，“SequenceStar”节点通过触发“CancelControl”节点来取消控制服务器，从而停止机器人的进一步导航。接下来，“SequenceStar”节点触发“Wait”节点，使机器人等待指定的用户指定时间。这里需要注意的是，“MonitorAndFollowPath”是一个“ReactiveSequence”节点，因此在“FollowPath”节点再次被触发之前，“PathLongerOnApproach”节点需要返回“SUCCESS”。(当机器人接近目标时，如果全局路径规划器（ComputePathToPose）检测到生成了一个更长的路径（可能是因为遇到了障碍物或其他问题），就会触发 PathLongerOnApproach 部分。这个部分会尝试取消当前导航，使机器人暂停一段时间等待。如果在等待期间发现障碍物已经消失，机器人会选择更短的路径继续导航。但如果障碍物仍然存在，或者等待时间结束，机器人将选择绕过障碍物的更长路径以到达目标。这个机制的目的是优化机器人在目标附近的路径选择，以确保它能够尽快到达目的地)

        在下面的GIF中，可以看到机器人正在接近目标位置，但在目标附近发现了障碍物，导致全局规划器计划了一条更长的路径绕过障碍物。这就是“PathLongerOnApproach”节点被触发的地方，它触发了其子节点，因此取消了控制服务器并等待障碍物是否消失。在这种情况下，障碍物没有消失，导致机器人走了更长的路径。

        或者，如果障碍物被清除，全局规划器生成了一条更短的路径。这时，PathLongerOnApproach 返回成功，导致 FollowPath 继续机器人的导航。

        除了上述情况之外，还需要注意的是，如果障碍物在给定的特定等待时间内没有消除，机器人将会选择更长的路径到达目标位置。

        总的来说，这个特定的行为树可以作为一个示例，也可以作为一个准备就绪的行为树，适用于希望优化其流程周期时间的特定组织应用。

<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"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <ReactiveSequence name="MonitorAndFollowPath">
          <PathLongerOnApproach path="{path}" prox_len="3.0" length_factor="2.0">
            <RetryUntilSuccessful num_attempts="1">
              <SequenceStar name="CancelingControlAndWait">
                <CancelControl name="ControlCancel"/>
                <Wait wait_duration="5"/>
              </SequenceStar>
            </RetryUntilSuccessful>
          </PathLongerOnApproach>
          <RecoveryNode number_of_retries="1" name="FollowPath">
            <FollowPath path="{path}" controller_id="FollowPath"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </RecoveryNode>
        </ReactiveSequence>
      </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.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

• Navigate To Pose With Consistent Replanning And If Path Becomes Invalid

        这个行为树实现了 Nav2 行为树中更为成熟的版本。它从起始点导航到自由空间中的单一目标点。它包含了在特定子树上下文中使用自定义恢复措施以及用于系统级故障的全局恢复子树。它还为用户提供了在返回失败状态之前多次重试任务的机会。

        ComputePathToPose 和 FollowPath 行为树节点都指定了它们要使用的算法。按照惯例，我们将这些命名为它们所代表的算法风格（例如，不是 DWB 而是 FollowPath），以便行为树或应用程序开发人员无需关心技术细节。他们只想使用路径跟随控制器。

        在这个行为树中，我们尝试在返回任务失败之前重试整个导航任务 6 次。这使得导航系统有充分的机会尝试从失败条件中恢复或等待暂时性问题解决，例如人群拥挤或临时传感器故障。

        在正常执行中，重新规划可以由无效的先前路径、新目标或者如果在 10 秒内没有创建新路径来触发。如果规划器或控制器失败，它将在其子树中触发具有上下文意识的恢复。目前，如果规划器失败，恢复将清除全局代价图，如果控制器失败，恢复将清除本地代价图。可以向这些子树添加额外的上下文特定恢复措施。

        如果这些上下文恢复失败，该行为树将进入恢复子树。这个子树用于解决诸如机器人卡住或处于糟糕位置等系统级故障问题。该子树具有 GoalUpdated BT 节点，它每次迭代都会进行检查以确保对新目标的响应性。接下来，恢复子树将尝试以下恢复措施：清除代价图操作、旋转、等待和后退。在子树中的每个恢复措施之后，将重新尝试主导航子树。如果继续失败，将触发恢复子树中的下一个恢复措施。

        尽管这个行为树没有使用到，但 PlannerSelector、ControllerSelector 和 GoalCheckerSelector 行为树节点也可能很有用。这些行为树节点不是硬编码要使用的算法（如 GridBased 和 FollowPath），而是允许用户通过 ROS 主题动态更改导航系统中使用的算法。可能更明智的做法是使用具有指定算法的条件节点在不同的子树上下文中创建不同的子树。但选择器节点可以是一个有用的方式，从外部应用程序而不是通过内部行为树控制流逻辑来更改算法。通过行为树方法实现变更更佳，但我们了解许多专业用户有外部应用程序来动态更改其导航器的设置。                       

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="2.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <Fallback>
              <ReactiveSequence>
                <Inverter>
                  <PathExpiringTimer seconds="10" path="{path}"/>
                </Inverter>
                <Inverter>
                  <GlobalUpdatedGoal/>
                </Inverter>
                <IsPathValid path="{path}"/>
              </ReactiveSequence>
              <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            </Fallback>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </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.30" backup_speed="0.05"/>
        </RoundRobin>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

• Follow Dynamic Point

        这个行为树实现了从一个起始点导航到一个动态点的行为。这个“动态点”可以是一个人、另一个机器人、一个虚拟的对象，或者其他什么。唯一的要求是你想要跟随的姿势被发布到了 GoalUpdater 节点指定的主题中。

        在这个树中，我们以与“导航到指定位姿”相同的 1Hz 频率进行重新规划，使用 ComputePathToPose 节点。然而，这一次当我们重新规划时，我们会根据更新的目标主题中的最新信息来更新目标点。当我们规划到达动态点的路径后，我们使用 TruncatePath 节点来删除靠近动态点末端的路径点。这个行为树节点非常有用，它确保即使机器人停下来，也始终保持与障碍物的最小距离。它还平滑了在尝试朝着代价地图中可能被占用的空间进行路径规划时出现的任何偏离路径的行为。

（

在进行路径规划后对路径进行修剪的过程。在这个场景中，使用了 TruncatePath 节点，它的作用是删除已规划路径末端靠近动态点的路径点。这样做的目的有两个方面：

1. 保持安全距离： 机器人在接近动态点时，需要保持一定的安全距离，即使停止也不会与障碍物相撞。TruncatePath 确保机器人在接近目标时仍能保持安全距离，即使停下来。

2. 路径平滑： 当机器人尝试朝着可能被占用的空间进行路径规划时，可能会导致路径偏离，TruncatePath 节点的作用是平滑路径，以避免机器人在规划路径时出现异常或与障碍物发生碰撞。

在机器人感知的环境中，有些区域可能被其他物体、障碍物或者移动物体所占据。因此，当机器人尝试规划路径到一个动态点，即可能被占据的地方时，可能出现路径偏离或不稳定的情况。TruncatePath 节点的作用就是确保机器人在接近这些可能被占据的区域时，能够维持安全距离，并保持路径的稳定性。

）

        在计算出新的到达动态点的路径并对其进行截断后，再次通过 FollowPath 节点传递给控制器。然而，请注意，它位于 KeepRunningUntilFailure 修饰节点下，确保控制器持续执行直到出现故障模式。这个行为树将一直运行直到导航请求被抢占或取消。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <Sequence>
          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
            <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
          </GoalUpdater>
         <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
        </Sequence>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{truncated_path}" controller_id="FollowPath"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

• Odometry Calibration

        这个行为树使用“DriveOnHeading”和“Spin”行为让机器人逆时针方向绕正方形行驶三次。机器人会以0.2米/秒的速度行驶每条边长2米的正方形，然后进行90度转弯。这是一个简单的实验，用于测量里程计的准确性，并可以重复使用来调整与里程计相关的参数，以提高质量。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <Repeat num_cycles="3">
      <Sequence name="Drive in a square">
        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
        <Spin spin_dist="1.570796" is_recovery="false"/>
        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
        <Spin spin_dist="1.570796" is_recovery="false"/>
        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
        <Spin spin_dist="1.570796" is_recovery="false"/>
        <DriveOnHeading dist_to_travel="2.0" speed="0.2" time_allowance="12"/>
        <Spin spin_dist="1.570796" is_recovery="false"/>
      </Sequence>
    </Repeat>
  </BehaviorTree>
</root>