详细的行为树演练

概述
必要条件
Navigate To Pose With Replanning and Recovery
Navigation Subtree
Recovery Subtree

概述

本文作为Nav2中主要行为树 (BT) 的参考指南。 [校准@123---向佐转]

在 nav2_bt_navigator/behavior_trees 中提供了行为树示例，但是这些示例必须基于机器人的应用进行重新配置。以下文档将详细介绍当前主要默认的BT navigate_to_pose_w_replanning_and_recovery.xml 。 [校准@123---向佐转]

前提条件

在继续之前，请熟悉行为树的概念 [校准@123---向佐转]
- 阅读导航相关概念介绍的简短解释 [校准@mzebra]
- 阅读 BehaviorTree CPP V3 网站上的一般教程和指南 (非Nav2专用)。具体来说 BehaviorTree CPP V3 网站上的 "Learn the Basics" 部分解释了本指南基础部分所使用的一般节点类型的介绍。 [校准@mzebra]
熟悉 Nav2 specific BT nodes [校准@123---向佐转]

通过重新规划和恢复导航到某个位姿

下节将详细描述目前在Nav2、 navigate_to_pose_w_replanning_and_recovery.xml 中使用的主行为树和默认行为树的概念。此行为树以1Hz定期重新规划全局路径，并且它还具有恢复操作。 [校准@123---向佐转]

行为树（BT）主要在XML中定义。上面显示的树在 XML 中表示如下。 [校准@123---向佐转]

<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``的子树。这可以用以下方式表示: [校准@123---向佐转]

这个 Navigation 子树实际上主要包含导航行为: [校准@123---向佐转]

calculating a path
路径跟随 [校准@123---向佐转]
上述每种主要导航行为的恢复行为 [校准@123---向佐转]

这个 Recovery 子树包括系统故障恢复行为或者项目内部不易处理的恢复行为。 [校准@123---向佐转]

整个行为树希望大部分时间花在“Navigation”子树中。如果 Navigation 子树中的两个主要行为有一个失败 (路径计算或路径跟随)，将尝试恢复整个系统。 [校准@123---向佐转]

如果整个系统恢复仍然不够， Navigation 子树将返回 FAILURE 。系统将移动到 Recovery 子树，然后尝试清除所有系统级导航故障。 [校准@123---向佐转]

这种情况发生，直到原系统 RecoveryNode 被``number_of_retries`` 超过 (默认情况下为6)。 [校准@123---向佐转]

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

导航子树

既然我们已经讨论了 Navigation 子树和 Recovery 子树之间的控制流，下面让我们关注导航子树。 [校准@123---向佐转]

该子树的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 有两个主要作用。如果这两个操作有一个失败，他们将尝试根据上下文清除失败。树的关键只能用一个父节点和两个子节点来表示，如下所示: [校准@123---向佐转]

父节点``PipelineSequence`` 允许 ComputePathToPose 被勾选，并且一旦被勾选， FollowPath 将被勾选。当 FollowPath 子树被勾选时， ComputePathToPose 子树也将被勾选。这允许在机器人四处移动时重新计算路径。 [校准@123---向佐转]

ComputePathToPose 和 FollowPath 都遵循相同的总体结构。 [校准@123---向佐转]

执行动作 [校准@123---向佐转]
如果动作执行失败，请尝试查看我们是否可以上下文恢复 [校准@123---向佐转]

The below is the ComputePathToPose subtree:

父级 RecoveryNode 控制动作和上下文恢复子树之间的流程。 ComputePathToPose 和 FollowPath 的上下文恢复包括检查目标是否已经更新，和清除相关的成本图。 [校准@123---向佐转]

如果应用程序在进入系统之前可以接受较多的上下文恢复尝试，您可以更改父“RecoveryNode”控制节点中的“number_of_retries”参数。 [校准@123---向佐转]

在BT子树``ComputePathToPose`` 和 ``FollowPath``中的唯一区别概述如下: [校准@123---向佐转]

子树中的动作节点: [校准@123---向佐转]
- ComputePathToPose 子树以``ComputePathToPose`` 动作为中心。 [校准@123---向佐转]
- FollowPath 子树以 FollowPath 动作为中心。 [校准@123---向佐转]
修饰``ComputePathToPose``子树的``RateController`` [校准@123---向佐转]
RateController``控制``ComputePathToPose``子树以保证规划在指定的频率。BT树的默认频率是1 hz。这样做是为了防止BT以树更新速率 (100Hz) 用太多无用的请求浪费规划服务器资源。根据应用和计算路径的计算成本，可以考虑将该频率更改为更高或更低的频率。还有其他装饰可以用来代替这个 ``RateController 。如果合适，考虑使用 SpeedController 或 DistanceController 装饰器。 [校准@123---向佐转]
在上下文恢复中清除costmap: [校准@小鱼]
- 子树``ComputePathToPose`` 清除全球成本图。全球成本图是planner背景下的相关成本图 [校准@小鱼]
- 这个 FollowPath 子树清除了当地的成本图。本地costmap是控制者上下文中的相关costmap [待校准@145]

恢复（Recovery）子树

这个 Recovery 子树是Nav2默认 navigate_to_pose_w_replanning_and_recovery.xml 树的第二大 "half" 。简而言之，当 Navigation 子树返回 FAILURE 控制系统级别的恢复时，就会触发这个子树 (在这种情况下， Navigation 子树中的上下文恢复是不够的)。 [待校准@147]

以及XML片段: [待校准@148]

<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模式，“除非发生'某种情况'，否则正常执行某动作”。 [校准@Excitingship]

这些条件节点非常强大，通常与 ReactiveFallback 配对。可以很容易地想象用 isBatteryLow 条件将整个 navigate_to_pose_w_replanning_and_recovery 树包裹在 ReactiveFallback 中 -- 这意味着 navigate_to_pose_w_replanning_and_recovery 树将执行 * 除非 * 电池电量变低 (然后新的一整个不同的子树来表示对接充电)。 [校准@Excitingship]

如果目标从未更新，行为树将继续到 RoundRobin 节点。以下是BT中默认的四个系统级恢复: [待校准@151]

清除两个成本图 (本地和全局) 的序列 [待校准@152]
Spin recovery action
Wait recovery action
BackUp recovery action

在 SUCCESS 的四个孩子父 RoundRobin ，机器人将尝试renavigate在 Navigation 子树。如果重新隔离不成功， RoundRobin 的下一个孩子将被勾选。 [待校准@156]

例如，假设机器人卡住， Navigation 子树返回 FAILURE :( 为了这个例子，让我们假设目标从未更新)。 [校准@小鱼]

尝试使用 Recovery 子树中的代价地图清除序列，并返回 SUCCESS 。机器人现在再次移动到 Navigation 子树 [待校准@158]
让我们假设清除两个成本图是不够的，并且 Navigation 子树再次返回 FAILURE 。机器人现在滴答 Recovery 子树 [待校准@159]
在 Recovery 子树中， Spin 的作用将被勾选。如果这返回 SUCCESS ，那么机器人将返回到主 Navigation 子树 * 但是 * 让我们假设 Spin 的恢复返回 FAILURE 。在这种情况下，树将 * 保留 * 在 Recovery 子树中 [待校准@160]
假设下一个恢复行动， Wait 返回 SUCCESS 。然后机器人将移动到 Navigation 子树 [待校准@161]
假设 Navigation 子树返回 FAILURE (清除成本图，尝试旋转，等待仍然不足以恢复系统。机器人将移动到 Recovery 子树上并尝试 BackUp 行动。假设机器人尝试了 BackUp 动作，并且能够成功完成动作。 BackUp 动作节点返回 SUCCESS ，所以现在我们再次进入导航子树。 [待校准@162]
在这个假设的场景中，让我们假设 BackUp 的动作允许机器人在 Navigation 子树中成功导航，并且机器人到达目标。在这种情况下，整个BT仍将返回 SUCCESS 。 [校准@Excitingship]

如果 BackUp 的作用不足以让机器人不卡住，上述逻辑将无限期地继续下去，直到 Navigate 子树和 Recovery 子树的母树中的 number_of_retries 被超越，或者如果 Recovery 子树中的所有系统范围的恢复都返回 FAILURE (这不太可能，并且可能指向其他一些系统故障)。 [待校准@164]