开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> C++知识库 -> ROS学习|Behavoir Tree(BT树)--c++实现 -> 正文阅读

[C++知识库]ROS学习|Behavoir Tree(BT树)--c++实现

创建行为树

行为树，类似于状态机，过是一种在正确的时间在正确的条件下调用回调的机制。
此外，我们将交替使用“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 第一次和第二次返回 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!"