行为树详细演练

概述
先决条件
通过重新规划和恢复导航至姿势
导航子树
恢复子树

概述

本文档是 Nav2 中使用的主要行为树 (BT) 的参考指南。

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

先决条件

在继续本演练之前，请熟悉行为树的概念
阅读`导航概念<../../concepts/index.html>`_中的简短说明
阅读`BehaviorTree CPP V3 <https://www.behaviortree.dev/>`_网站上的一般教程和指南（不是特定于 Nav2 的）。具体来说，BehaviorTree CPP V3 网站上的“学习基础知识”部分介绍了本指南将基于的基本通用节点。
熟悉自定义`Nav2 特定 BT 节点<nav2_specific_nodes.html>`_

通过重新规划和恢复导航至姿势

以下部分将详细描述 Nav2 中当前使用的主要和默认 BT 的概念“navigate_to_pose_w_replanning_and_recovery.xml”。此行为树以 1 Hz 的频率定期重新规划全局路径，并且还具有恢复操作。

BT 主要以 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”子树将被称为“Navigation”子树，“RecoveryFallback”子树将被称为“Recovery”子树。这可以用以下方式表示：

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

计算路径
遵循路径
上述每个主要导航行为的上下文恢复行为

恢复 子树包括系统级故障或内部不易处理的项目的行为。

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

如果上下文恢复仍然不够，导航 子树将返回 失败。

系统将转到 恢复 子树以尝试清除任何系统级导航故障。

这种情况会发生，直到超出父 恢复节点 的 ``重试次数``（默认情况下为 6）。

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

导航子树

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

该子树的 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”动作为中心。
装饰“ComputePathToPose”子树的“RateController”

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

在上下文恢复中清除的代价地图：
“FollowPath”子树清除本地代价地图。本地代价图是控制器上下文中的相关代价图

恢复子树

恢复 子树是 Nav2 默认 navigate_to_pose_w_replanning_and_recovery.xml 树的第二个大“半部分”。简而言之，当 导航 子树返回 FAILURE 时触发此子树，并在系统级别控制系统级的恢复（在 导航 子树中的上下文恢复不足的情况下）。

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”控制系统范围内其余恢复之间的流程，并异步检查是否已收到新目标。如果目标在任何时候得到更新，此子树将停止所有子节点并返回“SUCCESS”。这允许对新目标做出快速反应并抢占当前正在执行的恢复。这应该与“Navigation”子树的上下文恢复部分相似。这是一种常见的 BT 模式，用于处理“除非发生‘此条件’，否则执行操作 A”的情况。

这些条件节点可能非常强大，通常与“ReactiveFallback”配对。可以很容易地想象将整个 navigate_to_pose_w_replanning_and_recovery 树包装在具有 isBatteryLow 条件的 ReactiveFallback 中 - 这意味着 navigate_to_pose_w_replanning_and_recovery 树将执行*除非*电池电量不足（然后进入不同的子树进行对接充电）。

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

清除两个成本图（本地和全局）的序列
Spin 操作
Wait 操作
BackUp 操作

当父级 RoundRobin 的四个子级中的任何一个“成功”时，机器人将尝试在 Navigation 子树中重新导航。如果此重新导航不成功，则将勾选 RoundRobin 的下一个子级。

例如，假设机器人卡住了，并且“导航”子树返回“失败”：（为了便于说明，我们假设目标从未更新）。

尝试“恢复”子树中的 Costmap 清除序列，并返回“成功”。机器人现在再次移动到“导航”子树
假设清除两个 costmap 还不够，并且“导航”子树再次返回“失败”。机器人现在勾选“恢复”子树
在“恢复”子树中，将勾选“旋转”操作。如果返回“成功”，则机器人将返回主“导航”子树但假设“旋转”操作返回“失败”。在这种情况下，树将*保留*在“恢复”子树中
假设下一个操作“等待”返回“成功”。然后，机器人将移动到“导航”子树
假设“导航”子树返回“失败”（清除成本图、尝试旋转和等待仍然不足以恢复系统。机器人将移动到“恢复”子树并尝试“备份”操作。假设机器人尝试“备份”操作并能够成功完成该操作。“备份”操作节点返回“成功”，因此现在我们再次转到导航子树。
在这个假设场景中，我们假设“备份”操作允许机器人在“导航”子树中成功导航，并且机器人达到目标。在这种情况下，整体 BT 仍将返回“成功”。

如果“备份”操作不足以让机器人脱离卡住，则上述逻辑将无限期地持续下去，直到超出“Navigate”子树和“Recovery”子树的父级中的“number_of_retries”，或者“Recovery”子树中的所有系统范围的恢复都返回“FAILURE”（这不太可能，很可能指向其他系统故障）。