BehaviorTree + Groot 在ros中的运用

 一、BehaviorTree + Groot 在ros中的运用的参考资料

1、BehaviorTree.cpp开源库

地址：github.com/BehiaviorTree/BehaviorTree.CPP

提供BehaviorTree框架，提供examples学习，但是相对比较零碎，需要对BehaviorTree有一个初步了解

2、BehaviorTree主要概念、API与教程

地址：BehaviorTree.CPP

对开源库有一些讲解，相对清晰

3、古月居小明工坊的 《ROS实验 | 行为树实现机器人智能》
网页链接：ROS实验 | 行为树实现机器人智能 - 古月居

二、BehaviorTree + Groot 在ros中的运用基础

1、运行过程

1. 一个名为“tick”的信号被发送到树的根部，并在树中传播，直到到达叶节点。

2. 接收tick信号的树节点执行其callback。此回调必须返回。

   SUCCESS,FAILURE or RUNNING

3. 如果一个TreeNode有一个或多个子节点，它将根据其状态、外部参数或前一个同级节点的结果来顺序tick他们。

4. LeafNode，即那些没有任何子节点的TreeNode，是实际的命令，即行为树与系统其余部分交互的地方。Actions节点是最常见的叶节点类型。

2、tick的运行原理

Sequence 处在运行中, tick 传到 DetectObject, 返回SUCCESS，tick传到GraspObject,返回SUCCESS，子节点全部完成，父节点Sequence状态变成SUCCESS。

其余详细资料请参考提供的参考资料

三、调试工具

1、groot

Groot是与BehaviorTree.CPP搭配使用的工具，分为Editor、Monitor、Log Replay 3种模式，具有行为树编辑、状态监控、历史log回放等功能。

指南：Groot - Interacting with Behavior Trees — Navigation 2 1.0.0 documentation

2、StdCoutLogger

作用：在终端打印行为树中的节点执行状态变化。

代码仅需在加载tree后添加StdCoutLogger类的1个实例（且只能有1个实例），运行效果如下：

StdCoutLogger logger_cout(tree);

四、实践

        理论的东西看起来容易，但离实践还有一定距离，接下来用一个实践小项目来对BehaviorTree + Groot + Ros进行演示，项目题目来源于古月居，实现一个巡逻的小乌龟的游戏

Groot下的行为树如下图

运行过程

1、attack子节点haveEnemy判断周围是否有敌人。

2、若没有敌人，Ation节点moveToEnemy将不被触发，守卫将执行patrol节点，对区域进行搜索。

3、若找到敌人，moveToEnemy节点将被触发，守卫向敌人前进。

1、小乌龟生成与tf发布

此部分不再详细描述,代码如下

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    static tf::TransformBroadcaster br;
    // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    // 发布坐标变换
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };
    turtle_name = argv[1];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    ros::spin();
    return 0;
};

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // tf广播器
    static tf::TransformBroadcaster br;
    // 根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // 发布坐标变换
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

    // 发布固定点坐标
    tf::Transform transform_a;
    transform_a.setOrigin(tf::Vector3(1.0, 6.0, 0.0));
    tf::Quaternion q_a;
    q_a.setRPY(0.0, 0.0, 0.0);
    transform_a.setRotation(q_a);
    br.sendTransform(tf::StampedTransform(transform_a, ros::Time::now(), "world", "point_a"));
    
    tf::Transform transform_b;
    transform_b.setOrigin(tf::Vector3(10.0, 6.0, 0.0));
    tf::Quaternion q_b;
    q_b.setRPY(0.0, 0.0, 0.0);
    transform_b.setRotation(q_b);
    br.sendTransform(tf::StampedTransform(transform_b, ros::Time::now(), "world", "point_b"));
}

int main(int argc, char** argv)
{
    // 初始化节点
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument"); 
        return -1;
    };
    turtle_name = argv[1];

    // 订阅乌龟的pose信息
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
    
    // 通过服务调用，产生第二只乌龟turtle2
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
    node.serviceClient("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);
    
    ros::spin();

    return 0;
};

2、Action节点构建

        节点构建以静态库的方式进行构建，构建的类需要对behaviorTree的类进行继承，巡逻节点类构建如下

class BTActionPatrol : public BT::AsyncActionNode
{
protected:
    ros::NodeHandle nh_;
    // 定义turtle2的速度控制发布器
    ros::Publisher turtle_vel =
    nh_.advertise("turtle2/cmd_vel", 10);


public:
    BTActionPatrol(const std::string& name, const BT::NodeConfiguration& config)
      :BT:: AsyncActionNode(name, config)
    {
    }

    ~BTActionPatrol(void)
    {}

    BT::NodeStatus tick() override;

    static BT::PortsList providedPorts()
    {
        return{ BT::InputPort("message") };
    }

    virtual void halt() override;
};

publisher与providedPorts定义本不应该放头文件里面。

在构建节点时需要注意重写 tick 与 halt函数，需要提供static BT::PortsList providedPorts接口函数

重写的tick函数如下，主要操作在这里实现。

BT::NodeStatus BTActionPatrol::tick()
{
    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;
    }
        
    // 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度
    // 并发布速度控制指令，使turtle2向turtle1移动
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 1.0 * atan2(transform.getOrigin().y(),
                                    transform.getOrigin().x());
//      vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
//                                          pow(transform.getOrigin().y(), 2));
    vel_msg.linear.x = 0.8;
                                                                        
    turtle_vel.publish(vel_msg);
    ROS_INFO("Action nav_b is successful!");
    return BT::NodeStatus::SUCCESS;
    
}

moveToEnemy节点与上一节点一样，继承Action类，判断是否有enemy的节点为条件节点，继承Condition类，详细代码将不在此处详写。

3、构建树

使用xml文件构建行为树，代码如下

载入节点

BehaviorTreeFactory factory;

factory.registerNodeType("moveToEnemy");
factory.registerNodeType("patrol");
factory.registerNodeType("haveEnemy");

auto tree = factory.createTreeFromText(xml_text);

添加Groot调试

PublisherZMQ publisher_zmq(tree);

4、修改CmakeLists

构建静态库

add_library(sample_nodes STATIC
  src/treeNode/action_nav_enemy.cpp
  src/treeNode/action_patrol.cpp
  src/treeNode/condition_have_enemy.cpp
    )

target_link_libraries(sample_nodes 
${catkin_LIBRARIES}
BT::behaviortree_cpp_v3)

 添加可执行文件

add_executable(guard_robot_tree src/tree/guard_robot_tree.cpp)
target_link_libraries(
  guard_robot_tree
  ${catkin_LIBRARIES}
  BT::behaviortree_cpp_v3
  sample_nodes
  # /opt/ros/melodic/lib/librosconsole.so
  # /opt/ros/melodic/lib/libroscpp_serialization.so
)

5、运行效果

五、总结

对BehaviorTree进行了学习，还有很多功能有待开发，如节点间的通讯，由可以使用ros话题通讯代替，暂时不需要，后续有新内容还会进行更新，对于文章中的错误希望大家一起讨论。

六、参考

BT9：各种调试工具介绍_linxigjs的博客-CSDN博客

ROS实验 | 行为树实现机器人智能 - 古月居

Behavior-tree 在ROS中的应用（入门）_bug有点多的博客-CSDN博客