本文已参与「新人创造礼」活动，一同开启掘金创造之路。

创立行为树

行为树，类似于状态机，过是一种在正确的时刻在正确的条件下调用回调的机制。 此外，咱们将替换运用“callback”和“tick”这两个词。

简单Beahvoir实例

在这儿刺进图片描述

任务调度学习|Behavoir Tree(BT树)--c++实现

初始(推荐)的创立树节点的方式是继承：

// 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机制来处理数据流。接下来哦咱们创立下面这个树：
    任务调度学习|Behavoir Tree(BT树)--c++实现

输入接口

一个有效的输入能够是：

  • 能被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!"