上面是标题～-～

行为树

行为树是一种形式化的图形建模语言，由Dromey于2001年首次提出，通过树结构和形式语义将自然语言需求转化为可执行系统模型，用于解决大规模系统中需求的模糊性、冗余性及短期记忆过载问题。以上摘自百度百科。

行为树，顾名思义，就是通过树形结构去描述和控制系统的行为流程。不同于有限状态机(Finite State Machine, FSM)，在复杂任务中，通常多状态的状态转移流程十分复杂，而且随着系统的规模增大，状态机会变得越来越不直观，难以管理和修改。

但是行为树通过层次化的节点结构，能够更直观地表示复杂的行为流程，并且便于修改和维护。

行为树概念

顾名思义，由于行为树使用树形结构去描述系统的运行流程，那么肯定也有一些数据结构中有关树的基本概念：

根节点(Root Node)：行为树的起始节点，通常位于树的最顶端。
子节点(Child Node)：根节点或其他节点的下一级节点，用于描述具体的行为或子任务。
父节点(Parent Node)：子节点的上一级节点，用于描述更高层级的行为或任务。
叶子节点(Leaf Node)：没有子节点的节点，通常用于描述具体的行为或任务。

在行为树中，通过不同功能的节点进行树形结构的组合，可以实现复杂的行为逻辑控制。一个行为树的示例如下：

这来自BTCPPV4的示例。

上面的行为树通过不同节点的组合实现了进入房间的行为流程，具体的每个节点是干什么的，将在下一节中详细介绍。

节点(Node)类型与功能

在行为树中，按照其功能划分，节点可以分为四类。具体可以看BTCPV4的中的图片所示：

即行为树分为装饰器节点(Decorator Node)、控制节点(Control Node)、条件节点(Condition Node)和动作节点(Action Node)四类。

python 复制代码

# 以下节点可以作为父节点亦或子节点，但不可作为叶子节点。
- 控制节点(Control Node)：用于控制子节点的执行逻辑，可以有一个到多个子节点。
- 装饰器节点(Decorator Node)：用于对子节点的行为进行修饰或增强，通常只有一个子节点。

# 以下节点通常作为叶子节点，即它们没有子节点。
- 条件节点(Condition Node)：用于判断某个条件是否满足，通常只有一个子节点。
- 动作节点(Action Node)：用于执行具体的动作，通常只有一个子节点。

对于每个节点，其会接受来自父节点的Tick信号，即父节点会通知子节点进行更新或执行，然后子节点会根据自身的逻辑返回执行结果给父节点。通常的返回结果包括成功、失败或运行中。

python 复制代码

- 成功(Success)：子节点执行成功，父节点根据逻辑继续执行。
- 失败(Failure)：子节点执行失败，父节点根据逻辑处理失败情况。
- 运行中(Running)：子节点尚未完成执行，父节点等待其返回结果。

# 父节点接受这个返回结果，并根据自身的逻辑继续执行下一步的动作。

当然，对于Condition Node，其返回结果通常只有成功或失败，不会返回运行中。而对于Action Node，其返回结果可能包括成功、失败或运行中。

常见控制节点

控制节点(Control Node)用于控制子节点的执行逻辑，可以有一个到多个子节点。

序列节点(Sequence Node)：按顺序执行其子节点，只有当所有子节点都成功时，该节点才返回成功。

python 复制代码

- 当子节点1执行成功时，序列节点会继续执行子节点2，依此类推，直到所有子节点都执行完毕。
- 如果某个子节点执行失败，序列节点会立即返回失败，不再执行后续的子节点。
- 仅有当所有子节点都成功时，序列节点才会返回成功。

即

python 复制代码

if child_node_1.execute() == "Success" then  tick(child_node_2)

if child_node_x.execute() == "Failure" then  Return "Failure"

if all child nodes execute successfully then  Return "Success"

应用Sequence Node 的一个简单的例子如下
flowchart TD A $"序列节点（Sequence）"$ --> B $"解锁门\动作节点（Action）"$ A --> C $"开门\动作节点（Action）"$

tick的运行顺序：

python 复制代码

tick->解锁门->failure->序列节点返回失败
or

tick->解锁门->success->开门->failure->序列节点返回失败
or

tick->解锁门->success->开门->success->序列节点返回成功

回退节点(Fallback Node)：用于在子节点执行失败时提供备选方案，通常有一个到多个子节点。

Fallback Node也被叫做选择节点(Selector Node)

python 复制代码

- 当子节点1执行失败时，回退节点会继续执行子节点2，依此类推，直到某个子节点成功或所有子节点都失败。
- 如果某个子节点执行成功，回退节点会立即返回成功，不再执行后续的子节点。
- 仅有当所有子节点都失败时，回退节点才会返回失败。

即

python 复制代码

if child_node_1.execute() == "Failure" then  tick(child_node_2)

if child_node_x.execute() == "Success" then  Return "Success"

if all child nodes execute failure then  Return "Failure"

其常见的场景是一个条件节点加动作节点的组合，用于在条件满足时执行动作，而在条件不满足时才执行备选动作。
flowchart TD A $"回退节点（Fallback）"$ --> B $"门是否已打开？\条件节点（Condition）"$ A --> C $"开门\动作节点（Action）"$

运行顺序

python 复制代码

tick->门是否已打开？->success->回退节点返回成功
or

tick->门是否已打开？->failure->开门->success->回退节点返回成功
or

tick->门是否已打开？->failure->开门->failure->回退节点返回失败

常见装饰器节点

装饰器节点(Decorator Node)通常用来对其唯一的子节点的行为进行修饰或增强，如对子节点的返回进行取反、重试或限制执行次数等操作。

下面仅仅是举几个例子：

取反节点(Invert Node): 用于对子节点的返回结果进行取反操作，如果子节点返回成功，则取反节点返回失败；如果子节点返回失败，则取反节点返回成功。

这个逻辑非常好理解

python 复制代码

if child_node.execute() == "Success" then  Return "Failure"
or
if child_node.execute() == "Failure" then  Return "Success"

重试节点(Retry Node): 用于对子节点的返回结果进行重试操作，如果子节点返回失败，则重试节点会再次执行子节点，直到子节点成功或达到最大重试次数。

python 复制代码

max_retries = 3 # 最大重试次数
retry_count = 0 # 当前重试次数
while retry_count < max_retries:
    if child_node.execute() == "Success":
        Return "Success"
    else:
        retry_count += 1
Return "Failure"

## 这里声明，所有的代码仅为伪代码示例，并非真实可运行的代码

正如BTCPV4文档中给出的示例：

当Fallback Node向其左孩子节点发送Tick后，IsDoorClosed的结果会通过Inverter节点进行取反，然后再传递会父节点，这相当于IsDoorOpen的逻辑。同时如果门没有打开，即IsDoorClosed返回成功，经过取反后，Fallback Node接收到失败，从而会继续执行右孩子节点，即开门动作。

而其右侧的孩子节点先经过一个叫做RetryUntilSuccess的装饰器节点，该节点会确保子节点在达到最大重试次数之前一直重试，直到子节点成功(Return Success)或者达到最大重试次数(Return Failure)为止。

最后，至于Action Node，它通常用于执行具体的动作，如开门、关门等，这部分实际上是真正干活的地方，就如Nav2导航时的一个个节点如planner、controller等。只要定义好其运行的接口，做好返回的操作即可。

简单的行为树示例

flowchart TD A $"顺序节点（Sequence）"$ --> B $"回退节点（Fallback）"$ A --> C $"进入房间\动作节点（Action）"$ B --> D("门是否打开？<br/>条件节点（Condition）") B --> E $"开门\动作节点（Action）"$ B --> F $"顺序节点（Sequence）"$ B --> G $"破门\动作节点（Action）"$ F --> H("有钥匙吗？<br/>条件节点（Condition）") F --> I $"解锁门\动作节点（Action）"$ F --> J $"开门\动作节点（Action）"$

这是一个通过行为树来实现的开门逻辑，根节点为顺序节点（Sequence）。

第二层中的Fallback Node及其子节点作用是进行开门逻辑的判断和执行

复制代码

1. 若门已经打开，则Fallback Node返回成功，不用在去执行多余的动作去开门
2. 若门未打开，则Fallback Node会尝试执行开门动作，如果开门成功，则返回成功；如果开门失败，则继续尝试其他备选动作。
3. 备选动作中，解锁门的逻辑使用一个顺序节点去执行，只有当有钥匙时才会尝试解锁门，如果解锁成功，则继续尝试开门动作。
4. 若解锁门失败，则Fallback Node会继续尝试其他备选动作，如破门动作。

然后当Fallback Node返回失败时，整体失败，顺序节点也会返回失败，整个行为树的执行结束。

当Fallback Node返回成功时，顺序节点会继续执行其右侧的子节点，即进入房间的动作节点。

使用xml描述树结构

在BTCPV4中，行为树的结构可以使用XML文件进行描述，一个典型的XML描述示例如下：

xml 复制代码

 <root BTCPP_format="4">
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <SaySomething   name="action_hello" message="Hello"/>
            <OpenGripper    name="open_gripper"/>
            <ApproachObject name="approach_object"/>
            <CloseGripper   name="close_gripper"/>
        </Sequence>
     </BehaviorTree>
 </root>

block-beta columns 8 space:3 A $"root_sequence\序列节点（Sequence）"$ :2 space:3 space:8 B $"SaySomething\说：Hello\动作节点（Action）"$ :2 C $"OpenGripper\打开夹爪\动作节点（Action）"$ :2 D $"ApproachObject\接近物体\动作节点（Action）"$ :2 E $"CloseGripper\关闭夹爪\动作节点（Action）"$ :2 A --> B A --> C A --> D A --> E

可以看到，使用xml描述行为树，必须的框架如下

xml 复制代码

<!-- 指明行为树的格式，这里使用BTCPP_format="4"表示使用BTCP的第四版的格式 -->
 <root BTCPP_format="4"> 
     <!-- 描述一棵行为树，ID是该行为树的唯一标识，必须指定 -->
     <BehaviorTree ID="MainTree">
        ....
     </BehaviorTree>
 </root>

而在行为树中，每个节点都可以有相应的子节点，通过XML的结构可以清晰地表示出节点之间的关系。

xml 复制代码

<root BTCPP_format="4">
     <BehaviorTree ID="MainTree">
        <!-- 根序列节点及其子节点 -->
        <Sequence name="root_sequence">
            <!-- 顺序节点的子节点 -->
            <SaySomething   name="action_hello" message="Hello"/>
            <OpenGripper    name="open_gripper"/>
            <ApproachObject name="approach_object"/>
            <CloseGripper   name="close_gripper"/>
        </Sequence>
     </BehaviorTree>
 </root>

上面是BTCPV4中xml格式行为树描述的标签规范。可以看到，每个节点的标签是注册在行为树解析器中的，通过其ID进行引用。

其中name是一个可选属性，用于给节点实例命名，方便在日志、调试和 Groot 可视化工具中区分节点。即对于多个同一类型的节点，可以通过name属性进行区分。

xml 复制代码

<Sequence>
    <SaySomething name="say_hello" message="Hello"/>
    <SaySomething name="say_goodbye" message="Goodbye"/>
</Sequence>

<!-- 两个SaySomething节点,但是可以通过不同的name属性进行区分 -->

当然，上面的SaySomething，我们并没有在xml中指明其节点类型，实际上在注册行为树节点时，需要明确指定每个节点的类型，以便行为树解析器能够正确识别和执行，然后在XML中使用其ID即可，无需显式地指明节点类型。比如对于一个Condition Node , ID=IsDoorOpen,然后有一个Decorator Node , ID=NotIsDoorOpen，那么可以这样写：

xml 复制代码

<root BTCPP_format="4">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <NotIsDoorOpen name="negate_door_check">
                <IsDoorOpen name="check_door"/>
            </NotIsDoorOpen>
        </Sequence>
    </BehaviorTree>
</root>

当然, 手动指明节点类型也是可以的:

xml 复制代码

 <root BTCPP_format="4" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
           <Action ID="SaySomething"   name="action_hello" message="Hello"/>
           <Action ID="OpenGripper"    name="open_gripper"/>
           <Action ID="ApproachObject" name="approach_object"/>
           <Action ID="CloseGripper"   name="close_gripper"/>
        </Sequence>
     </BehaviorTree>
 </root>

 <!-- 使用 节点类型 ID=""可以明确指定节点的类型 -->
 <!-- 上面等同于 -->
<root BTCPP_format="4">
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
           <SaySomething   name="action_hello" message="Hello"/>
           <OpenGripper    name="open_gripper"/>
           <ApproachObject name="approach_object"/>
           <CloseGripper   name="close_gripper"/>
        </Sequence>
     </BehaviorTree>
 </root>

对于需要传参的节点，可以在XML中通过Port标签传递参数，例如：

xml 复制代码

<SaySomething   name="action_hello" message="Hello"/>
<!-- 将Hello传递给SaySomething节点的message端口 -->

虽然可以直接在XML中只用ID,但是使用Groot可视化工具时，要求在xml中明确指定节点的类型，以便Groot能够正确解析和显示行为树的结构。下面是完整的示例：

xml 复制代码

 <root BTCPP_format="4" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
           <SaySomething   name="action_hello" message="Hello"/>
           <OpenGripper    name="open_gripper"/>
           <ApproachObject name="approach_object"/>
           <CloseGripper   name="close_gripper"/>
        </Sequence>
    </BehaviorTree>
	
	<!-- 给Groot提供节点类型信息 -->
    <TreeNodeModel>
        <Action ID="SaySomething">
            <input_port name="message" type="std::string" />
        </Action>
        <Action ID="OpenGripper"/>
        <Action ID="ApproachObject"/>
        <Action ID="CloseGripper"/>      
    </TreeNodeModel>
 </root>

当然可以在该树中添加子树或者引用外部的xml等,这里不再展开。

注意,对于每个xml文件，可以通过<BehaviorTree ID=xxx>去定义多个行为树。但是文件中仅有一个默认的执行树，我们可以使用<main_tree_to_execute="Trees_ID"去指定默认执行的行为树。当然对于仅有一棵树的情况，可以省略该属性。

xml 复制代码

<root BTCPP_format="4" main_tree_to_execute="MainTree">
    <BehaviorTree ID="MainTree">
        <!-- 主树的节点 -->
    </BehaviorTree>

    <BehaviorTree ID="OtherTree">
        <!-- 另一棵树的节点 -->
    </BehaviorTree>
</root>

Nav2中的行为树

在Nav2中，其使用了行为树这一架构去实现导航任务的行为控制以及故障时的恢复策略。由于BTCPPV4支持使用其开发库去自定义节点，因此你会在下面看到一些Nav2自定义的行为树节点，这些节点的一些细节和逻辑可能与上述介绍的节点同，但是依旧可以分为Control，Decorator，Condition以及Action四类。最后我们将通过一个完整的Nav2导航行为树示例贴在下方，并介绍其中的一些节点：

Nav2中的行为树节点

Nav2中的控制节点

PipelineSequence: 用于按顺序执行一系列子节点，类似于Sequence节点，但是有一点不一样。

Nav2文档对其的描述是：The PipelineSequence control node re-ticks previous children when a child returns RUNNING.

即

当PipelineSequence中的某个子节点返回RUNNING时，PipelineSequence会重新tick之前的子节点。

下面我们用Nav2给出的PipelineSequence节点示例来说明其行为，这里我将四个Tick的过程拼成了一张图，每一部分是当前Tick的状态。

可以用下面的xml文件来描述这棵树的结构：

xml 复制代码

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

我们可以分tick去观察PipelineSequence的行为，看看每个子节点在不同tick中的状态变化。

python 复制代码

Tick 1
MainTree 调用 PipelineSequence.tick()
    └─ tick A → RUNNING → 本轮结束
       B、C 未被 tick，保持 IDLE

PipelineSequence 返回 RUNNING


Tick 2
MainTree 调用 PipelineSequence.tick()
    ├─ tick A → SUCCESS → 继续
    └─ tick B → RUNNING → 本轮结束
       C 未被 tick，保持 IDLE

PipelineSequence 返回 RUNNING


Tick 3
MainTree 调用 PipelineSequence.tick()
    ├─ tick A → RUNNING → 之前已推进到 B，因此继续
    ├─ tick B → SUCCESS → 继续
    └─ tick C → RUNNING → 本轮结束

PipelineSequence 返回 RUNNING


Tick 4
MainTree 调用 PipelineSequence.tick()
    ├─ tick A → RUNNING → 之前已推进到 C，因此继续
    ├─ tick B → SUCCESS → 继续
    └─ tick C → SUCCESS → 遍历结束

PipelineSequence 停止所有子节点并重置内部记录
PipelineSequence 返回 SUCCESS

# 同时，当过程中某个子节点返回FAILURE时，PipelineSequence会停止所有子节点并Return FAILURE
# 当且仅当最后一个子节点返回SUCCESS时，PipelineSequence才会返回SUCCESS
# 其余情况下，PipelineSequence会返回RUNNING

注意，tick X，代表去执行X节点的tick()函数，至于该函数干了什么，返回什么，取决于节点的具体实现。也就是说即使一个节点被重新tick，其内部逻辑可能不会对该tick作出什么回应。

可以看出，PipelineSequence相较于之前的Sequence节点，最大的不同在于当某个子节点返回RUNNING时，它会重新tick之前的子节点，而后者仅仅会对当前正在执行的子节点进行tick。

典型的一个使用PipelineSequence的场景是，在导航过程中，Planner会在到达目标点之前不断重新规划路径，这样的话可以根据当前机器人的位姿与周围环境动态调整导航策略，使用当前最优的路径，而不是在一开始就固定了一条路径。

Recovery: 用于在行为树执行过程中处理恢复逻辑。其执行逻辑类似于Fallback节点，但是也是有一些不同的。一般该节点都会带有一个number_of_retries属性，用于指定重试次数。

我们截取Nav2文档中关于Recovery节点的示例来说明其行为：

其xml描述如下：

xml 复制代码

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

其中蓝色的部分表示Recovery节点的子节点，一般其左孩子为要执行的主逻辑，右孩子为恢复逻辑。这里的主逻辑是ComputePathToPose，即计算到达目标点的路径，而恢复逻辑是ClearLocalCostmap，即清除局部代价地图。

Nav2文档对其的描述：In the above example, let's assume ComputePathToPose fails. ClearLocalCostmap will be ticked in response, and return SUCCESS. Now that we have cleared the costmap, let's say the robot is correctly able to compute the path and ComputePathToPose now returns SUCCESS. Then, the parent RecoveryNode will also return SUCCESS and the BT will be complete.

即当主逻辑ComputePathToPose失败时，恢复逻辑ClearLocalCostmap会被执行，如果恢复逻辑成功，则会重试主逻辑。这个流程会一直重复，直到一下情况发生：

复制代码

1. 主逻辑成功，RecoveryNode返回SUCCESS。
2. 恢复逻辑失败，RecoveryNode返回FAILURE。
3. number_of_retries即重试次数用完，RecoveryNode返回FAILURE。

RoundRobin: 用于按循环顺序执行一系列子节点，类似于Sequence节点，但是会循环执行。

The RoundRobin control node ticks its children in a round robin fashion until a child returns SUCCESS, in which the parent node will also return SUCCESS. If all children return FAILURE so will the parent RoundRobin. round robin fashin: 轮询方法

即RoundRobin节点会按循环顺序执行其子节点，直到某个子节点返回SUCCESS，此时父节点也会返回SUCCESS。如果所有子节点都返回FAILURE，那么父节点也会返回FAILURE。同时，RoundRobin节点会记住当前执行到的子节点位置，在下一次tick时从该位置继续执行。

上述RoundRobin树的XML描述如下：

xml 复制代码

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

上面执行的RoundRobin节点的过程如下：

python 复制代码

Tick 1
MainTree 调用 RoundRobin.tick()
    └─ tick A → RUNNING → 本轮结束
       B、C 未被 tick

RoundRobin 返回 RUNNING
记住当前节点 A，下轮继续 tick A


Tick 2
MainTree 调用 RoundRobin.tick()
    ├─ tick A → FAILURE → 转向下一个节点 B
    └─ tick B → RUNNING → 本轮结束
       C 未被 tick

RoundRobin 返回 RUNNING
记住当前节点 B，下轮继续 tick B


Tick 3
MainTree 调用 RoundRobin.tick()
    └─ tick B → SUCCESS → 本轮结束
       A、C 未被 tick

RoundRobin 停止并重置所有子节点
RoundRobin 返回 SUCCESS
记住下一节点 C，再次被 tick 时从 C 开始


Tick 4
MainTree 调用 RoundRobin.tick()
    └─ tick C → RUNNING → 本轮结束
       A、B 未被 tick

RoundRobin 返回 RUNNING
记住当前节点 C，下轮继续 tick C


Tick 5
MainTree 调用 RoundRobin.tick()
    ├─ tick C → FAILURE → 到达末尾，循环回到 A
    └─ tick A → RUNNING → 本轮结束
       B 未被 tick

RoundRobin 返回 RUNNING
记住当前节点 A，下轮继续 tick A

# 当所有的节点都返回FAILURE时，RoundRobin节点也会返回FAILURE。
# 否则，RoundRobin节点会环形的执行其子节点，直到某个子节点返回SUCCESS。

可以看到RoundRobin节点会循环执行其中的所有可尝试的子节点，直到某个子节点返回SUCCESS，或者所有子节点都返回FAILURE。在Nav2中，这种行为可以用于实现多种策略，如在开头的恢复任务中，该节点用于尝试不同的恢复动作，当一个动作成功后，Return SUCCESS，同时导航任务重启，若重启失败，则会继续尝试下一个恢复动作。

使用xml描述Nav2中的树结构

在Nav2的示例中，选用了navigation2/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml所描述的这棵行为树作为示例，其结构如下：

其描述如下

xml 复制代码

<!--
  This Behavior Tree replans the global path periodically at 1 Hz and it also has
  recovery actions specific to planning / control as well as general system issues.
  This will be continuous if a kinematically valid planner is selected.
-->
<root BTCPP_format="4" main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
            <Sequence>
              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
            </Sequence>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
          <Sequence>
            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
          </Sequence>
        </RecoveryNode>
      </PipelineSequence>
      <Sequence>
        <Fallback>
          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
        </Fallback>
        <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" error_code_id="{spin_error_code}"/>
            <Wait wait_duration="5.0"/>
            <BackUp backup_dist="0.30" backup_speed="0.15" error_code_id="{backup_code_id}"/>
          </RoundRobin>
        </ReactiveFallback>
      </Sequence>
    </RecoveryNode>
  </BehaviorTree>
</root>

不难看出，Nav2的树描述使用的就是BTCPPV4文档中所描述的语法和结构。同时使用了大量的自定义节点和端口(Port)去传递静态或者动态的参数。

Nav2导航行为树拆分

上面的那棵行为树可以拆解为两个子树(Subtree),其中一棵作用是导航(navigation)，而另一颗的作用是恢复(recovery)。而这两棵树都连接在一个RecoveryNode上，该节点的尝试次数为6(number_of_retries="6")。

在导航子树中，其工作流程可以概括为以下几个步骤：

控制器(Controller)与规划器(Planner)选择
路径规划(ComputePathToPose)
路径跟随(FollowPath)

首先，导航子树会根据传入的参数选择对应的控制器(Controller)和规划器(Planner)，该控制器和规划器将用于后续的路径跟随与路径规划。

接下来，导航子树会调用ComputePathToPose节点进行路径规划，生成一条从当前位姿到目标位姿的路径。当然，在该节点之前，会经过一个名字为RateController的Decorator，这个节点用于限制路径的更新频率，当前设置为1Hz，也就是当一次路径规划完成后，至少间隔1秒才会进行下一次路径规划。

图中的节点功能介绍大致如下：

ComputePathToPose: Action Node

goal:输入端口，接收目标位姿作为输入。
planner_id:输入端口，指定用哪一个规划器进行路径规划。
path:输出端口，输出计算得到的路径。
error_code_id:输出端口，输出路径规划过程中可能出现的错误码。

当该节点返回Failure时，表示路径规划失败。回到其父节点的RecoveryNode，触发恢复机制，接着去执行其左子树。其左子树是由一个Sequence Node组成的，别忘了在开始介绍行为树时介绍过点击这里，Sequence Node会按顺序执行其子节点，只有当所有子节点都返回Success时，该节点才会返回Success。

因此该节点先去执行其左孩子节点WouldAPlannerRecoveryHelp，这是一个Condition Node，会通过路径规划的结果所给的error_code_id来判断这次的错误是不是值得规划器进行恢复的错误类型。

若返回Success，则表示规划器可以进行恢复，接着执行其右孩子节点ClearEntireCostmap，这个Action Node的作用是清除整个代价地图，减少之前可能错误的代价信息对后续路径规划的影响。当然该节点的name标签为ClearGlobalCostmap-Context，也就是这里清除的是全局代价地图的上下文信息。

为什么要去清除全局代价地图呢？我们看如下情况：

移动时，有移动的障碍物出现，如有人从机器人面前走过，此时人已经不再遮挡，但是全局代价地图中仍然保留着该障碍物的信息。

因此此时去清理全局代价地图，然后在通过路径规划重新生成路径，就可以避免之前错误的代价信息对后续路径规划的影响。

当ComputePathToPose返回Suceess后，则会执行下一个节点FollowPath，该节点的作用是沿着规划好的路径进行跟随，直到到达目标位姿。注意，这也是一个Recovery Node, 意味着如果在路径跟随过程中出现问题，也会触发恢复机制。

其主体结构与前面的ComputePathToPose节点一模一样，其左孩子节点是一个Action Node，去执行路径跟随操作，直到到达目标位姿。

path: 输入端口，接收规划好的路径作为输入。
controller_id: 输入端口，指定用哪一个控制器进行路径跟随。
error_code_id: 输出端口，输出路径跟随过程中可能出现的错误码。

当FollowPath节点返回Failure时，表示路径跟随失败，同样会触发恢复机制，回到其父节点的RecoveryNode，执行恢复操作。这部分依旧是先看一看其左孩子节点WouldAControllerRecoveryHelp，判断控制器是否可以进行恢复，若返回的是Success，则表示控制器可以进行恢复，接着执行其右孩子节点ClearEntireCostmap，清除整个代价地图(这里是ClearLocalCostmap-Context即清理局部代价地图)，减少之前可能错误的代价信息对后续路径规划的影响。

恢复子树(Recovery Subtree)

根据上面的描述，我们可以将导航子树的主要功能简化为下面的树形结构：

当上述的ComputePathToPose以及FollowPath节点其中一个返回Failure时，此时有Pipeline Node的特性点这里查看可知，整个导航子树会返回Failure，此时作为root的RecoveryNode会触发恢复机制，去执行其右子树，也就是恢复子树。

该子树也是主要由两部分完成，且两部分由一个Sequence Node串联起来，确保在恢复过程中各个步骤按顺序执行。

其第一部分通过一个Fallback Node去执行判断操作，这个节点连着两个子节点，分别是WouldAPlannerRecoveryHelp和WouldAControllerRecoveryHelp，这两个Condition Node我们在前面的导航子树中已经介绍过，它们的作用是判断规划器和控制器是否可以进行恢复。

而该Fallback Node的作用是，只要其子节点中有一个返回Success，它就会返回Success，也就是说，此时判断是只要规划器或控制器中有一个可以进行恢复，整个恢复子树就继续进行后续恢复操作。此时转入RecoveryFallback这个节点进行后续的恢复动作。

这里解释一下ReactiveFallback节点的作用。

对于Fallback节点，他会按顺序执行其子节点，当子节点返回Running时，此时Fallback节点也会返回Running，继续等待子节点完成。在下一轮tick时，Fallback节点会继续去tick该子节点，而不会去看其他子节点。

对于 ReactiveFallback，假设它有两个子节点：A（条件节点）和 B（动作节点）。当 A 返回 FAILURE、B 返回 RUNNING 时，父节点也返回 RUNNING。下一轮 tick 会先重新检查 A：如果 A 仍返回 FAILURE，则继续 tick B；如果 A 返回 SUCCESS，则停止 B，父节点返回 SUCCESS。

也就是说，ReactiveFallback节点会在每一轮tick时重新检查其条件子节点，只有在条件子节点返回FAILURE时，才会继续执行动作子节点。这种机制使得恢复过程更加灵活，能够根据条件的变化动态调整恢复动作。

我们回到上面的恢复子树，ReactiveFallback节点的左孩子节点为GoalUpdated，这是一个Condition Node，当检查该节点时，若发现当前要导航到的目标已经更新，则会返回Success，否则返回Failure。而其右孩子节点则是具体的恢复动作。

因此可以将主要的恢复逻辑简化为下面的树结构：

此时，每次执行RecoveryFallback时，都会先检查GoalUpdated节点的条件：

复制代码

1. 若当前导航目标已经更新(GoalUpdated返回Success)，那么无需再去对当前的目标进行恢复操作，直接进入下一轮的导航规划即可。此时不论RecoveryActions节点是否还在运行，都会向父节点返回Success。

2. 若当前导航目标尚未更新(GoalUpdated返回Failure)，则会继续执行`RecoveryActions`节点，进行相应的恢复操作。

因此这里采用ReactiveFallback节点来实现恢复逻辑，使得在每一轮tick时都能够根据目标是否更新来动态调整恢复动作，避免动态的目标更新后仍然执行不必要的恢复操作。

下面再来简单的说明恢复动作节点RecoveryActions

实际上该节点所执行的工作即为顺序遍历其所有的子节点，依次执行每个恢复动作。按从左到右的顺序，恢复动作依次为：

清理代价地图，包括全局代价地图和局部代价地图

转圈，spin_dist=1.57 = 逆时针(正向)旋转弧度1.57,即角度约90度

等待，wait_duration=1.0，即等待1秒

后退，backup_dist=0.3，backup_speed=0.15，即后退0.3米，速度为0.15米/秒

这些Action Node经由一个RoundRobin节点串联起来。该节点在这里有讲过。RoundRobin节点会按顺序依次执行其子节点。

nav2文档中提到：Upon SUCCESS of any of the four children of the parent RoundRobin, the robot will attempt to renavigate in the Navigation subtree. If this renavigation was not successful, the next child of the RoundRobin will be ticked.

即

父节点 RoundRobin 的四个子节点中，只要有任意一个返回 SUCCESS，机器人就会重新进入导航子树，尝试再次导航。如果这次导航仍然失败，则会 tick RoundRobin 的下一个子节点，尝试下一种恢复策略。

当然，当其中一个子节点返回 FAILURE 时，RoundRobin 节点会继续 tick 下一个子节点，即尝试下一个恢复动作。当然，其结果有下面的几种：

若某个子节点返回 SUCCESS，则机器人会尝试重新导航，若重新导航失败，则会继续尝试下一个恢复动作。
若某个子节点返回 FAILURE，则 RoundRobin 会继续尝试下一个恢复动作。
若所有子节点都返回 FAILURE，则 RoundRobin 节点会返回 FAILURE，表示所有恢复动作都尝试失败。此时整个恢复子树也会返回 FAILURE，这将会返回到最初的root，即

此时根据Recovery Node的逻辑，该root也会返回 FAILURE，整个行为树执行失败。

正如文档中写的：If the BackUp action was not sufficient enough to allow the robot to become un-stuck, the above logic will go on indefinitely until the number_of_retries in the parent of the Navigate subtree and Recovery subtree is exceeded, or if all the system-wide recoveries in the Recovery subtree return FAILURE (this is unlikely, and likely points to some other system failure).

即

如果（包括前面的三个动作以及）后退（BackUp）动作仍不足以让机器人脱困，上述逻辑就会持续循环，直到出现以下任一情况：

包含导航子树和恢复子树的父节点达到 number_of_retries 所设置的重试上限。
恢复子树中的所有系统级恢复策略都返回 FAILURE。这种情况不太常见，通常意味着系统存在其他故障。

情况	结果
导航子树返回 FAILURE，且 `retry_count_ >= number_of_retries_`	重试机会已用完，整体失败
恢复子树返回 FAILURE	立即失败，即使还有重试机会
恢复子树返回 SUCCESS	恢复结束，尝试重新导航

结束^-

参考

BTCPPV4 Document

Nav2 Document

Nav2 SourceCode

行为树与Nav2中的行为树

行为树

行为树概念

节点(Node)类型与功能

常见控制节点

常见装饰器节点

简单的行为树示例

使用xml描述树结构

Nav2中的行为树

Nav2中的行为树节点

Nav2中的控制节点

使用xml描述Nav2中的树结构

Nav2导航行为树拆分

导航子树(Navigation Subtree)

恢复子树(Recovery Subtree)

参考