Behavior-tree模块化编程简明教程

写在前面的话

本文立足于行为树的《user manual》，将对行为树模块化编程做一些较为细致的介绍。《user manual》可从此处下载获得。
且由于《user manual》较为简洁，本文将对于行为树的基础概念以及模块化编程等方面做一些额外的补充。

Behavior-tree的模块化编程

行为树（Behavior trees）优于有限状态机(FSMs)的方面在于行为树引入了模块化编程以及具有反馈性质的ticks()函数。而组成行为树的主要模块结构可分为：控制流结点（control flow nodes）以及执行结点（execution nodes）。其中控制流结点主要包括：Fallback, Sequence, Parallel, 以及 Decorator;执行结点主要包括：condition以及action。行为树实现其特殊功能主要就是通过根节点->控制流结点->执行结点，这样的模式来实现其功能。
下面将就这些结点做一些简单的介绍。

（一）Behavior-tree基本模块介绍

Tick

在执行行为树时，行为树的控制循环的基础是tick。tick将从行为树的根节点(root node)出发，不断向下，最终到达叶子结点（leaf nodes），而叶子结点通常由action nodes 与 condition nodes 组成。一旦tick到达了上述的两种结点，将激活该结点，使其行驶其功能，并且将视结点的运行情况返回 SUCCESS , FAILURE , RUNNING 的其中一种状态。返回的状态将沿着行为树逆向上升，最终到达根节点。同时，由于结点类型的不同，对于tick的处理与反馈也不尽相同。要注意的是，如果一个正在运行的结点将被处理为不再接收到tick，它必须被停止。我们将通过 halt 程序来使其强制停止。下面，我们将阐述不同结点对tick的不同处理方式以及对不同返回方式的处理，即每个结点的特性。

Control flow nodes

1. Fallback
   Fallback 节点用于查找和执行不失败的第一个子节点。Fallback将从左到右依次运行其子节点，直到其子节点返回SUCCESS或者RUNNING。Fallback结点将立即返回SUCCESS或RUNNING至其父结点，且不会tick下一个子节点。当且仅当其所有的子节点均返回FAILURE时，Fallback结点才返回FAILURE。
2. Sequence
   Sequence 用于查找和执行尚未成功的第一个子节点。Sequence将从左到右依次运行其子节点，直到其子节点返回FAILURE或者RUNNING。Sequence结点将立即返回FAILURE或RUNNING至其父节点，且不会tick下一个子节点。当且仅当其所有的子节点均返回SUCCESS时，Fallback结点才返回SUCCESS。
3. Parallel
   Parallel结点将同时 tick其所有的子节点，当其中有M个子节点返回SUCCESS时，Parallel返回SUCCESS；当有N-M+1个子节点返回FAILURE时，Parallel返回FAILURE；除此之外将返回RUNNING。其中，N为其子结点个数，M<=N且为用户自定义数。
4. Decorator
   Decorator结点将根据用户定义的策略操作其子节点的返回状态。在Behavior-tree.cpp中所给的Decorator类型为：Decorator retry（重新尝试运行FAILURE的子结点）以及Decorator Negation（逆转子节点所传回的SUCCESS或FAILURE）。

Execution nodes

1. Action
   Action结点执行动作，如果操作完成，则返回SUCCESS，如果无法完成则返回FAILURE，如果正在完成则RUNNING。
2. Condition
   Condition节点将判断是否满足了用户定义的条件C。Condition结点理论上来说可以被视作Action结点的一个子集，但是在行为树系统中依然加入了Condition结点，提高了程序的可读性。

Node with Memory

在Execution nodes与Control flow nodes之外还存在着一类带有“记忆”的结点。带有记忆的结点在执行时无需从头开始依次执行子节点，而是从上一次执行中断处继续依照从左至右的顺序依次执行，从而解决了Sequence与Fallback结点必须此次执行其前馈结点的弊端。此类结点主要包括SequenceMem与FallbackMem两类。

（二）Behavior-tree基本模块编程实例

在对各个模块编程前，首先要将行为树的结构建立起来。而建立一棵行为树与二叉树在思路上极为相似，即先声明创建新的结点，之后再使用指针的方式将其传递入树,，并在最后调用Execute函数激活所要运行的结点。但不同的是由于behavior_tree.h库的存在，使得行为树的创建较为简洁，如BT::FallbackNode()函数等。使用了behavior_tree.h库，建立一棵行为树就变得较为方便了。关于behavior_tree.h库具体的使用方法可以查看ROS官方所给出的代码。对于编译后生成这些.h文件的 .cpp文件 也可以从这里查看。
下面给出了古月居中小明工坊中创建一棵行为树的具体案例：

#include 

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "BehaviorTree");
    try
    {
        int TickPeriod_milliseconds = 200;

        BT::ROSCondition* have_enemy = new BT::ROSCondition("have_enemy");
        BT::ROSAction* nav_enemy = new BT::ROSAction("nav_enemy");
        BT::ROSAction* patrol = new BT::ROSAction("patrol");
        
        BT::FallbackNode* guard = new BT::FallbackNode("guard");
        BT::SequenceNode* attack = new BT::SequenceNode("attack");

        guard->AddChild(attack);
        guard->AddChild(patrol);
        attack->AddChild(have_enemy);
        attack->AddChild(nav_enemy);

        Execute(guard, TickPeriod_milliseconds);  
    }
    catch (BT::BehaviorTreeException& Exception)
    {
        std::cout << Exception.what() << std::endl;
    }

    return 0;
}

在使用了行为树库之后，对于其中的控制流结点如Sequence，Fallback等的建立就较为简单，但行动结点，即Action,Condition等的功能的实现依然需要额外编程实现。
1.Action
文章中所给的github文件中的实例较为简略，在下面给出一个通过C++中类的方式完成Action功能的模板。
程序与一般的Action的创建类似。关于ACTION结点的创建，可以参考古月居的介绍。行为树编程的不同之处主要在于其中的BTFeedback以及BTResult。对于其中代码详细分析的可以看我之前的博客，但其中的注释也很完备了，有兴趣可以自己做一些修改。对于常规的Action编程，通过修改回调函数（execute_callback） 即可实现自定义的动作。
下面给出的是Action Server的编程实例，此处发布的消息类型为behavior_tree_core::BTAction：

#include 
#include 
#include 

#include 


enum Status {RUNNING, SUCCESS, FAILURE};  // BT return status


class BTAction
{
protected:
    ros::NodeHandle nh_;
    // NodeHandle instance must be created before this line. Otherwise strange error may occur.
    actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
    std::string action_name_;
    // create messages that are used to published feedback/result
    behavior_tree_core::BTFeedback feedback_;  // action feedback (SUCCESS, FAILURE)
    behavior_tree_core::BTResult result_;  // action feedback  (same as feedback for us)


public:
    explicit BTAction(std::string name) :
        as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
        action_name_(name)
    {
        // Starts the action server
        as_.start();
    }

    ~BTAction(void)
    {}

    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)//编辑次函数即可
    {
        // publish info to the console for the user
        ROS_INFO("Starting Action");

        // start executing the action
        int i = 0;
        while (i < 5)
        {
            // check that preempt has not been requested by the client
            if (as_.isPreemptRequested())
            {
                ROS_INFO("Action Halted");

                // set the action state to preempted
                as_.setPreempted();
                break;
            }
            ROS_INFO("Executing Action");

            ros::Duration(0.5).sleep();  // waiting for 0.5 seconds
            i++;
        }

        if (i == 5)
        {
            set_status(SUCCESS);
        }
    }


    // returns the status to the client (Behavior Tree)
    void set_status(int status)
    {
        // Set The feedback and result of BT.action
        feedback_.status = status;
        result_.status = feedback_.status;
        // publish the feedback
        as_.publishFeedback(feedback_);
        // setSucceeded means that it has finished the action (it has returned SUCCESS or FAILURE).
        as_.setSucceeded(result_);

        switch (status)  // Print for convenience
        {
        case SUCCESS:
            ROS_INFO("Action %s Succeeded", ros::this_node::getName().c_str() );
            break;
        case FAILURE:
            ROS_INFO("Action %s Failed", ros::this_node::getName().c_str() );
            break;
        default:
            break;
        }
    }
};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "action");
    ROS_INFO(" Enum: %d", RUNNING);
    ROS_INFO(" Action Ready for Ticks");
    BTAction bt_action(ros::this_node::getName());
    ros::spin();

    return 0;
}

Action Client的编程可以依照Action编程模板对进行适当的修改，此处Client的主要功能就是对服务器发送一个请求Goal，再处理反馈的Goal的状态以判断动作是否完成。
2. Condition
Condition作为Action的一个子集，在程序的总体架构上还是十分相似的，掌握了Action，Condition应该易如反掌。其中唯一不同的地方在于Action使用的是While循环，而Condition使用的是条件判断if。与上文类似，修改if程序块即可完成自定义。

结语

通过对行为树最基本控制模块与运行模块基本概念的理解，以及对Action以及Condition模块的编程实现，完成一些基本的行为树功能实现应该是可以了。但行为树并不是一个孤立的部分，经过了多年的发展已经与PAA，机器学习等方面高度融合了。对于行为树的扩展插件可以阅读 《Behavior trees in ROS and AI》 中的相关部分进行更深程度的学习。