本文已参与「新人创造礼」活动,一同开启掘金创造之路。
创立行为树
行为树,类似于状态机,过是一种在正确的时刻在正确的条件下调用回调的机制。 此外,咱们将替换运用“callback”和“tick”这两个词。
简单Beahvoir实例
在这儿刺进图片描述
初始(推荐)的创立树节点的方式是继承:
// Example of custom SyncActionNode (synchronous action)
// without ports.
class ApproachObject : public BT::SyncActionNode
{
public:
ApproachObject(const std::string& name) :
BT::SyncActionNode(name, {})
{
}
// You must override the virtual function tick()
BT::NodeStatus tick() override
{
std::cout << "ApproachObject: " << this->name() << std::endl;
return BT::NodeStatus::SUCCESS;
}
};
树节点有一个name,它不必是唯一的。 rick()方法是完成功能的地方,它有必要回来一个NodeStatus,例如:running、success或failure。 咱们也能够用dependency injection的方法基于函数指针来 创立一个树节点: 函数指针的格局为:
BT::NodeStatus myFunction()
BT::NodeStatus myFunction(BT::TreeNode& self)
举个栗子~
using namespace BT;
// Simple function that return a NodeStatus
BT::NodeStatus CheckBattery()
{
std::cout << "[ Battery: OK ]" << std::endl;
return BT::NodeStatus::SUCCESS;
}
// We want to wrap into an ActionNode the methods open() and close()
class GripperInterface
{
public:
GripperInterface(): _open(true) {}
NodeStatus open() {
_open = true;
std::cout << "GripperInterface::open" << std::endl;
return NodeStatus::SUCCESS;
}
NodeStatus close() {
std::cout << "GripperInterface::close" << std::endl;
_open = false;
return NodeStatus::SUCCESS;
}
private:
bool _open; // shared information
};
咱们能够经过下列函数指针创立SimpleActionNode
- CheckBattery()
- GripperInterface::open()
- GripperInterface::close()
运用 XML 动态创立树
考虑下面my_tree.xml文件
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<CheckBattery name="check_battery"/>
<OpenGripper name="open_gripper"/>
<ApproachObject name="approach_object"/>
<CloseGripper name="close_gripper"/>
</Sequence>
</BehaviorTree>
</root>
咱们吧自己自定义的Treenode注册进BehavoirTreeFactory,接着加载xml。XML 中运用的标识符有必要与用于注册 TreeNode 的标识符共同
#include "behaviortree_cpp_v3/bt_factory.h"
// file that contains the custom nodes definitions
#include "dummy_nodes.h"
int main()
{
// We use the BehaviorTreeFactory to register our custom nodes
BehaviorTreeFactory factory;
// Note: the name used to register should be the same used in the XML.
using namespace DummyNodes;
// The recommended way to create a Node is through inheritance.
factory.registerNodeType<ApproachObject>("ApproachObject");
// Registering a SimpleActionNode using a function pointer.
// you may also use C++11 lambdas instead of std::bind
factory.registerSimpleCondition("CheckBattery", std::bind(CheckBattery));
//You can also create SimpleActionNodes using methods of a class
GripperInterface gripper;
factory.registerSimpleAction("OpenGripper",
std::bind(&GripperInterface::open, &gripper));
factory.registerSimpleAction("CloseGripper",
std::bind(&GripperInterface::close, &gripper));
// Trees are created at deployment-time (i.e. at run-time, but only
// once at the beginning).
// IMPORTANT: when the object "tree" goes out of scope, all the
// TreeNodes are destroyed
auto tree = factory.createTreeFromFile("./my_tree.xml");
// To "execute" a Tree you need to "tick" it.
// The tick is propagated to the children based on the logic of the tree.
// In this case, the entire sequence is executed, because all the children
// of the Sequence return SUCCESS.
tree.tickRoot();
return 0;
}
/* Expected output:
*
[ Battery: OK ]
GripperInterface::open
ApproachObject: approach_object
GripperInterface::close
*/
基本接口(port)
输入输出接口
node能够完成简单或杂乱的功能,具有很好的抽象性。所以跟函数在概念上有多不同。可是跟函数类似的。咱们通常期望node能够:
- 向node传递参数
- 从node获取信息
- 一个node的输出是另一个node的输入 Behavior.cpp经过ports机制来处理数据流。接下来哦咱们创立下面这个树:
输入接口
一个有效的输入能够是:
- 能被node解析的字符串
- 指向blackboard entry的指针,由“key”定义 “balackboard”是一个树节点同享的贮存空间,存放着键/值 key/value对。假设咱们创立一个名为SaySomething的ActionNode,它打印给定的字符串。这个字符串经过名为message的port被传递。 考虑下面这两行代码有何不同:
<SaySomething message="hello world" />
<SaySomething message="{greetings}" />
第一行代码”hello world”字符串经过”meaasge”的接口被传递,这个字符串正在运行时不能被改变。 第二行代码读取了在blackboard中entry是”greetings”的值,这个值在运行中能够被改变。 ActionNode Saysomething代码示例:
// SyncActionNode (synchronous action) with an input port.
class SaySomething : public SyncActionNode
{
public:
// If your Node has ports, you must use this constructor signature
SaySomething(const std::string& name, const NodeConfiguration& config)
: SyncActionNode(name, config)
{ }
// It is mandatory to define this static method.
static PortsList providedPorts()
{
// This action has a single input port called "message"
// Any port must have a name. The type is optional.
return { InputPort<std::string>("message") };
}
// As usual, you must override the virtual function tick()
NodeStatus tick() override
{
Optional<std::string> msg = getInput<std::string>("message");
// Check if optional is valid. If not, throw its error
if (!msg)
{
throw BT::RuntimeError("missing required input [message]: ",
msg.error() );
}
// use the method value() to extract the valid message.
std::cout << "Robot says: " << msg.value() << std::endl;
return NodeStatus::SUCCESS;
}
};
这儿tick的功能也能够在函数中完成。这个函数的输入时BT:TreeNode的实例,为了要取得”message”接口。详细代码如下:
// Simple function that return a NodeStatus
BT::NodeStatus SaySomethingSimple(BT::TreeNode& self)
{
Optional<std::string> msg = self.getInput<std::string>("message");
// Check if optional is valid. If not, throw its error
if (!msg)
{
throw BT::RuntimeError("missing required input [message]: ", msg.error());
}
// use the method value() to extract the valid message.
std::cout << "Robot says: " << msg.value() << std::endl;
return NodeStatus::SUCCESS;
}
别的,声明输入输出接口的函数有必要是static: static MyCustomNode::PortsList providedPorts(); 别的能够运用模板函数TreeNode::getInput(key)来取得接口的输入内容。
输出接口
下面这个比如ThinkWhatToSay运用一个输出接口将字符串写入blackboard的entry中。
class ThinkWhatToSay : public SyncActionNode
{
public:
ThinkWhatToSay(const std::string& name, const NodeConfiguration& config)
: SyncActionNode(name, config)
{
}
static PortsList providedPorts()
{
return { OutputPort<std::string>("text") };
}
// This Action writes a value into the port "text"
NodeStatus tick() override
{
// the output may change at each tick(). Here we keep it simple.
setOutput("text", "The answer is 42" );
return NodeStatus::SUCCESS;
}
};
或者,大多数时候出于调试意图,能够运用称为 SetBlackboard 的内置操作将静态值写入entry。
<SetBlackboard output_key="the_answer" value="The answer is 42" />
一个杂乱的示例
本例,一个有四个动作的Sequence将被履行。
- Action1和2读message接口
- action3写入blackboard的the_answer。
- Action4jiangblackboard的the_answer读出来
#include "behaviortree_cpp_v3/bt_factory.h"
// file that contains the custom nodes definitions
#include "dummy_nodes.h"
int main()
{
using namespace DummyNodes;
BehaviorTreeFactory factory;
factory.registerNodeType<SaySomething>("SaySomething");
factory.registerNodeType<ThinkWhatToSay>("ThinkWhatToSay");
// SimpleActionNodes can not define their own method providedPorts().
// We should pass a PortsList explicitly if we want the Action to
// be able to use getInput() or setOutput();
PortsList say_something_ports = { InputPort<std::string>("message") };
factory.registerSimpleAction("SaySomething2", SaySomethingSimple,
say_something_ports );
auto tree = factory.createTreeFromFile("./my_tree.xml");
tree.tickRoot();
/* Expected output:
Robot says: hello
Robot says: this works too
Robot says: The answer is 42
*/
return 0;
}
通用类型接口ports with generic types
上面的比如中,接口的类型都为std::string。这个接口最简单,由于xml的格局就是一个字符串的类型。接下来学习下如何运用其他类型。
解析一个字符串
BehavoirTree.cp能够自动将字符串转化为经过类型,比如int,long,double,bool,NodeStatus等等。 同样用户也能够自己定义一个数据类型,比如
// We want to be able to use this custom type
struct Position2D
{
double x;
double y;
};
为了吧字符产解析为一个Position2D的类型,咱们应该链接到 BT::convertFromString(StringView) 的模板特例。咱们能够运用任何咱们想要的语法;在这种情况下,咱们只需用分号分隔两个数字。
// Template specialization to converts a string to Position2D.
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
这段代码中
- StringVIew是C++11版本的std::string_view。能够传递std::string或const char*
- spltString函数是library提供的,也能够用boost::algorithm::split
- 当咱们将输入分解成单独的数字时,能够重用特例”convertFromString()”
比如
在下面这个比如中,咱们自定义两个动作节点,一个向接口写入,另一个从接口读出。
class CalculateGoal: public SyncActionNode
{
public:
CalculateGoal(const std::string& name, const NodeConfiguration& config):
SyncActionNode(name,config)
{}
static PortsList providedPorts()
{
return { OutputPort<Position2D>("goal") };
}
NodeStatus tick() override
{
Position2D mygoal = {1.1, 2.3};
setOutput<Position2D>("goal", mygoal);
return NodeStatus::SUCCESS;
}
};
class PrintTarget: public SyncActionNode
{
public:
PrintTarget(const std::string& name, const NodeConfiguration& config):
SyncActionNode(name,config)
{}
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) };
}
NodeStatus tick() override
{
auto res = getInput<Position2D>("target");
if( !res )
{
throw RuntimeError("error reading port [target]:", res.error());
}
Position2D target = res.value();
printf("Target positions: [ %.1f, %.1f ]\n", target.x, target.y );
return NodeStatus::SUCCESS;
}
};
同样地,咱们也能够把输入输出接口,经过同一个blackboard的entry链接。
下面这个比如,完成四个动作的sequence
- 经过entry”GaolPosition”贮存Position2D的值
- 触发PringTarget,打印”GoalPosition”对应的value
- 运用”SetBlackboard”写入entry”OtherGoal”值。
- 再次触发PringTarget,打印“Other”对应的value
static const char* xml_text = R"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<SequenceStar name="root">
<CalculateGoal goal="{GoalPosition}" />
<PrintTarget target="{GoalPosition}" />
<SetBlackboard output_key="OtherGoal" value="-1;3" />
<PrintTarget target="{OtherGoal}" />
</SequenceStar>
</BehaviorTree>
</root>
)";
int main()
{
using namespace BT;
BehaviorTreeFactory factory;
factory.registerNodeType<CalculateGoal>("CalculateGoal");
factory.registerNodeType<PrintTarget>("PrintTarget");
auto tree = factory.createTreeFromText(xml_text);
tree.tickRoot();
/* Expected output:
Target positions: [ 1.1, 2.3 ]
Converting string: "-1;3"
Target positions: [ -1.0, 3.0 ]
*/
return 0;
}
Reactive Sequence 和异步节点
下一个比如展示了SequenceNode和ReactiveSequence的区别。 一个异步节点有它自己的线程。它能够允许用户运用阻塞函数并将履行流程回来给树。
// Custom type
struct Pose2D
{
double x, y, theta;
};
class MoveBaseAction : public AsyncActionNode
{
public:
MoveBaseAction(const std::string& name, const NodeConfiguration& config)
: AsyncActionNode(name, config)
{ }
static PortsList providedPorts()
{
return{ InputPort<Pose2D>("goal") };
}
NodeStatus tick() override;
// This overloaded method is used to stop the execution of this node.
void halt() override
{
_halt_requested.store(true);
}
private:
std::atomic_bool _halt_requested;
};
//-------------------------
NodeStatus MoveBaseAction::tick()
{
Pose2D goal;
if ( !getInput<Pose2D>("goal", goal))
{
throw RuntimeError("missing required input [goal]");
}
printf("[ MoveBase: STARTED ]. goal: x=%.f y=%.1f theta=%.2f\n",
goal.x, goal.y, goal.theta);
_halt_requested.store(false);
int count = 0;
// Pretend that "computing" takes 250 milliseconds.
// It is up to you to check periodicall _halt_requested and interrupt
// this tick() if it is true.
while (!_halt_requested && count++ < 25)
{
SleepMS(10);
}
std::cout << "[ MoveBase: FINISHED ]" << std::endl;
return _halt_requested ? NodeStatus::FAILURE : NodeStatus::SUCCESS;
}
方法 MoveBaseAction::tick() 在与调用 MoveBaseAction::executeTick() 的主线程不同的线程中履行。 代码中halt() 功能并不完整,需要加入真正暂停的功能。 用户还有必要完成 convertFromString(StringView),如上面的比如所示。
Sequence vs ReactiveSequence
下面的比如用了下面的sequence
<root>
<BehaviorTree>
<Sequence>
<BatteryOK/>
<SaySomething message="mission started..." />
<MoveBase goal="1;2;3"/>
<SaySomething message="mission completed!" />
</Sequence>
</BehaviorTree>
</root>
int main()
{
using namespace DummyNodes;
BehaviorTreeFactory factory;
factory.registerSimpleCondition("BatteryOK", std::bind(CheckBattery));
factory.registerNodeType<MoveBaseAction>("MoveBase");
factory.registerNodeType<SaySomething>("SaySomething");
auto tree = factory.createTreeFromText(xml_text);
NodeStatus status;
std::cout << "\n--- 1st executeTick() ---" << std::endl;
status = tree.tickRoot();
SleepMS(150);
std::cout << "\n--- 2nd executeTick() ---" << std::endl;
status = tree.tickRoot();
SleepMS(150);
std::cout << "\n--- 3rd executeTick() ---" << std::endl;
status = tree.tickRoot();
std::cout << std::endl;
return 0;
}
期望的成果应该是:
--- 1st executeTick() ---
[ Battery: OK ]
Robot says: "mission started..."
[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
--- 2nd executeTick() ---
[ MoveBase: FINISHED ]
--- 3rd executeTick() ---
Robot says: "mission completed!"
您可能现已注意到,当调用 executeTick() 时,MoveBase 第一次和第2次回来 RUNNING,最后第三次回来 SUCCESS。BatteryOK 只履行一次。 如果咱们改用 ReactiveSequence,当子 MoveBase 回来 RUNNING 时,将重新启动序列并再次履行条件 BatteryOK。 如果在任何时候,BatteryOK 回来 FAILURE,MoveBase 操作将被中断。
<root>
<BehaviorTree>
<ReactiveSequence>
<BatteryOK/>
<Sequence>
<SaySomething message="mission started..." />
<MoveBase goal="1;2;3"/>
<SaySomething message="mission completed!" />
</Sequence>
</ReactiveSequence>
</BehaviorTree>
</root>
期望 输出为:
--- 1st executeTick() ---
[ Battery: OK ]
Robot says: "mission started..."
[ MoveBase: STARTED ]. goal: x=1 y=2.0 theta=3.00
--- 2nd executeTick() ---
[ Battery: OK ]
[ MoveBase: FINISHED ]
--- 3rd executeTick() ---
[ Battery: OK ]
Robot says: "mission completed!"