行为树 — [6] 获取child的状态变化

有时执行Sequence时想要知道每个child的状态变化，这个时候就需要添加回调函数来实现。

一 接口

child节点的类型是BT::TreeNode，里面有个接口如下，

/**
     * @brief subscribeToStatusChange is used to attach a callback to a status change.
     * When StatusChangeSubscriber goes out of scope (it is a shared_ptr) the callback
     * is unsubscribed automatically.
     *
     * @param callback The callback to be execute when status change.
     *
     * @return the subscriber handle.
     */
    StatusChangeSubscriber subscribeToStatusChange(StatusChangeCallback callback);

回调函数类型是StatusChangeCallback，具体定义如下，

最终指向Signal::CallableFunction，由于Signal是个模板类，所以最终StatusChangeCallback的类型是

void subscribeCallback(BT::TimePoint tp, const BT::TreeNode& node, BT::NodeStatus prev, BT::NodeStatus curr);

有点绕

二 思路

导入xml文件后会生成树，类型是BT::Tree，

BT::Tree tree = factory.createTreeFromText(xml_text);

在BT::Tree里包含了所有node（类型是BT::TreeNode）的指针，

所以可以通过这个nodes来添加回调函数，

    BT::Tree tree = factory.createTreeFromText(xml_text);

    std::vector<BT::TreeNode::StatusChangeSubscriber> subscribers_;

    for (const BT::TreeNode::Ptr& pNode : tree.nodes)
    {
        subscribers_.push_back(pNode->subscribeToStatusChange(std::move(subscribeCallback)));
    }

注意，这里的subscribers_是必须的，否则这个回调会被释放…

三 代码

#include <vector>
#include <iostream>
#include "behaviortree_cpp_v3/bt_factory.h"


static const char* xml_text = R"(

<root main_tree_to_execute = "MainTree" >

     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <OpenDoor   name="open_door_of_house"/>
            <EnterHouse name="enter_house"/>
            <CloseDoor  name="close_door_of_house"/>
        </Sequence>
     </BehaviorTree>

 </root>
 )";


class OpenDoorImpl : public BT::SyncActionNode
{
  public:
    OpenDoorImpl(const std::string& name) :
        BT::SyncActionNode(name, {})
    {
    }

    // You must override the virtual function tick()
    BT::NodeStatus tick() override
    {
        std::cout << "Door is opened" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

class EnterHouseImpl : public BT::SyncActionNode
{
  public:
    EnterHouseImpl(const std::string& name) :
        BT::SyncActionNode(name, {})
    {
    }

    // You must override the virtual function tick()
    BT::NodeStatus tick() override
    {
        std::cout << "Enter house" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};

class CloseDoorImpl : public BT::SyncActionNode
{
  public:
    CloseDoorImpl(const std::string& name) :
        BT::SyncActionNode(name, {})
    {
    }

    // You must override the virtual function tick()
    BT::NodeStatus tick() override
    {
        std::cout << "Close door" << std::endl;
        return BT::NodeStatus::SUCCESS;
    }
};



void subscribeCallback(BT::TimePoint tp, const BT::TreeNode& node, BT::NodeStatus prev, BT::NodeStatus curr)
{
    std::cout << node.name() << ": prev status " << prev << ", curr status " << curr << "\\n";
}

int main()
{
    // We use the BehaviorTreeFactory to register our custom nodes
    BT::BehaviorTreeFactory factory;


    factory.registerNodeType<OpenDoorImpl>("OpenDoor");
    factory.registerNodeType<EnterHouseImpl>("EnterHouse");
    factory.registerNodeType<CloseDoorImpl>("CloseDoor");


    BT::Tree tree = factory.createTreeFromText(xml_text);

    std::vector<BT::TreeNode::StatusChangeSubscriber> subscribers_;

    for (const BT::TreeNode::Ptr& pNode : tree.nodes)
    {
        subscribers_.push_back(pNode->subscribeToStatusChange(std::move(subscribeCallback)));
    }
    

    tree.tickRoot();

    return 0;
}

这里定义的回调函数subscribeCallback，里面打印了节点名称和状态变化。

编译运行后执行，输出如下，

节点有4个，一个是父节点root_sequence，另外三个是父节点的children

tick之后，父节点状态由IDLE变成RUNNING，三个children的状态由IDLE变成SUCCESS，执行完毕后，三个children的状态又变回IDLE，而父节点的状态先从RUNNING变成SUCCESS，最后变成IDLE，这个状态变化很合理。