ROS学习|Behavoir Tree(BT树)--c++实现

David小伟同学

已于 2022-07-14 10:45:07 修改

阅读量1.9k

点赞数

分类专栏： ros coding 文章标签：学习 c++ 开发语言

于 2022-05-18 15:52:22 首次发布

本文链接：https://blog.csdn.net/JeSuisDavid/article/details/124822944

版权

ros 同时被 2 个专栏收录

28 篇文章 3 订阅

订阅专栏

coding

22 篇文章 1 订阅

订阅专栏

系列文章目录

Behavoir Tree(BT树)–基本概念
 Behavoir Tree(BT树)–c++实现
 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!"