在ROS中使用行为树

在ROS中使用行为树

0、 学习本教程的前提

note : 使用本教程前，建议先弄懂 behaviorTree的使用 ，ros server 的使用， ros action 的使用。

• ros server 的教程
• ros action 的教程
• BehaviorTree 教程
• BehaviorTree 源码

下载参考我提供的demo：demo链接

1、 behaviorTree 库安装(ROS使用下的安装方式)

  sudo apt-get install ros-$ROS_DISTRO-behaviortree-cpp-v3

2、 处理 behaviortree 库并准备好 service 和 ros action

以本教程的 demo 为例子，讲解如何在ROS中使用行为树

2.1 对ROS 的 service 和 action 进行封装，使其在 behaviortree 上使用更加方便。

库的作者已经给出给出了一个demo ，直接复制就行，参考 官方教程， 可以对照参考我的教程和这份官方教程，理解更深。

步骤：将behaviortree_ros文件夹复制到 include 目录下，可以看到里面有bt_action_node.h 和 bt_service_node.h , 这两个头文件就是对 action 和 service 的 封装。

2.2 创建一个 service 和一个 action

1. 在srv 文件夹中创建 服务 do_what.srv
2. 在 action 文件夹中创建 action Shoot.action

do_what.srv 这个服务的逻辑，通过剩余血量和子弹数量，判断机器人下一步该做什么

int32 blood
int32 remain_bullets
---
int32 dowhat

Shoot.action 这个 action 的逻辑，开枪动作，action过程中反馈正在开第几枪，action 结束后，返回是否打完子弹的结果

# Define the goal
int32 Number_of_shots  # 一共打几枪
---
# Define the result
string finish  # 打完没
---
# Define a feedback message
int32 Which_shot  # 开到第几枪了

2.3 创建与决策树相关的数据类型

复制msg文件夹进来就行

2.4 修改package.xml文件

<package format="2">
  <name>ros_behaviortree_demoname>
  <version>0.0.0version>
  <description>The ros_behaviortree_demo packagedescription>

  
  
  
  <maintainer email="[email protected]">liaomaintainer>

  <license>TODOlicense>


  <buildtool_depend>catkinbuildtool_depend>

  <build_depend>roscppbuild_depend>
  <build_depend>std_msgsbuild_depend>
  <build_depend>behaviortree_cpp_v3build_depend>
  <build_depend>actionlibbuild_depend>
  <build_depend>actionlib_msgsbuild_depend>
  <build_depend>message_generationbuild_depend>
  <build_depend>genmsgbuild_depend>

  <build_export_depend>roscppbuild_export_depend>
  <build_export_depend>actionlibbuild_export_depend>
  <build_export_depend>actionlib_msgsbuild_export_depend>
  <build_export_depend>std_msgsbuild_export_depend>
  <build_export_depend>genmsgbuild_export_depend>

  <exec_depend>roscppexec_depend>
  <exec_depend>std_msgsexec_depend>
  <exec_depend>behaviortree_cpp_v3exec_depend>
  <exec_depend>actionlibexec_depend>
  <exec_depend>actionlib_msgsexec_depend>
  <exec_depend>message_generationexec_depend>
  <exec_depend>genmsgexec_depend>


  
  <export>
    

  export>
package>

2.5 编写 cmakeList.txt 文件，并编译生成相关代码

note: 这一步做4件重要的事

• 生成 do_whta service 相关代码
• 生成 Shoot action 相关代码
• 生成 与行为树相关的数据类型
• 包含行为树库的路径

cmake_minimum_required(VERSION 3.0.2)
project(ros_behaviortree_demo)

set(CMAKE_CXX_STANDARD 14)

set(ROS_DEPENDENCIES
        roscpp std_msgs
        behaviortree_cpp_v3
        actionlib_msgs
        actionlib
        message_generation
        genmsg
        )

# 包含行为树库的路径，同时还包含了创建action 和 service 相关的库
find_package(catkin REQUIRED COMPONENTS ${ROS_DEPENDENCIES})


# 生成与行为树相关的数据类型
add_message_files(
        FILES
        BehaviorTree.msg
        NodeParameter.msg
        NodeStatus.msg
        StatusChange.msg
        StatusChangeLog.msg
        TreeNode.msg
)


# 生成 do_whta service 相关代码
add_service_files(
        FILES
        do_what.srv
)

# 生成 Shoot action 相关代码
add_action_files(
        DIRECTORY action
        FILES Shoot.action
)

# Generate added messages and services with any dependencies listed here
generate_messages(
        DEPENDENCIES
        std_msgs  # Or other packages containing msgs
        actionlib_msgs
)


catkin_package(
        INCLUDE_DIRS include
        LIBRARIES ros_behaviortree_demo
        CATKIN_DEPENDS ${ROS_DEPENDENCIES}
        #  DEPENDS system_lib
)

include_directories(
        include
        ${catkin_INCLUDE_DIRS}
)

编译

catkin build ros_behaviortree_demo # 如果没有安装catkin tools,那就用catkin_make

编译成功的话，可以看到生成的相关代码

3、 行为树开发

3.1 编写do_what 和 shoot 的 服务端代码

note: 这里与普通的ROS 服务端没区别

3.1.1 do_what_service_server

在src文件夹创建 do_what_service_server.cpp

#include 
#include 


bool DoSomething(ros_behaviortree_demo::do_what::Request  &req,
         ros_behaviortree_demo::do_what::Response &res)
{
    if( (req.blood + req.remain_bullets) < 5){
        // 如果子弹数量+血量 < 5 ,就躲避，躲避用1表示
        res.dowhat = 1;
        ROS_INFO("dowhat = %d", res.dowhat);
    }

    if( (req.blood + req.remain_bullets) >= 5){
        // 如果子弹数量+血量 >= 5 ,就进攻，进攻用2表示
        res.dowhat = 2;
        ROS_INFO("dowhat = %d", res.dowhat);
    }
    return true;
}




int main(int argc, char **argv)
{
    ros::init(argc, argv, "do_what_server");
    ros::NodeHandle n;

    ros::ServiceServer service = n.advertiseService("do_what", DoSomething);
    ROS_INFO("Ready to do what.");

    ros::spin();

    return 0;
}

3.1.2 shoot_action_server

在 src 文件夹创建 shoot_action_server.cpp

#include   // Note: "Action" is appended
#include 

typedef actionlib::SimpleActionServer<ros_behaviortree_demo::ShootAction> Server;

void shoot_action_callback(const ros_behaviortree_demo::ShootGoalConstPtr& goal, Server* server)
{
    ros_behaviortree_demo::ShootFeedback feedback;
    ros_behaviortree_demo::ShootResult result;

    ros::Rate r(2);  // 一秒开两枪
    bool success = true;


    for(int i=1; i <= goal->Number_of_shots; i++)
    {
        if (server->isPreemptRequested() || !ros::ok())
        {
            ROS_INFO("shoot is Preempted");
            success = false;
            break;
        }

        ROS_INFO("the NO.%d shoot ", i);
        feedback.Which_shot = i;
        server->publishFeedback(feedback);
        r.sleep();
    }

    if(success)
    {
        result.finish = "yes";
        server->setSucceeded(result);
    }

}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "shoot_server");
    ros::NodeHandle n;
    Server server(n, "shoot", boost::bind(&shoot_action_callback, _1, &server), false);
    server.start();
    ros::spin();
    return 0;
}

3.1.3 在CmakeLists.txt 添加 do_what_service_server.cpp 和 shoot_action_server.cpp 的 编译规则

# 编译 do_what_service_server
add_executable(do_what_service_server src/do_what_service_server.cpp)
add_dependencies(do_what_service_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ros_behaviortree_demo_gencpp)
target_link_libraries(do_what_service_server ${catkin_LIBRARIES} )

# 编译 shoot_action_server
add_executable(shoot_action_server src/shoot_action_server.cpp)
add_dependencies(shoot_action_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ros_behaviortree_demo_gencpp)
target_link_libraries(shoot_action_server ${catkin_LIBRARIES} )

3.2 行为树的实现

3.2.1 行为树的逻辑构建

在include/ros_behaviortree_demo 文件夹创建tree_xml.h，这里面放行为树的xml文件

// 本行为树的逻辑：整个树是一个序列节点，依次运行以下行为
// 1. 一个简单的printf行为
// 2. 给do_what 服务发送请求，并等待结果
// 3. 给 shoot action 发送请求，并等待结果
#ifndef SRC_TREE_XML_H
#define SRC_TREE_XML_H

static const char* xml_text = R"(
 
     
        

            

            

            
                
                    
                
            

        
     
 
 )";

#endif //SRC_TREE_XML_H

3.2.2 行为树各个节点的实现 tree_node.h 、tree_node.cpp

在 src 文件夹创建 tree_node.cpp,在include/ros_behaviortree_demo 文件夹创建tree_node.h。
这两个文件的作用是实现决策树里面叶节点的具体工作。
从上面的决策树逻辑可以看到一共有三个叶节点，分别做出了三种行为，分别是 SaySomething doWhat， shoot。对应的具体行为是SayHelloWorld， doWhat，shoot_5_bullets。

因此，要知道怎么实现具体行为，就必须先在tree_node.cpp和tree_node.h文件里面定义该种行为的实现方法。

tree_node.h : 代码链接

tree_node.cpp :代码链接

3.2.3 注册行为树的各个叶节点，并运行行为树 run_bh_tree.cpp

在src 文件夹创建 run_bh_tree.cpp

#include 
#include 
#include 
#include 


int main(int argc, char **argv)
{
    ros::init(argc, argv, "run_tree");
    ros::NodeHandle nh;

    BT::BehaviorTreeFactory factory;

    // 注册各个叶节点，将行为树的叶节点与实现方式绑定
    factory.registerNodeType<TreeNode::SaySomething>("SayHelloWorld");
    BT::RegisterRosService<TreeNode::DoWhat>(factory, "doWhat", nh);
    BT::RegisterRosAction<TreeNode::Shoot>(factory, "shoot_5_bullets", nh);

    auto tree = factory.createTreeFromText(xml_text);

    BT::NodeStatus status = BT::NodeStatus::IDLE;

   while( ros::ok())
    {
        ros::spinOnce();
        status = tree.tickRoot();
        std::cout << status << std::endl;
        ros::Duration sleep_time(0.1);
        sleep_time.sleep();

    }

    return 0;
}

3.2.4 在CmakeLists.txt 添加 run_bh_tree.cpp 的编译规则

# 创建行为树
add_executable(run_bh_tree  src/run_bh_tree.cpp src/tree_node.cpp)
add_dependencies(run_bh_tree ${catkin_EXPORTED_TARGETS} ros_behaviortree_demo_gencpp)
target_link_libraries(run_bh_tree ${catkin_LIBRARIES})

3.3 编译整个工程

catkin build ros_behaviortree_demo

4、 测试行为树

打开三个命令行，分别执行

启动 do_what 服务端

rosrun ros_behaviortree_demo do_what_service_server

启动 shoot 服务端

rosrun ros_behaviortree_demo shoot_action_server

启动 行为树

rosrun ros_behaviortree_demo run_bh_tree 

如果没有意外，将是下面的显示