Behavior tree 编程实战

写在前面的话

本文主要在前文的基础上，通过《Behavior trees in ROS and AI》以及小明工坊中所提供的部分代码进行实战化分析。
此处给出两处代码地址以及原文：
古月居小明工坊：https://www.guyuehome.com/5311
Behavior trees in ROS and AI:
https://behaviortree.github.io/BehaviorTree.CPP/tutorial_01_first_tree/

如何建立一棵行为树

行为树枝干的编辑

建立行为树的方法主要有两种，一种是静态编译，一种是XML动态生成，这里主要剖析的是静态编译的方法。
建立一棵行为树的最基本的架构在
ROS-BehaviorTree/behavior_tree_core/src/gtest/external_ros_nodes_test.cpp
中得到了详细的体现：

#include <behavior_tree.h>

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


        BT::ROSAction* action = new BT::ROSAction("action");
        BT::NegationNode* decorator = new BT::NegationNode("decorator");
        BT::ROSCondition* condition = new BT::ROSCondition("condition");


        BT:: SequenceNode* sequence1 = new BT::SequenceNode("seq1");

        sequence1->AddChild(decorator);
        decorator->AddChild(condition);
        sequence1->AddChild(action);

        Execute(sequence1, TickPeriod_milliseconds);  // from BehaviorTree.cpp
    }
    catch (BT::BehaviorTreeException& Exception)
    {
        std::cout << Exception.what() << std::endl;
    }

    return 0;
}

代码心得：
behavior_tree.h 文件为mit所开发的一款关于行为树创建的库，可以通过以下的链接获得：
https://github.com/BehaviorTree/BehaviorTree.CPP
整个枝干的创建过程基本上与C++中基础的二叉树的创建类似，通过new来生成新的结点以及定义指针来完成结点间对应关系的构建。但不同的是行为树结合了ROS的特点，可以通过结点的调用将叶子进行单独的编写，从而起到了模块化的作用。
如果代码正确而所观察到的rqt_graph产生了错误，即发生了节点间相互作用失败的案例，可以尝试通过检查Cmake文件得以解决。
并在此处推荐ROS编译的常用软件Roboware，可以自动生成Cmake.list以及pakeage.xml文件，还是比较方便的。
在创建完行为树的基本枝干后就进入到了行为树叶子结点的编辑中。

行为树叶子结点的编辑

ROS-BehaviorTree功能包中并没有对叶子结点进行具体功能的实现，只是在ROS-BehaviorTree/behavior_tree_core/src/路径下给出了一些叶子结点编译的模板程序。
而在古月居小明工坊的《ROS实验 | 行为树实现机器人智能》一文中给出了关于行为树真正实现其功能的具体的案例。
我们以ROS-Behavior-Tree/behavior_tree_leaves/nodes/action_patrol.cpp为例，现在把代码贴在下面：

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <behavior_tree_core/BTAction.h>
#include <string>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>


enum Status 
{
    RUNNING, SUCCESS, FAILURE
}; 


class BTAction
{
protected:
    ros::NodeHandle nh_;
    actionlib::SimpleActionServer<behavior_tree_core::BTAction> as_;
    std::string action_name_;
    //定义发布的反馈和结果
    behavior_tree_core::BTFeedback feedback_;  //action 反馈，SUCCESS,FAILURE
    behavior_tree_core::BTResult result_;  

    ros::Publisher turtle_vel = nh_.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);



public:
//构造函数，boost参数绑定
    explicit BTAction(std::string name) :
        as_(nh_, name, boost::bind(&BTAction::execute_callback, this, _1), false),
        action_name_(name)
    {
        as_.start();
    }

    ~BTAction(){}

    void execute_callback(const behavior_tree_core::BTGoalConstPtr &goal)
    {
        tf::TransformListener listener;
        tf::StampedTransform transform_a, transform_b, transform;
        listener.waitForTransform("/turtle2", "/point_a", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/turtle2", "/point_a", ros::Time(0), transform_a);
        listener.waitForTransform("/turtle2", "/point_b", ros::Time(0), ros::Duration(3.0));
        listener.lookupTransform("/turtle2", "/point_b", ros::Time(0), transform_b);
        double distance_a, distance_b;
        distance_a = sqrt(pow(transform_a.getOrigin().x(), 2) + pow(transform_a.getOrigin().y(), 2));
        distance_b = sqrt(pow(transform_b.getOrigin().x(), 2) + pow(transform_b.getOrigin().y(), 2));
         if (nh_.hasParam("/goal_point"))
        {
            if(distance_a < 0.5) 
            {
                nh_.setParam("/goal_point", "b");
                ROS_INFO("Change direction to b");
            }
            else if(distance_b < 0.5)
            {
                nh_.setParam("/goal_point", "a");
                ROS_INFO("Change direction to b");
            }
        }
        else
        {
            nh_.setParam("/goal_point", "a");
        }
        
        std::string direction;
        nh_.getParam("/goal_point", direction);
        
        if(direction == "a")
        {
            ROS_INFO("Nav to a");
            transform = transform_a;
        }
        else if(direction == "b")
        {
            ROS_INFO("Nav to b");
            transform = transform_b;
        }
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 1.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.8;
                                                                            
        turtle_vel.publish(vel_msg);

        set_status(SUCCESS);
        ROS_INFO("Action nav_b is successful!");
    }


    void set_status(int status)
    {
        feedback_.status = status;
        result_.status = feedback_.status;
        as_.publishFeedback(feedback_);
        as_.setSucceeded(result_);

        switch (status) 
        {
        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;
}

这段代码主要是在小海龟范围内没有敌情时，在固定的两个点间以拟好的速度进行巡逻。
比较重要的是首先用了enum枚举出了行为树的三种反馈模式:
SUCCESS,FAILURE,RUNNING.
接下来使用了C++中较为方便的类以及析构函数，将参数较为方便的传递给内部函数以及变量，有助于实现程序的模块化进行。
接下来比较重要的就是这两行：

 behavior_tree_core::BTFeedback feedback_;  //action 反馈，SUCCESS,FAILURE
 behavior_tree_core::BTResult result_;  

这两行代码主要是定义了行为树所特有的反馈，以及结果（对于其实现的功能可以理解为结果，但实际上并不是结果那么简单）。
以及对于反馈与结果具体内容的实现，可以在函数 set_status中具体体现：

  void set_status(int status)
    {
        feedback_.status = status;
        result_.status = feedback_.status;
        as_.publishFeedback(feedback_);
        as_.setSucceeded(result_);

        switch (status) 
        {
        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;
        }
    }
};

行为树就是想这子叶反馈SUCCESS，FAILURE,或者RUNNING，将子叶的工作状况反馈给上一级，从而实现不同情况的判断，动作执行的时机等，使其具有了一定的智能。
在回调这一块，我们在
ROS-Behavior-Tree/behavior_tree_leaves/nodes/condition_have_enemy.cpp
文件中可以得到更具体的展示：

	    distance = sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));
        nh_.setParam("/produc_status",distance);
        if(distance<2)
        {
            ROS_INFO("Find enemy!");
            set_status(SUCCESS);
        }
        else
        {
            ROS_INFO("Everything is ok");
            set_status(FAILURE);
        }
    }

在这里我们可以更加清晰的看到SUCCESS与FAILURE在行为树状态结点实现其功能时的重要性，从而可以一窥行为树实现其功能中tick()，即回调，的重要性。