行为树源码解析——各节点与调试工具详解

最新推荐文章于 2024-05-18 10:00:13 发布

cpeterz

最新推荐文章于 2024-05-18 10:00:13 发布

阅读量1.2k

点赞数 9

文章标签： b树 c++ Powered by 金山文档

本文链接：https://blog.csdn.net/WDHDNRYX/article/details/128741716

版权

1.DecoratorNodes

1.1 基类

classDecoratorNode : publicTreeNode {

protected: TreeNode*child_node_;

... ...

}

1.2 BlackboardPreconditionNode

细分为3个节点：BlackboardCheckInt、BlackboardCheckDouble、BlackboardCheckString。

检查blackboard的某个port的值是否符合预期的。

包含3个InputPort，当value_A和value_B的值相等时，执行子节点。否则，不执行子节点，并返回return_on_mismatch设定的值。

staticPortsListprovidedPorts() {

return {InputPort("value_A"),

InputPort("value_B"),

InputPort<NodeStatus>("return_on_mismatch") };

}

<BlackboardCheckIntvalue_A="{the_answer}"

value_B="42"

return_on_mismatch="FAILURE"/>

1.3 DelayNode

延时delay_msec毫秒后，执行子节点，并返回子节点的执行结果。延时期间，返回RUNNING。

staticPortsListprovidedPorts() {

return {InputPort<unsigned>("delay_msec", "Tick the child after a few milliseconds")};

}

<Delaydelay_msec="5000">

</Delay>

1.4 ForceFailureNode，ForceSuccessNode

1.5 InverterNode

1.6 KeepRunningUntilFailureNode

如果子节点执行后返回RUNNING或SUCCESS，下次tick()继续执行子节点，直到子节点返回FAILURE。

1.7 RepeatNode

重复执行子节点NUM_CYCLES 次，若每次都返回 SUCCESS，该节点返回SUCCESS；

若子节点某次返回FAILURE，该节点不再重复执行子节点，立即返回FAILURE；

若子节点返回RUNNING，该节点也返回RUNNING。

staticPortsListprovidedPorts() {

return { InputPort<int>(NUM_CYCLES,

"Repeat a succesful child up to N times. "

"Use -1 to create an infinite loop.") };

}

<Repeatnum_cycles="3">

</Repeat>

1.8 RetryNode

如果子节点执行后返回RUNNING，该节点返回RUNNING；

如果子节点执行后返回SUCCESS，该节点返回SUCCESS，不再执行；

如果子节点执行后返回FAILURE，该节点再次尝试执行子节点，直到尝试了num_attempts次；

staticPortsListprovidedPorts() {

return { InputPort<int>(NUM_CYCLES,

"Repeat a succesful child up to N times. "

"Use -1 to create an infinite loop.") };

}

<Repeatnum_cycles="3">

</Repeat>

1.9 SubtreeNode

用来封装一个subtree，这样会有一个独立的blackboard，shared_blackboard port的默认值是false，因此开发者要自行重映射端口。但tick()函数中并没有使用shared_blackboard port，而是在 BehaviorTree.CPP\src\xml_parsing.cpp中使用的，这点要注意，SubtreePlusNode的__autoremap port也是如此。

staticPortsListprovidedPorts() {

return { InputPort<bool>("__shared_blackboard", false,

"If false (default) the subtree has its own blackboard and you"

"need to do port remapping to connect it to the parent") };

}

1.10 SubtreePlusNode

控制重映射的强化版SubtreeNode。当__autoremap port为true时，会自动重映射名称相同的port。结合代码示例会更容易理解。

staticPortsListprovidedPorts() {

return { InputPort<bool>("__autoremap", false,

"If true, all the ports with the same name will be remapped") };

}

上面有3种重映射的实现方式。第1、2种是最常见的。

第1种将Subtree的blackboard的param entry映射到Parent tree的blackboard的myParam entry，将其值设置为字符串"Hello"。

第2种将Subtree的blackboard的param entry的值直接设置为字符串"World"。

第3种在Parent tree的blackboard中增加了param entry，没有指定映射到subtree的哪个port。但由于设定__autoremap=true，该entry会自动映射到subtree的blackboard的param entry。SaySomething节点会在其message port中获取到值为字符串“Auto remapped”。

1.11 TimeoutNode

在设置的msec 毫秒内，返回子节点执行的状态。若子节点返回FAILURE或SUCCESS，不再执行。如果超时，终止子节点执行，并返回FAILURE。类中使用了TimerQueue作为计时器，可以定时多个任务。

staticPortsListprovidedPorts() {

return { InputPort<unsigned>("msec", "After a certain amount of time, "

"halt() the child if it is still running.") };

}

<Timeoutmsec="5000">

</Timeout>

2.ControlNodes

2.1 基类

ControlNode类。

classControlNode : publicTreeNode {

protected:

std::vector<TreeNode*>children_nodes_;

... ...

}

2.2 FallbackNode

如果某个子节点返回RUNNING，返回RUNNING，且下次tick()时之前的子节点不会再执行。

如果某个子节点返回SUCCESS，返回SUCCESS。

如果某个子节点返回FAILURE，立即执行下一个子节点（不会等下一次tick()）。如果所有子节点返回FAILURE，返回FAILURE。

2.3 ReactiveFallback

FallbackNode的reactive版本，类似ParallelNode，最多含1个asynchronous node。

如果某个子节点返回RUNNING，返回RUNNING，且下次tick()时之前的子节点会再次执行，reactive所在。

如果某个子节点返回SUCCESS，不再执行，且返回SUCCESS。

如果某个子节点返回FAILURE，立即执行下一个子节点（不会等下一次tick()）。如果所有子节点返回FAILURE，返回FAILURE。

2.4 ParallelNode

当返回SUCCESS的子节点个数>=THRESHOLD_SUCCESS时，返回SUCCESS。

当返回FAILURE的子节点个数>=THRESHOLD_FAILURE时，返回FAILURE。

当程序判断绝不可能SUCCESS时，返回FAILURE。如 failure_children_num > children_count - success_threshold_。

2.5 IfThenElseNode

有2或3个子节点，node1就是if判断的条件。如果node1返回SUCCESS，那么node2执行；否则，node3执行。如果没有node3，返回FAILURE。该结点not reactive，体现在一旦node1不返回RUNNING了，就进入了node2或node3的执行，以后tick()不会再执行node1了，也即不会再检查if条件的变化。

<Work/>

</IfThenElse>

2.6 WhileDoElseNode

是IfThenElseNode的reactive版本。功能同上，reactive体现在每次tick()都会执行node1，即检查if条件的变化。若node1返回值有SUCCESS、FAILURE的切换变化，就会打断node2或node3的执行，重新选择对应的node。

2.7 SwitchNode

switch-case。blackboard的某个entry的值和哪个case的值相等，就执行哪个case。同样的，最后1个未指定值的case就是default默认执行的分支。SwitchN有N个分支，必须指定N个子节点对应。reactive体现在每次tick()都会重新读取entry的值，选择对应的分支，并终止其他节点。

<Switch3variable="{var}" case_1="1"case_2="42"case_3="666">

<ActionAname="action_when_var_eq_1"/>

<ActionBname="action_when_var_eq_42"/>

<ActionCname="action_when_var_eq_666"/>

<ActionDname="default_action"/>

</Switch3>

3. ActionNodes

3.1 ActionNodeBase

最通用的action node基类，子类要实现executeTick()、tick()、halt()等函数。

3.2 SyncActionNode

继承自ActionNodeBase，同步action node，不会返回RUNNING，无需开发者实现halt()。

3.3 SimpleActionNode

继承自SyncActionNode，常使用lambdas或std::bind构造std::function对象来构造SimpleActionNode，这个function就是tick()的内容。这样开发者无需定义node，只需指定node的类型ID和tick()即可，SimpleConditionNode同理。

GripperInterfacegripper;

// open()是GripperInterface类的成员函数

factory.registerSimpleAction("OpenGripper",

std::bind(&GripperInterface::open, &gripper));

3.4 AsyncActionNode

继承自ActionNodeBase，会在executeTick()函数中创建1个线程来执行tick()，通过halt_requested_变量监控节点是否被终止。开发者需要在子类tick()中周期性检查isHaltRequested()的返回值，以便及时终止执行。子类halt()要记得调用父类AsyncActionNode::halt()。子类不必显式的返回RUNNING，只需根据结果返回SUCCESS/FAILURE，未执行完成时会自动置位和返回RUNNING。

用C++11的std::async代替线程的创建

3.5 StatefulActionNode

继承自ActionNodeBase，像状态机的运行方式。如果节点在IDLE state就会调用onStart()，如果在RUNNING state就会调用onRunning()，如果被halt()就会调用onHalted()。

4. 调试工具

4.1 Groot

Editor、Monitor、Log Replay 3种模式，具有行为树编辑、状态监控、历史log回放等功能。

在Groot中可以图形化的方式创建节点（node），为节点添加输入输出端口（port），可以像Visio一样拖动、连接节点，从而构造行为树，而无需在意节点代码是否完成。将树保存、导出为xml文件，可以被BehaviorTree.CPP的接口读入并解析。这样就可以避免开发者自行编写xml文件的复杂局面。

4.2 StdCoutLogger

作用：在终端打印行为树中的节点执行状态变化。

代码仅需在加载tree后添加StdCoutLogger类的1个实例（且只能有1个实例）

4.3 FileLogger

作用：行为树中的节点执行状态变化保存在文件中（必须是*.fbl格式文件），可以通过Groot打开并回放执行过程。

当选中左侧的节点时，右侧会通过线条的颜色来表示执行的状态（绿色-SUCCESS，橙色-RUNNING，青色-未执行）。

4.4 MinitraceLogger

作用：保存节点的执行时序。

4.5 PublisherZMQ

作用：在节点执行的同时发布其状态变化，在Groot中实时观察。

Groot需要选择Monitor模式，并设置下列输入。如果行为树与Groot都在同一台机器运行的话，就自发自收，Server IP可以设置为“127.0.0.1”，Publisher Port设置为“1666”，Server Port设置为“1667”。

Groot会自动获得树的结构，无需用户手动加载，但是它会自动展开1棵树中的所有子树，使得界面内容非常密集，因此复杂的树并不方便观察。

4.6 printTreeRecursively()内置函数

作用：层级打印树结构，默认打印在终端。

4.7 debugMessage()内置函数

打印不同的树之间的端口（port）映射关系，也可以反映出port是否被设置值。

5.XML创建加载行为树

5.1 BehaviorTreeFactory::createTreeFromText()

树的加载和创建由createTreeFromText() 实现，该函数的第2个参数具有默认参数，即初创建的blackboard，是一个局部变量，但是由智能指针指向它。因此，只要引用计数大于0，该变量仍然不会释放，可以访问得到。

TreecreateTreeFromText(conststd::string&text,

Blackboard::Ptrblackboard=Blackboard::create());

TreeBehaviorTreeFactory::createTreeFromText(conststd::string&text,

Blackboard::Ptrblackboard) {

XMLParserparser(*this);

// 加载和解析文本，检查各项元素是否符合BT的概念要求。

parser.loadFromText(text);

// 创建树和所有节点的实例，构造好树之间、节点之间的父子关系，port的映射关系等。

autotree=parser.instantiateTree(blackboard);

// 将树的节点信息绑定给树实例变量

tree.manifests=this->manifests();

returntree;

}

createTreeFromText() 主要有3部分。其中的manifests包含了树的所有节点类型信息，其实节点的builder和manifest在树建立之前已经通过register函数传给factory变量了。

template<typenameT>

voidregisterNodeType(conststd::string&ID, PortsListports) {

...

registerBuilder(CreateManifest<T>(ID, ports), CreateBuilder<T>());

}

voidBehaviorTreeFactory::registerBuilder(constTreeNodeManifest&manifest,

constNodeBuilder&builder) {

autoit=builders_.find(manifest.registration_ID);

if (it!=builders_.end()) {

throwBehaviorTreeException("ID [", manifest.registration_ID,

"] already registered");

}

builders_.insert({manifest.registration_ID, builder});

manifests_.insert({manifest.registration_ID, manifest});

}

5.2 XMLParser::loadFromText()

具体由XMLParser::Pimpl::loadDocImpl()执行，主要有如下几个步骤。

第1个for循环，递归加载本xml中所include的子树xml文件，先加载子树，再加载外层树，相当于深度优先搜索。

第2个for循环，遍历本xml文件中的树的名称或ID（相当于树的根节点），保存在类XMLParser::Pimpl的成员变量tree_roots中。

第3、4个for循环，将构造树之前就注册的所有节点，和2中读取的树的根节点，都存入局部变量std::set<std::string> registered_nodes; 然后将其传入VerifyXML()。

VerifyXML()负责检查树的设计要求是否满足。检查项有：

TreeNodesModel标签是否合法，主要用于Groot可视化。

各种node的子节点数量是否合法，是否有ID。比如ControlNode至少有1个子节点，DecoratorNode只有1个子节点，Subtree没有子节点。

是否有未注册的不认识的节点。

针对非subtree节点进行递归检查。

是否指定main_tree_to_execute 标签。如果有多个BehaviorTree，则必须指定main_tree_to_execute，如果只有1个BehaviorTree，就不需要指定。

5.3 XMLParser::Pimpl::recursivelyCreateTree()

函数内递归执行recursiveStep()，注意第1个参数是父节点。

voidBT::XMLParser::Pimpl::recursivelyCreateTree(

conststd::string&tree_ID, Tree&output_tree, Blackboard::Ptrblackboard,

constTreeNode::Ptr&root_parent) {

std::function<void(constTreeNode::Ptr&, constXMLElement*)>recursiveStep;

recursiveStep= [&](constTreeNode::Ptr&parent, constXMLElement*element) {

...

};

autoroot_element=tree_roots[tree_ID]->FirstChildElement();

// start recursion

recursiveStep(root_parent, root_element);

}

recursiveStep()分为3部分。

调用XMLParser::Pimpl::createNodeFromXML()创建节点实例，将该实例保存在树的std::vector<TreeNode::Ptr> nodes 成员变量中。

如果该节点是SUBTREE类型的，细分SubtreeNode和SubtreePlusNode来处理。

如果是SubtreeNode，就根据__shared_blackboard的值来创建blackboard，并添加映射信息，然后递归调用recursivelyCreateTree()来创建子树。

如果是SubtreePlusNode，就根据__autoremap的值来创建blackboard的port的映射，然后递归调用recursivelyCreateTree()来创建子树。

如果该节点不是SUBTREE类型的，递归调用recursiveStep()，并把该节点作为接下来待创建节点的父节点。如果该节点没有其他包含的元素了，就不再递归了，从recursiveStep()返回，进而从recursivelyCreateTree()返回，进而从instantiateTree() 返回。