【行为树】基于BehaviorTree.CPP库的简单Demo实现

行为树简单Demo实现

在我的上篇博客行为树基本概念与BehaviorTree.CPP库在C++的代码实现中，我简要介绍了行为树的基本概念以及库的使用，由于本人做一些机器人开发相关的领域，深感状态机的使用繁琐以及容易导致逻辑混乱。为此，为了深入了解学习行为树的代码实现，我自己设计了一个小的Demo来强化对BehaviorTree.CPP库的学习与使用，本Demo中包含了本人最常用的实例化后的类函数调用作为行为树的叶节点以及通过函数指针创建行为树叶节点两种实现方式。

Demo概述

实现如下的项目目标:

• 机器人的初始坐标为(0,0)
• 机器人不知道目标坐标
• 机器人在x、y方向的移动步长为1
• 通过行为树控制机器人逐步移动到目标点

行为树构建

根据Demo任务描述，可以构建如图所示的流程图，为了尽可能模拟实践真实项目环境中的代码编写习惯，示例代码在节点创建方面将使用基于类的成员函数的方式实现。分析树的结构，可以发现其实只有5个叶节点，其余的都是中间节点

至此，可以使用XML语言构建出行为树的描述格式，具体如下：

<root BTCPP_format="4" >
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <Fallback name="x_Fallback">
                <isXArrived name="isXArrived"/>
                <moveX      name="moveX"/>
            </Fallback>
            <Fallback name="y_Fallback">
                <isYArrived name="isYArrived"/>
                <moveY      name="moveY"/>
            </Fallback>
            <PrintPosition name="PrintPosition"/>
        </Sequence>
    </BehaviorTree>
</root>

后续在代码中通过头文件中声明文本的方式实现

Robot类

class Robot{
    public:
        int x_step = 1;
        int y_step = 1;
        PointXY position = {0,0};
        BT::NodeStatus moveX(){
            std::cout << "Moving along the x axis: " << position.x << std::endl;
            position.x += x_step;
            return BT::NodeStatus::SUCCESS;
        };
        BT::NodeStatus moveY(){
            std::cout << "Moving along the y axis: " << position.y << std::endl;
            position.y += y_step;
            return BT::NodeStatus::SUCCESS;
        };
};

按照Demo要求，设计了一个简单的机器人类，每次可以在x、y方向移动一个固定的步长。

Obs类

class Obs{
    public:
        PointXY target = {3,3};
        void setTarget(int x, int y){
            target.x = x;
            target.y = y;
        };
        BT::NodeStatus isXArrived(int x_now){
            if(x_now < target.x){
                std::cout << "X not arrived" << std::endl;
                return BT::NodeStatus::FAILURE;
            }
            else{
                std::cout << "X arrived" << std::endl;
                return BT::NodeStatus::SUCCESS;
            }
        };
        BT::NodeStatus isYArrived(int y_now){
            if(y_now < target.y){
                std::cout << "Y not arrived" << std::endl;
                return BT::NodeStatus::FAILURE;
            }
            else{
                std::cout << "Y arrived" << std::endl;
                return BT::NodeStatus::SUCCESS;
            }
        };
};

设计了一个观测器类，仅用来存储目标位置，并用来判断是否到达目标点

完成了机器人类与观测器类后，就可以通过循环调用来实现指导机器人的移动：

while (true)
{
    // * 树运行
    tree.tickOnce();
    std::cout << "------------------" << std::endl;
}

在主函数中将target设置为(2,5)，终端运行可以看到：

X not arrived
Moving along the x axis: 0
Y not arrived
Moving along the y axis: 0
Current position: 1, 1
------------------
X not arrived
Moving along the x axis: 1
Y not arrived
Moving along the y axis: 1
Current position: 2, 2
------------------
X arrived
Y not arrived
Moving along the y axis: 2
Current position: 2, 3
------------------
X arrived
Y not arrived
Moving along the y axis: 3
Current position: 2, 4
------------------
X arrived
Y not arrived
Moving along the y axis: 4
Current position: 2, 5
------------------

成功通过行为树完成了对机器人移动的引导与控制。

完整代码

moveRobot.cpp

#include "moveRobot.hpp"

int main(){
    BT::BehaviorTreeFactory factory;
    Robot myRobot;
    Obs myObs;
    myObs.setTarget(2,5);
    // ? `std::bind` 绑定了成员函数 `Obs::isXArrived` 到对象 `myObs`，并且将 `myRobot.position.x` 作为该成员函数的参数。
    // * 注意使用std::ref生成一个参考，否则对于myObs.isXArrived函数，myRobot.position.x的值不会更新
    // ? 这意味着当新生成的可调用对象被调用时，它会调用 `myObs.isXArrived(myRobot.position.x)`
    factory.registerSimpleCondition("isXArrived", std::bind(&Obs::isXArrived, &myObs, std::ref(myRobot.position.x)));
    factory.registerSimpleCondition("isYArrived", std::bind(&Obs::isYArrived, &myObs, std::ref(myRobot.position.y)));
    factory.registerSimpleAction("moveX",std::bind(&Robot::moveX, &myRobot));
    factory.registerSimpleAction("moveY",std::bind(&Robot::moveY, &myRobot));
    factory.registerSimpleAction("PrintPosition",std::bind(PrintPosition, &myRobot));
    // todo: 通过xml文本创建一个行为树
    auto tree = factory.createTreeFromText(xml_text);
    
    while (true)
    {
        // * 树运行
        tree.tickOnce();
        std::cout << "------------------" << std::endl;
    }
}

moveRobot.hpp

#ifndef MOVEROBOT_HPP
#define MOVEROBOT_HPP

#include "behaviortree_cpp/bt_factory.h"
// * nodes
#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/condition_node.h"
#include "behaviortree_cpp/control_node.h"
#include "behaviortree_cpp/decorator_node.h"

static const char *xml_text = R"(
    <root BTCPP_format="4" >
        <BehaviorTree ID="MainTree">
            <Sequence name="root_sequence">
                <Fallback name="x_Fallback">
                    <isXArrived name="isXArrived"/>
                    <moveX      name="moveX"/>
                </Fallback>
                <Fallback name="y_Fallback">
                    <isYArrived name="isYArrived"/>
                    <moveY      name="moveY"/>
                </Fallback>
                <PrintPosition name="PrintPosition"/>
            </Sequence>
        </BehaviorTree>
    </root>
)";

struct PointXY
{
    int x;
    int y;
};


class Robot{
    public:
        int x_step = 1;
        int y_step = 1;
        PointXY position = {0,0};
        BT::NodeStatus moveX(){
            std::cout << "Moving along the x axis: " << position.x << std::endl;
            position.x += x_step;
            return BT::NodeStatus::SUCCESS;
        };
        BT::NodeStatus moveY(){
            std::cout << "Moving along the y axis: " << position.y << std::endl;
            position.y += y_step;
            return BT::NodeStatus::SUCCESS;
        };
};

class Obs{
    public:
        PointXY target = {3,3};
        void setTarget(int x, int y){
            target.x = x;
            target.y = y;
        };
        BT::NodeStatus isXArrived(int x_now){
            if(x_now < target.x){
                std::cout << "X not arrived" << std::endl;
                return BT::NodeStatus::FAILURE;
            }
            else{
                std::cout << "X arrived" << std::endl;
                return BT::NodeStatus::SUCCESS;
            }
        };
        BT::NodeStatus isYArrived(int y_now){
            if(y_now < target.y){
                std::cout << "Y not arrived" << std::endl;
                return BT::NodeStatus::FAILURE;
            }
            else{
                std::cout << "Y arrived" << std::endl;
                return BT::NodeStatus::SUCCESS;
            }
        };
};

BT::NodeStatus PrintPosition(Robot* robot){
    std::cout << "Current position: " << robot->position.x << ", " << robot->position.y << std::endl;
    return BT::NodeStatus::SUCCESS;
}
#endif

1