行为树 — [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,这个状态变化很合理。

© 版权声明
THE END
喜欢就支持一下吧
点赞461 分享
评论 抢沙发
头像
欢迎您留下宝贵的见解!
提交
头像

昵称

取消
昵称表情代码图片

    暂无评论内容