库
主要用于机器人和游戏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状态,不能改变系统状态
序列例子
BT使用最多的SequenceNode
控制节点的子节点都是有序的,从左到右执行
简单来说:
- 子节点返回成功,tick下一个
- 子节点返回失败,没有子节点将会tick,序列返回失败
- 如果所有的子节点返回成功,序列返回成功
Decorator
目的:
- 转换从子节点收到的结果
- 停止子节点的执行
- 重复tick子节点,取决于装饰节点的类型
节点Inverter装饰器,把返回的值反转
节点Retry重复N次子节点
以上节点组合形式在门开了后会取消所有动作
FallbackNodes
又名为被理解为Selectors,是一些节点,顾名思义,可以表达回退策略,即如果一个子节点返回FAILURE,接下来该怎么做。
按照顺序触发:
- 如果子节点返回失败,tick下个节点
- 如果返回成功,不再tick节点,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
包装遗留代码
使用λ函数
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>