组合实际任务

把已有节点组合成移动机器人任务

对应 BehaviorTree.CPP 3.8 官方文档：Using SubTrees、Port Remapping 和 Use multiple XML files。实现细节参考 3.8.7 的 xml_parsing.cpp、blackboard.h 和 retry_node.cpp。

前面的实践已经分别给出了端口、Blackboard 和持续动作。 本篇把它们组合成一个完整主题：机器人先确保电量足够，再依次前往取货点和放货点；每段导航允许有限次尝试，成功后报告结果，尝试耗尽后报告任务失败。

本篇的导航仍用回调计数模拟，充电简化为一次电量更新，全部运行都不需要连接机器人。 先完成前面几篇的实践，再运行本篇，便于把注意力集中在任务组织与子树边界上。

MainTree
└── Sequence：一次运输任务
    ├── Fallback：确保电量(如果电量不OK，则充电)
    │   ├── BatteryOK
    │   └── ChargeBattery
    └── Fallback：执行任务，或报告失败（如果任务执行失败，则报Failure）
        ├── Sequence
        │   ├── SubTree：NavigateLeg，前往取货点
        │   ├── SubTree：NavigateLeg，前往放货点
        │   └── ReportMission
        └── ForceFailure
            └── ReportFailure

NavigateLeg：每次调用都创建自己的节点和局部 Blackboard
└── RetryUntilSuccessful：最多尝试两次
    └── NavigateToGoal：跨多次 tick 检查本次导航

电量检查失败时，Fallback 继续执行模拟充电；充电成功后，外层 Sequence 才进入导航。 导航序列返回 RUNNING 时，任务 Fallback 也返回 RUNNING，不会提前执行失败报告。

复用子树时明确数据边界

两段导航执行规则相同，只是目标和结果条目不同，因此可以共用 NavigateLeg 这份 XML 定义。 它在每次调用时创建不同的节点对象和局部 Blackboard，内部的 target、reached 名称可以相同，再分别连接到主树的不同条目。

主树 Blackboard                         第一次 NavigateLeg 的局部 Blackboard
pickup_goal = loading_area   ──映射──→   target
pickup_reached               ←─映射──   reached

主树 Blackboard                         第二次 NavigateLeg 的局部 Blackboard
dropoff_goal = unloading_area ─映射──→   target
dropoff_reached              ←─映射──   reached

本篇使用官方 3.8 端口映射教程中的普通 SubTree 写法：

<SubTree ID="NavigateLeg" target="pickup_goal" reached="pickup_reached"/>

这里属性名 target、reached 是子树局部 Blackboard 的键，属性值 pickup_goal、pickup_reached 是父 Blackboard 的键名。 它们建立映射关系，子树对 reached 的写入会反映到主树的 pickup_reached。

不要把本篇 SubTree 的映射值改成 {pickup_goal}。 3.8.7 的普通 SubTree 分支直接把属性值当作父键名，不会替你去掉花括号，之后就可能找不到正确输入。 源码中还提供了使用不同映射语义的 SubTreePlus，本篇统一使用普通 SubTree，按表中的边界规则填写。

子树边界的键名也不等于 C++ 叶节点的端口名。 本例经过的是：goal 端口 → 局部 target → 父树 pickup_goal，每一层分别声明自己的接口。

建立组合任务工程

本篇直接复制整个 bt_running 文件夹，再把副本重命名为 bt_mission，保留原来的持续任务工程供后续对照。

在系统文件管理器中完成下面几步：

1. 打开 bt_running 所在的父目录，复制并粘贴整个文件夹，将副本重命名为 bt_mission，让两个工程位于同一层目录。
2. 在新副本 bt_mission 中，删除复制过来的 build 和 install 文件夹（如果存在）。其中的构建缓存和安装结果属于旧工程，新工程稍后会重新生成。
3. 在 VSCode 中选择 文件 → 新建窗口，再选择 文件 → 打开文件夹，打开新的 bt_mission。

框架继续来自 CMake工程模板，保留 C++20、CMake 3.22、预设格式版本 3，以及优先查找 BehaviorTree.CPP 3.8.7 的依赖配置。 Ubuntu 22.04 的 CMake 3.22.1 可以继续使用。复用 Humble 安装包时，VSCode 的配置、运行和调试进程仍需继承 ROS 2 环境。

在顶层 CMakeLists.txt 中，将项目声明改为：

project(bt_mission VERSION 1.0.0 LANGUAGES C CXX)

src/main.cpp 和 trees/mission.xml 已经随工程复制过来，稍后用本篇内容替换。 在左侧资源管理器的 trees 目录中新建 navigation.xml，用于保存导航子树。参与本篇构建的目录为：

bt_mission/
├── CMakeLists.txt
├── CMakePresets.json
├── cmake/
│   ├── ProjectOptions.cmake
│   └── BehaviorTree.cmake
├── src/
│   ├── CMakeLists.txt
│   └── main.cpp
└── trees/
    ├── mission.xml          任务入口，引用导航子树
    └── navigation.xml       可复用的 NavigateLeg 定义

BT_XML_PATH 仍指向 trees/mission.xml，不需要因为增加了第二个 XML 而修改 CMake。

实现任务所需的节点

导航节点沿用上一篇的生命周期，在此基础上增加“前几次尝试失败”的模拟参数，以及成功后写出到达目标的输出端口。

用以下完整程序替换 src/main.cpp 的原有内容，再保存。四个节点类和一个简单动作都在这个文件中，方便先把组合任务跑通。

#include "behaviortree_cpp_v3/bt_factory.h"

#include <chrono>
#include <cmath>
#include <iostream>
#include <stdexcept>
#include <string>

class BatteryOK : public BT::ConditionNode
{
public:
    BatteryOK(const std::string& name, const BT::NodeConfiguration& config)
        : BT::ConditionNode(name, config)
    {}

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<double>("level")};
    }

    BT::NodeStatus tick() override
    {
        auto level = getInput<double>("level");
        if (!level)
        {
            throw BT::RuntimeError("Cannot read input [level]: ", level.error());
        }
        if (!std::isfinite(level.value()) || level.value() < 0.0 || level.value() > 100.0)
        {
            throw std::runtime_error("battery level must be between 0 and 100");
        }
        const auto result = level.value() >= 30.0
                                ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
        std::cout << "[BatteryOK] level=" << level.value()
                  << " -> " << BT::toStr(result) << '\n';
        return result;
    }
};

class ChargeBattery : public BT::SyncActionNode
{
public:
    ChargeBattery(const std::string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config)
    {}

    static BT::PortsList providedPorts()
    {
        return {BT::OutputPort<double>("level")};
    }

    BT::NodeStatus tick() override
    {
        // 本例把充电简化为一次数据更新，真实充电应实现为持续任务。
        auto written = setOutput("level", 80.0);
        if (!written)
        {
            throw BT::RuntimeError("Cannot write output [level]: ", written.error());
        }
        std::cout << "[ChargeBattery] level=80\n";
        return BT::NodeStatus::SUCCESS;
    }
};

class NavigateToGoal : public BT::StatefulActionNode
{
public:
    NavigateToGoal(const std::string& name, const BT::NodeConfiguration& config)
        : BT::StatefulActionNode(name, config)
    {}

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<std::string>("goal"),
                BT::InputPort<int>("ticks", 2, "每次导航尝试的检查次数"),
                BT::InputPort<int>("fail_attempts", 1, "模拟前几次尝试失败"),
                BT::OutputPort<std::string>("reached_goal")};
    }

    BT::NodeStatus onStart() override
    {
        auto goal = getInput<std::string>("goal");
        auto ticks = getInput<int>("ticks");
        auto failures = getInput<int>("fail_attempts");
        if (!goal)
        {
            throw BT::RuntimeError("Cannot read input [goal]: ", goal.error());
        }
        if (!ticks)
        {
            throw BT::RuntimeError("Cannot read input [ticks]: ", ticks.error());
        }
        if (!failures)
        {
            throw BT::RuntimeError("Cannot read input [fail_attempts]: ", failures.error());
        }
        if (goal.value().empty() || ticks.value() < 1 || failures.value() < 0)
        {
            throw std::runtime_error("invalid navigation simulation parameters");
        }

        goal_ = goal.value();
        required_ticks_ = ticks.value();
        failed_attempts_ = failures.value();
        completed_ticks_ = 0;
        // 每个节点实例分别保留尝试次数，用于模拟先失败、后成功。
        ++attempt_;
        std::cout << "[NavigateToGoal] start goal=" << goal_
                  << ", attempt=" << attempt_ << '\n';
        return BT::NodeStatus::RUNNING;
    }

    BT::NodeStatus onRunning() override
    {
        if (++completed_ticks_ < required_ticks_)
        {
            return BT::NodeStatus::RUNNING;
        }
        if (attempt_ <= failed_attempts_)
        {
            std::cout << "[NavigateToGoal] failed goal=" << goal_
                      << ", attempt=" << attempt_ << '\n';
            return BT::NodeStatus::FAILURE;
        }

        auto written = setOutput("reached_goal", goal_);
        if (!written)
        {
            throw BT::RuntimeError("Cannot write output [reached_goal]: ", written.error());
        }
        std::cout << "[NavigateToGoal] arrived goal=" << goal_
                  << ", attempt=" << attempt_ << '\n';
        return BT::NodeStatus::SUCCESS;
    }

    void onHalted() override
    {
        std::cout << "[NavigateToGoal] halted goal=" << goal_ << '\n';
        completed_ticks_ = 0;
    }

private:
    std::string goal_;
    int required_ticks_ = 0;
    int completed_ticks_ = 0;
    int failed_attempts_ = 0;
    int attempt_ = 0;
};

class ReportMission : public BT::SyncActionNode
{
public:
    ReportMission(const std::string& name, const BT::NodeConfiguration& config)
        : BT::SyncActionNode(name, config)
    {}

    static BT::PortsList providedPorts()
    {
        return {BT::InputPort<std::string>("pickup"),
                BT::InputPort<std::string>("dropoff")};
    }

    BT::NodeStatus tick() override
    {
        auto pickup = getInput<std::string>("pickup");
        auto dropoff = getInput<std::string>("dropoff");
        if (!pickup)
        {
            throw BT::RuntimeError("Cannot read input [pickup]: ", pickup.error());
        }
        if (!dropoff)
        {
            throw BT::RuntimeError("Cannot read input [dropoff]: ", dropoff.error());
        }
        std::cout << "[ReportMission] pickup=" << pickup.value()
                  << ", dropoff=" << dropoff.value() << '\n';
        return BT::NodeStatus::SUCCESS;
    }
};

BT::NodeStatus ReportFailure(BT::TreeNode&)
{
    std::cout << "[ReportFailure] transport mission failed\n";
    return BT::NodeStatus::SUCCESS;
}

int main()
{
    try
    {
        BT::BehaviorTreeFactory factory;
        factory.registerNodeType<BatteryOK>("BatteryOK");
        factory.registerNodeType<ChargeBattery>("ChargeBattery");
        factory.registerNodeType<NavigateToGoal>("NavigateToGoal");
        factory.registerNodeType<ReportMission>("ReportMission");
        factory.registerSimpleAction("ReportFailure", ReportFailure);

        auto blackboard = BT::Blackboard::create();
        blackboard->set("battery", 20.0);
        blackboard->set("pickup_goal", std::string("loading_area"));
        blackboard->set("dropoff_goal", std::string("unloading_area"));
        auto tree = factory.createTreeFromFile(BT_XML_PATH, blackboard);

        BT::NodeStatus result = tree.tickRoot();
        while (result == BT::NodeStatus::RUNNING)
        {
            tree.sleep(std::chrono::milliseconds(100));
            result = tree.tickRoot();
        }
        std::cout << "Tree result: " << BT::toStr(result) << '\n';
        if (result == BT::NodeStatus::SUCCESS)
        {
            std::cout << "[main] pickup_reached="
                      << blackboard->get<std::string>("pickup_reached") << '\n';
            std::cout << "[main] dropoff_reached="
                      << blackboard->get<std::string>("dropoff_reached") << '\n';
        }
        return result == BT::NodeStatus::SUCCESS ? 0 : 1;
    }
    catch (const std::exception& error)
    {
        std::cerr << "Error: " << error.what() << '\n';
        return 2;
    }
}

ticks 和 fail_attempts 都是本例的模拟参数。ticks="2" 表示一次导航在两次 onRunning() 检查后给出结果；fail_attempts="1" 表示同一个节点实例的第一次尝试失败，后续尝试成功。节点的 attempt_ 从 0 开始，在每次 onStart() 中递增。

attempt_ 是教学模拟器自己的计数，和 Retry 装饰节点内部的重试计数不是同一个变量。这里故意保留它，才能在重启导航后模拟“第一次失败、第二次成功”。每次 Launch 都会创建新的树；如果实际程序复用同一棵树处理多个任务，应明确什么时候重置业务成员和结果条目。

对照本例中的 API 参数

本篇主要组合前面已经使用过的接口。注册和端口接口的原型见 第一个完整程序，外部 Blackboard 的读写见 节点之间传数据，回调与驱动循环见 持续任务与中止。对照本例时，重点看每个实参对应哪一层名称：

setOutput() 的 reached_goal 是节点端口名，get() 的 pickup_reached 是主程序要读取的 Blackboard 键，两者由后面的 XML 映射连接。ReportFailure() 回调返回 SUCCESS 表示报告动作完成；整项运输任务的失败结果由 XML 中包裹它的 ForceFailure 保留，不能把回调返回值与注册函数返回值混为一谈。

用两个 XML 文件组织任务

trees/mission.xml

用下面的 XML 替换 trees/mission.xml 的原有内容，再保存。

<root main_tree_to_execute="MainTree">
    <include path="navigation.xml"/>

    <BehaviorTree ID="MainTree">
        <Sequence name="transport_job">
            <Fallback name="ensure_battery">
                <BatteryOK level="{battery}"/>
                <ChargeBattery level="{battery}"/>
            </Fallback>

            <Fallback name="run_or_report_failure">
                <Sequence>
                    <SubTree ID="NavigateLeg"
                             target="pickup_goal" reached="pickup_reached"/>
                    <SubTree ID="NavigateLeg"
                             target="dropoff_goal" reached="dropoff_reached"/>
                    <ReportMission pickup="{pickup_reached}"
                                   dropoff="{dropoff_reached}"/>
                </Sequence>
                <ForceFailure>
                    <ReportFailure/>
                </ForceFailure>
            </Fallback>
        </Sequence>
    </BehaviorTree>
</root>

主树中两次 SubTree 使用相同的 ID="NavigateLeg"，但分别映射到取货点和放货点的父键。NavigateLeg 是 XML 中定义的行为树名称，NavigateToGoal 才是注册到工厂的 C++ 叶节点名称。

trees/navigation.xml

将下面的内容保存到新建的 trees/navigation.xml。

<root>
    <BehaviorTree ID="NavigateLeg">
        <RetryUntilSuccessful num_attempts="2">
            <NavigateToGoal goal="{target}" reached_goal="{reached}"
                            ticks="2" fail_attempts="1"/>
        </RetryUntilSuccessful>
    </BehaviorTree>
</root>

<include path="navigation.xml"/> 会在加载主 XML 时引入子树定义，路径相对于包含它的 mission.xml 所在目录。主程序通过 BT_XML_PATH 的完整路径加载入口，所以从不同工作目录启动，仍能找到同目录的 navigation.xml。

C++ 中已经注册了叶节点；两个 XML 负责定义和连接任务。工厂加载入口文件时会处理 include 和 BehaviorTree ID，因此不需要再为 NavigateLeg 编写一个 C++ 节点类。

截图预留：主树和导航子树。 展示两个 XML 文件，以及两处 SubTree 映射到不同父键的位置。

在 VSCode 中运行完整任务

保存所有文件，选择 Linux Debug，执行 Configure → Build，将运行目标设为 bt_mission，再点击 Launch。完整输出为：

[BatteryOK] level=20 -> FAILURE
[ChargeBattery] level=80
[NavigateToGoal] start goal=loading_area, attempt=1
[NavigateToGoal] failed goal=loading_area, attempt=1
[NavigateToGoal] start goal=loading_area, attempt=2
[NavigateToGoal] arrived goal=loading_area, attempt=2
[NavigateToGoal] start goal=unloading_area, attempt=1
[NavigateToGoal] failed goal=unloading_area, attempt=1
[NavigateToGoal] start goal=unloading_area, attempt=2
[NavigateToGoal] arrived goal=unloading_area, attempt=2
[ReportMission] pickup=loading_area, dropoff=unloading_area
Tree result: SUCCESS
[main] pickup_reached=loading_area
[main] dropoff_reached=unloading_area

截图预留：组合任务正常完成。 展示从电量不足、模拟充电、两段导航分别重试，到主程序读取结果的完整输出。

初始电量为 20，因此先检查失败，再由 ChargeBattery 更新为 80。两段导航分别经历第一次失败、第二次成功，最后 ReportMission 和主程序都能读到两个到达结果。

电量检查是这次任务的启动准备。进入导航后，外层普通 Sequence 会从正在执行的位置继续，电量检查不会每次都重跑。如果任务要求导航期间持续检查某个条件，可以沿用上一篇的 ReactiveSequence 方式组织检查层。

有限重试与失败报告各自负责一层

num_attempts="2" 表示最多两次总尝试，不是首次尝试之后再额外重试两次。导航返回 RUNNING 时，装饰节点继续等待这一次尝试；只有导航返回 FAILURE，才会记录一次失败并决定是否重新开始。

在 3.8.7 中，Retry 收到失败后可以在同一次根 tick 内重置子节点并立即开始下一次尝试。本例新的 onStart() 返回 RUNNING 后，控制权就会回到主循环。因此日志中一次失败后可以紧接着出现下一次 start，不必假定它们一定属于不同的根 tick。

将 navigation.xml 中的 num_attempts="2" 改为 num_attempts="1"，只保存 XML，再点击 Launch。此时第一次失败已经耗尽次数，输出为：

[BatteryOK] level=20 -> FAILURE
[ChargeBattery] level=80
[NavigateToGoal] start goal=loading_area, attempt=1
[NavigateToGoal] failed goal=loading_area, attempt=1
[ReportFailure] transport mission failed
Tree result: FAILURE

没有出现放货点导航，也没有执行 ReportMission，因为第一段子树失败后，导航 Sequence 立即结束。

截图预留：重试次数耗尽。 展示 num_attempts="1"、失败报告以及最终的 FAILURE。

失败报告分支外面包了一层 ForceFailure：

ForceFailure
└── ReportFailure   报告动作执行完毕，返回 SUCCESS
         ↓
该分支向任务 Fallback 返回 FAILURE

ReportFailure 返回成功表示日志报告已经执行；ForceFailure 保留的是运输任务失败的结果。这样完成失败处理后，整棵树仍返回 FAILURE，主程序也不会进入成功结果的读取分支。

分别修改电量、重试和映射来对照

每次对照只改一项，完成后恢复原值，再进行下一项。修改 C++ 后先 Build → Launch；只改 XML 时保存后直接 Launch。

最后两种是数据配置错误，主程序打印 Error: 并返回 2。它们不同于导航尝试耗尽时的任务 FAILURE，后者正常返回任务失败码 1。

截图预留：子树映射错误。 展示其中一种错误映射和终端中的读取错误，便于和正常的任务失败区分。

如果遗漏输出映射，子树内部仍然可以完成一次导航；真正缺失的是通往主树结果条目的连接。局部键名字相同，不代表所有子树自动共享同一个存储位置。

在断点中确认两次调用是不同的节点实例

恢复主示例的电量与 XML，先 Build。在 NavigateToGoal::onStart() 中 ++attempt_ 之后的打印语句处设置断点，再点击 Debug。

每次暂停时查看 goal_、attempt_，并比较 this 指针。取货点的两次启动会在同一个节点对象上出现，尝试次数依次为 1、2；进入放货点时，this 指针不同，那个节点自己的尝试次数也从 1 开始。

这个过程对应的是“同一份子树定义，两次独立实例化”。复用的是结构和实现，任务成员与未映射的局部 Blackboard 条目分别属于各自实例。

截图预留：不同子树实例中的导航节点。 展示取货点和放货点暂停时的 this、goal_、attempt_。

本篇完整工程，以及电量充足、首次成功、重试耗尽、错误输入映射和遗漏输出映射等对照，已在 Ubuntu 22.04 的 ysu_ros_humble 容器中，使用 C++20、CMake 3.22.1、GCC 11.4、BehaviorTree.CPP 3.8.7 编译运行验证。入口 XML 从不同工作目录加载也已验证。测试使用临时工程和 Ninja，保留了容器内已有练习工程。

当前学习进度

到这里，教程已经给出了从最小程序到组合任务的完整实践路径：

基础编程实践的文档到此结束。