概论

Behavior Tree

库

主要用于机器人和游戏AI，代替有限元状态机

特性：

  • 可以执行异步动作
  • 可以在运行时创建树
  • 可以把自定义的树转换成插件链接，在运行时动态加载
  • 包含日志/优化架构可以可视化，记录回放分析状态转移

什么是行为树？

行为树(BT)是一种结构在不同自动化终端任务之间转换，比如机器人或者游戏的虚拟实体

BT相对于FSM的优点：

  • 他们本质上是分层的
  • 他们的图形表示语言意义
  • 他们更具表现力

为什么需要行为树？

用于Component Based Software Engineering

好的软件架构有以下特性：

  • 模块化
  • 可重用的组件
  • 可压缩性
  • 好的关注点分离

基础学习

介绍BT

tick()触发

当TreeNode触发，返回NodeStatus

  • SUCCESS
  • FAILURE
  • RUNNING

RUNNING表示异步执行

节点的种类

控制节点(ControlNodes)，拥有1-N个子节点，接收到tick之后，tick会被传播到1个或多个子节点

装饰节点(DecoratorNodes)，和控制节点相似，但是只有一个子节点

动作节点(ActionNodes)，是树叶节点，没有子节点，用户需要通过实现自己的动作节点来执行实际的任务

条件节点(ConditionNodes)，和动作节点相等，但是总是原子的和同步的，不能返回RUNNING状态，不能改变系统状态

Nodes

序列例子

BT使用最多的SequenceNode

控制节点的子节点都是有序的，从左到右执行

简单来说：

  • 子节点返回成功，tick下一个
  • 子节点返回失败，没有子节点将会tick，序列返回失败
  • 如果所有的子节点返回成功，序列返回成功

Decorator

目的：

  • 转换从子节点收到的结果
  • 停止子节点的执行
  • 重复tick子节点，取决于装饰节点的类型
Decorator

节点Inverter装饰器，把返回的值反转

节点Retry重复N次子节点

以上节点组合形式在门开了后会取消所有动作

FallbackNodes

又名为被理解为Selectors，是一些节点，顾名思义，可以表达回退策略，即如果一个子节点返回FAILURE，接下来该怎么做。

按照顺序触发：

  • 如果子节点返回失败，tick下个节点
  • 如果返回成功，不再tick节点，Fallback返回成功
fallback

教程

创建树

代码

#include <iostream>
#include <behaviortree_cpp_v3/bt_factory.h>

class ApproachGripper : public BT::SyncActionNode
{
public:
    explicit ApproachGripper(const std::string &name) : BT::SyncActionNode(name, {})
    {}

    BT::NodeStatus tick() override
    {
        std::cout << "ApproachObject: " << name() << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

bool batteryOK = false;

BT::NodeStatus CheckBattery()
{
    if (batteryOK) {
        std::cout << "[ Battery: OK ]" << std::endl;
        return BT::NodeStatus::SUCCESS;
    } else {
        std::cout << "[ Battery: BAD ]" << std::endl;
        return BT::NodeStatus::FAILURE;
    }
}

BT::NodeStatus Charge()
{
    std::cout << "[ Charge Battery ]" << std::endl;
    return BT::NodeStatus::SUCCESS;
}

class GripperInterface
{
public:
    GripperInterface() : _open(true)
    {}

    BT::NodeStatus open()
    {
        _open = true;
        std::cout << "GripperInterface::open" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }

    BT::NodeStatus close()
    {
        _open = false;
        std::cout << "GripperInterface::close" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }

private:
    bool _open;
};

int main()
{
    BT::BehaviorTreeFactory factory;
    factory.registerNodeType<ApproachGripper>("ApproachGripper");
    factory.registerSimpleCondition("CheckBattery",
                                    [](BT::TreeNode &node) { return CheckBattery(); });
    factory.registerSimpleCondition("Charge",
                                    [](BT::TreeNode &node) { return Charge(); });
    GripperInterface gripper;
    factory.registerSimpleAction("OpenGripper",
                                 [&gripper](BT::TreeNode &node) { return gripper.open(); });
    factory.registerSimpleAction("CloseGripper",
                                 [&gripper](BT::TreeNode &node) { return gripper.close(); });

    auto tree = factory.createTreeFromFile("../my_tree1.xml");
    tree.tickRoot();
}
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- ////////// -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Fallback>
                <Condition ID="CheckBattery"/>
                <Condition ID="Charge"/>
            </Fallback>
            <Action ID="OpenGripper"/>
            <Action ID="ApproachGripper"/>
            <Action ID="CloseGripper"/>
        </Sequence>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <Action ID="ApproachGripper"/>
        <Condition ID="Charge"/>
        <Condition ID="CheckBattery"/>
        <Action ID="CloseGripper"/>
        <Action ID="OpenGripper"/>
    </TreeNodesModel>
    <!-- ////////// -->
</root>

执行后结果如下

[ Battery: BAD ]
[ Charge Battery ]
GripperInterface::open
ApproachObject: ApproachGripper
GripperInterface::close

带<TreeNodesModel>节点才能正常用Groot读取

输入和输出接口

行为树支持用接口(ports)传输数据流，使用简单而且类型安全

输入接口

正确的输入具备：

  • 静态的字符串可以被节点解析
  • 指向blackboard上一个条目的“指针”，由一个键识别

blackboard是所有节点共享的简单的 键/值存储

blackboard的一个entry相当于一个键/值对（语义参考数据库）

输入接口可以读取blackboard中的entry，输出口可以写entry

输出接口

输出口可以是字符串也可以是blackboard的一个entry

类似{the_answer}的字符串代表blackboard上名字叫the_answer的条目

代码

//
// Created by jiang on 2021/9/18.
//

#include <behaviortree_cpp_v3/action_node.h>
#include <behaviortree_cpp_v3/bt_factory.h>

using namespace BT;

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

    static PortsList providedPorts()
    {
        return {InputPort<std::string>("message")};
    }

protected:
    NodeStatus tick() override
    {
        Optional<std::string> msg = getInput<std::string>("message");
        if (!msg) {
            throw RuntimeError("missing required input [message]: ", msg.error());
        }
        std::cout << "Robot says: " << msg.value() << std::endl;
        return NodeStatus::SUCCESS;
    }
};

NodeStatus SaySomethingSimple(TreeNode &self)
{
    Optional<std::string> msg = self.getInput<std::string>("message");
    if (!msg) {
        throw RuntimeError("missing required input [message]: ", msg.error());
    }
    std::cout << "Robot says: " << msg.value() << std::endl;
    return NodeStatus::SUCCESS;
}

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

    static PortsList providedPorts()
    {
        return {OutputPort<std::string>("text")};
    }

protected:
    NodeStatus tick() override
    {
        setOutput("text", "The answer is 42");
        return NodeStatus::SUCCESS;
    }
};

int main()
{
    BehaviorTreeFactory factory;
    factory.registerNodeType<SaySomething>("SaySomething");
    factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");

    PortsList saySomethingPorts = {InputPort<std::string>("message")};
    factory.registerSimpleAction("SaySomething2", SaySomethingSimple, saySomethingPorts);
    auto tree = factory.createTreeFromFile("../../test_port/port1.xml");
    tree.tickRoot();
    return 0;
}
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <!-- ////////// -->
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Action ID="SaySomething" message="start thinking..."/>
            <Action ID="ThinkWhatToSay" text="{the_answer}"/>
            <Action ID="SaySomething" message="{the_answer}"/>
            <Action ID="SaySomething2" message="say something 2 works"/>
            <Action ID="SaySomething2" message="{the_answer}"/>
        </Sequence>
    </BehaviorTree>
    <!-- ////////// -->
    <TreeNodesModel>
        <Action ID="SaySomething">
            <input_port name="message"/>
        </Action>
        <Action ID="SaySomething2">
            <input_port name="message"/>
        </Action>
        <Action ID="ThinkWhatToSay">
            <output_port name="text"/>
        </Action>
    </TreeNodesModel>
    <!-- ////////// -->
</root>

通用接口

通过重载模板函数使得可以把字符串转成自定义的类结构

template <typename T>
inline T convertFromString(StringView str);

// 例如
struct Position2D
{
  double x;
  double y;
};

namespace BT
{
    template <> inline Position2D convertFromString(StringView str)
    {
        // The next line should be removed...
        printf("Converting string: \"%s\"\n", str.data() );

        // We expect real numbers separated by semicolons
        auto parts = splitString(str, ';');
        if (parts.size() != 2)
        {
            throw RuntimeError("invalid input)");
        }
        else{
            Position2D output;
            output.x     = convertFromString<double>(parts[0]);
            output.y     = convertFromString<double>(parts[1]);
            return output;
        }
    }
} // end namespace BT

注册接口使用

static PortsList providedPorts()
{
    // Optionally, a port can have a human readable description
    const char*  description = "Simply print the goal on console...";
    return { InputPort<Position2D>("target", description) };
}

在xml文件中使用

<SequenceStar name="root">
    <CalculateGoal   goal="{GoalPosition}" />
    <PrintTarget     target="{GoalPosition}" />
    <SetBlackboard   output_key="OtherGoal" value="-1;3" />
    <PrintTarget     target="{OtherGoal}" />
</SequenceStar>

序列和异步动作节点

SequenceNode和ReactiveSequence的区别

异步的动作有自己的线程。这允许用户使用阻塞函数，但要把执行的流程返回给树。

机器人移动例子：

//
// Created by jiang on 2021/9/18.
//

#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/action_node.h>
#include <thread>

using namespace BT;

inline void SleepMS(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

struct Pose2D
{
    double x;
    double y;
    double theta;
};
namespace BT
{

template<>
inline Pose2D convertFromString(StringView str)
{
    auto parts = splitString(str, ';');
    if (parts.size() != 3) {
        throw RuntimeError("invalid input)");
    } else {
        Pose2D output{
                convertFromString<double>(parts[0]),
                convertFromString<double>(parts[1]),
                convertFromString<double>(parts[2]),
        };
        return output;
    }
}
}

class MoveBaseAction : public AsyncActionNode
{
public:
    MoveBaseAction(const std::string &name, const NodeConfiguration &config) :
            AsyncActionNode(name, config)
    {}

    static PortsList providedPorts()
    {
        return {InputPort<Pose2D>("goal")};
    }

protected:
    NodeStatus tick() override;
};

NodeStatus MoveBaseAction::tick()
{
    std::cout << "move base thread id: " << std::this_thread::get_id();
    Pose2D goal{};
    if (!getInput<Pose2D>("goal", goal)) {
        throw RuntimeError("missing required input [goal]");
    }
    printf("[MoveBase STATED]. goal: x=%.f y = %.1f theta=%.2f\n", goal.x, goal.y, goal.theta);
    int count = 0;
    while (!isHaltRequested() && count++ < 25) {
        SleepMS(10);
    }
    std::cout << "[MoveBase FINISHED]" << std::endl;
    return isHaltRequested() ? NodeStatus::FAILURE : NodeStatus::SUCCESS;
}

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

    static PortsList providedPorts()
    {
        return {InputPort<std::string>("message")};
    }

protected:
    NodeStatus tick() override
    {
        Optional<std::string> msg = getInput<std::string>("message");
        if (!msg) {
            throw RuntimeError("missing required input [message]: ", msg.error());
        }
        std::cout << "Robot says: " << msg.value() << std::endl;
        return NodeStatus::SUCCESS;
    }
};

NodeStatus CheckBattery(TreeNode &node)
{
    std::cout << "[ Battery: OK ]" << std::endl;
    return NodeStatus::SUCCESS;
}

int main()
{
    BehaviorTreeFactory factory;
    factory.registerSimpleCondition("BatteryOK", CheckBattery);
    factory.registerNodeType<MoveBaseAction>("MoveBase");
    factory.registerNodeType<SaySomething>("SaySomething");

    auto tree = factory.createTreeFromFile("../../test_async/async.xml");

    NodeStatus status;

    std::cout << "main thread id: " << std::this_thread::get_id();

    std::cout << "\n--- 1st executeTick() --- " << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    SleepMS(150);
    std::cout << "\n--- 2nd executeTick() ---" << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    SleepMS(150);
    std::cout << "\n--- 3rd executeTick() ---" << std::endl;
    status = tree.tickRoot();
    std::cout << "result : " << status << std::endl;

    std::cout << std::endl;

    return 0;
}
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
    <BehaviorTree ID="BehaviorTree">
        <Sequence>
            <Action ID="BatteryOK"/>
            <Action ID="SaySomething" message="mission started..."/>
            <Action ID="MoveBase" goal="1.0;2.0;3.0"/>
            <Action ID="SaySomething" message="mission completed!"/>
        </Sequence>
    </BehaviorTree>

    <TreeNodesModel>
        <Action ID="SaySomething">
            <input_port name="message"/>
        </Action>
        <Action ID="MoveBase">
            <input_port name="goal"/>
        </Action>
    </TreeNodesModel>
</root>

MoveBaseAction::tick()在线程中执行，而MoveBaseAction::executeTick()是在主线程中执行

有责任去实现halt()函数实现停止任务的功能

第一次和第二次都是最终返回RUNNING，第三次返回SUCCESS

RUNNING对于序列来说会停止执行后续任务

子树和日志

子树意味着可以创建分层的行为树，用很多小的可复用的行为树来组成大的行为树

例子

<root main_tree_to_execute = "MainTree">

    <BehaviorTree ID="DoorClosed">
        <Sequence name="door_closed_sequence">
            <Inverter>
                <IsDoorOpen/>
            </Inverter>
            <RetryUntilSuccesful num_attempts="4">
                <OpenDoor/>
            </RetryUntilSuccesful>
            <PassThroughDoor/>
        </Sequence>
    </BehaviorTree>

    <BehaviorTree ID="MainTree">
        <Fallback name="root_Fallback">
            <Sequence name="door_open_sequence">
                <IsDoorOpen/>
                <PassThroughDoor/>
            </Sequence>
            <SubTree ID="DoorClosed"/>
            <PassThroughWindow/>
        </Fallback>
    </BehaviorTree>

</root>

使用<SubTree ID="DoorClosed"/>调用子树

日志是用来显示记录发布状态改变的机制

int main()
{
    using namespace BT;
    BehaviorTreeFactory factory;

    // register all the actions into the factory
    // We don't show how these actions are implemented, since most of the
    // times they just print a message on screen and return SUCCESS.
    // See the code on Github for more details.
    factory.registerSimpleCondition("IsDoorOpen", std::bind(IsDoorOpen));
    factory.registerSimpleAction("PassThroughDoor", std::bind(PassThroughDoor));
    factory.registerSimpleAction("PassThroughWindow", std::bind(PassThroughWindow));
    factory.registerSimpleAction("OpenDoor", std::bind(OpenDoor));
    factory.registerSimpleAction("CloseDoor", std::bind(CloseDoor));
    factory.registerSimpleCondition("IsDoorLocked", std::bind(IsDoorLocked));
    factory.registerSimpleAction("UnlockDoor", std::bind(UnlockDoor));

    // Load from text or file...
    auto tree = factory.createTreeFromText(xml_text);

    // This logger prints state changes on console
    StdCoutLogger logger_cout(tree);

    // This logger saves state changes on file
    FileLogger logger_file(tree, "bt_trace.fbl");

    // This logger stores the execution time of each node
    MinitraceLogger logger_minitrace(tree, "bt_trace.json");

#ifdef ZMQ_FOUND
    // This logger publish status changes using ZeroMQ. Used by Groot
    PublisherZMQ publisher_zmq(tree);
#endif

    printTreeRecursively(tree.rootNode());

    //while (1)
    {
        NodeStatus status = NodeStatus::RUNNING;
        // Keep on ticking until you get either a SUCCESS or FAILURE state
        while( status == NodeStatus::RUNNING)
        {
            status = tree.tickRoot();
            CrossDoor::SleepMS(1);   // optional sleep to avoid "busy loops"
        }
        CrossDoor::SleepMS(2000);
    }
    return 0;
}

Groot使用ZMQ发布的状态信息

接口重映设

在CrossDoor的例子中，我们看到子树从其父节点（例子中的MainTree）的角度看就像一个单叶节点。此外，为了避免在非常大的树中出现名称冲突，任何树和子树都使用不同的blackboard实例。

由于这个原因，我们需要明确地将一棵树的端口与它的子树的端口连接起来。

例子

<root main_tree_to_execute = "MainTree">

    <BehaviorTree ID="MainTree">

        <Sequence name="main_sequence">
            <SetBlackboard output_key="move_goal" value="1;2;3" />
            <SubTree ID="MoveRobot" target="move_goal" output="move_result" />
            <SaySomething message="{move_result}"/>
        </Sequence>

    </BehaviorTree>

    <BehaviorTree ID="MoveRobot">
        <Fallback name="move_robot_main">
            <SequenceStar>
                <MoveBase       goal="{target}"/>
                <SetBlackboard output_key="output" value="mission accomplished" />
            </SequenceStar>
            <ForceFailure>
                <SetBlackboard output_key="output" value="mission failed" />
            </ForceFailure>
        </Fallback>
    </BehaviorTree>

</root>
  • MoveRobot是一个子树，被包含于MainTree
  • 想要把MoveRobot的端口和MainTree的端口连接起来
  • 使用XML标签，其中internal/external这两个词分别指的是子树和其父树

<SubTree ID="MoveRobot" target="move_goal" output="move_result" />

把move_goal传入子树的target，把子树的结果output传出到move_result

remap

包装遗留代码

使用λ函数

class LegacyClass {
public:
    bool method() {
        return true;
    }
}

LegacyClass legacy;
auto wrapFunc = [&legacy] (NodeTree &node) -> NodeStatus {
    ...
    return ...
}

PortsList ports = { ... };
factory.registerSimpleAction("...", wrapFunc, ports);

...

附加参数

方法1：使用NodeBuilder

假如节点的构造函数有更多的参数

Action_A(const std::string& name, const NodeConfiguration& config,
             int arg1, double arg2, std::string arg3 ):
        SyncActionNode(name, config),
        _arg1(arg1),
        _arg2(arg2),
        _arg3(arg3) {}

这时创建一个NodeBuilder函数

NodeBuilder builder_A =
   [](const std::string& name, const NodeConfiguration& config)
{
    return std::make_unique<Action_A>( name, config, 42, 3.14, "hello world" );
};

再传入注册函数的第二个参数中去

factory.registerBuilder<Action_A>( "Action_A", builder_A);

方法2：使用初始化方法

void init( int arg1, double arg2, const std::string& arg3 )
{
    _arg1 = (arg1);
    _arg2 = (arg2);
    _arg3 = (arg3);
}

使用C++ RTTI根据类型调用函数

for( auto& node: tree.nodes )
{
    if( auto action_B = dynamic_cast<Action_B*>( node.get() ))
    {
        action_B->init( 42, 3.14, "hello world");
    }
}

协程

行为树提供了两种方便使用的创建异步动作的抽象，对于以下动作：

  • 花费很长的时间结束
  • 可能会返回RUNNING
  • 能够被停止

第一种类为AsyncActionNode，在隔离的线程中执行tick()函数

在本教程中引入CoroActionNode，使用协程来完成相似结果的动作

协程不会产生新的线程同时更有效率，而且不需要担心线程安全问题

在协程中，当用户想让执行的动作取消，需要显式调用yield方法

CoroActionNode封装了yield函数到setStatusRunningYield()中

例子

typedef std::chrono::milliseconds Milliseconds;

class MyAsyncAction: public CoroActionNode
{
  public:
    MyAsyncAction(const std::string& name):
        CoroActionNode(name, {})
    {}

  private:
    // This is the ideal skeleton/template of an async action:
    //  - A request to a remote service provider.
    //  - A loop where we check if the reply has been received.
    //  - You may call setStatusRunningAndYield() to "pause".
    //  - Code to execute after the reply.
    //  - A simple way to handle halt().
    NodeStatus tick() override
    {
        std::cout << name() <<": Started. Send Request to server." << std::endl;

        TimePoint initial_time = Now();
        TimePoint time_before_reply = initial_time + Milliseconds(100);

        int count = 0;
        bool reply_received = false;

        while( !reply_received )
        {
            if( count++ == 0)
            {
                // call this only once
                std::cout << name() <<": Waiting Reply..." << std::endl;
            }
            // pretend that we received a reply
            if( Now() >= time_before_reply )
            {
                reply_received = true;
            }

            if( !reply_received )
            {
                // set status to RUNNING and "pause/sleep"
                // If halt() is called, we will NOT resume execution
                setStatusRunningAndYield();
            }
        }

        // This part of the code is never reached if halt() is invoked,
        // only if reply_received == true;
        std::cout << name() <<": Done. 'Waiting Reply' loop repeated "
                  << count << " times" << std::endl;
        cleanup(false);
        return NodeStatus::SUCCESS;
    }

    // you might want to cleanup differently if it was halted or successful
    void cleanup(bool halted)
    {
        if( halted )
        {
            std::cout << name() <<": cleaning up after an halt()\n" << std::endl;
        }
        else{
            std::cout << name() <<": cleaning up after SUCCESS\n" << std::endl;
        }
    }

    void halt() override
    {
        std::cout << name() <<": Halted." << std::endl;
        cleanup(true);
        // Do not forget to call this at the end.
        CoroActionNode::halt();
    }

    Timepoint Now()
    {
        return std::chrono::high_resolution_clock::now();
    };
};
 <root >
     <BehaviorTree>
        <Timeout msec="150">
            <SequenceStar name="sequence">
                <MyAsyncAction name="action_A"/>
                <MyAsyncAction name="action_B"/>
            </SequenceStar>
        </Timeout>
     </BehaviorTree>
 </root>