在ROS中使用行为树
参考资料
0、 准备工作
behaviorTree 库安装(ROS使用下的安装方式)
sudo apt-get install ros-$ROS_DISTRO-behaviortree-cpp-v3
下载我提供的demo
1、behaviorTree库内容简要了解
这个库提供了很多TreeNode,从功能上区分的话主要分为三种 ControlNode DecoratorNode LeafNode
一般情况下ControlNode DecoratorNode 是库里已经开发好的节点，主要是负责调整行为树的运行逻辑，比如最常用的SequenceNode和FallbackNode等,直接在行为树的xml里面调用就可以。
而我们对行为树的开发主要焦距在 LeafNode的开发，也可以说主要在开发ActionNode ，也就是具体的事务实现。一般来讲，SyncActionNode 能满足我们绝大部分开发需求，所以我的demo围绕这个节点来介绍行为树的具体开发。
2、 Demo 讲解
2.1、我提供的demo：
如图所示，我提供的行为树demo 有以上内容
- 行为树主树： MainTree
- 一个子树： SubA
- 普通action node的使用： SelfIntroduction
- 行为树blackboard 的使用：SetBlackboard 和 BlackboardRead
- ros server的使用： AddTwoInts
- ros action 的使用： Shoot
对应的行为树XML :
main_tree.xml
<root main_tree_to_execute="MainTree">
<include path="sub_tree.xml"/>
<BehaviorTree ID="MainTree">
<Sequence>
<SelfIntroduction my_name="robot_aa" age="6" output="{aa_output}"/>
<SetBlackboard output_key="cc_name" value="robot_cc"/>
<SubTree ID="SubA" bb_output="sub_output1" cc_name_sub="cc_name" cc_output="sub_output2"/>
<BlackboardRead aa_msg="{aa_output}" bb_msg="{sub_output1}" cc_msg="{sub_output2}"/>
<AddTwoInts/>
<Shoot/>
</Sequence>
</BehaviorTree>
</root>
sub_tree.xml
<root>
<BehaviorTree ID="SubA">
<Sequence>
<SelfIntroduction my_name="robot_bb" age="7" output="{bb_output}"/>
<SelfIntroduction my_name="{cc_name_sub}" age="8" output="{cc_output}"/>
</Sequence>
</BehaviorTree>
</root>
note:
"robot_aa"： 这种格式意味着这就是个右值,这个值就是robot_aa，全都是字符串类型，用的时候需要类型转换
"{aa_output}"： 这种格式意味着这是个左值，储存在这个树的blackboard里面，aa_output就是这个值的key
2.2 普通ActionNode 开发：self_intro.h
#pragma once
#include <behaviortree_cpp_v3/action_node.h>
#include <ros/ros.h>
namespace demo_behaviortree {
class SelfIntro : public BT::SyncActionNode {
public:
SelfIntro(const std::string& name, const BT::NodeConfiguration& config) : BT::SyncActionNode(name, config){};
// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
ports_list.insert(BT::InputPort<std::string>("my_name"));
ports_list.insert(BT::InputPort<int>("age"));
ports_list.insert(BT::OutputPort<std::string>("output"));
return ports_list;
}
BT::NodeStatus tick() override {
auto my_name = getInput<std::string>("my_name");
auto age = getInput<int>("age");
if (!my_name) {
throw BT::RuntimeError("missing required input [my_name]: ", my_name.error());
}
if (!age) {
throw BT::RuntimeError("missing required input [age]: ", age.error());
}
ROS_INFO("my name is %s, I`m %d years old. ", my_name->c_str(), age.value());
std::string op = "This is the msg of " + std::string(my_name.value());
setOutput("output", op);
return BT::NodeStatus::SUCCESS;
};
};
note:
static BT::PortsList providedPorts() 必须写，如果没有端口参数，就写成下面格式
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
return ports_list;
}
2.3 行为树blackboard 的使用
#pragma once
#include <behaviortree_cpp_v3/action_node.h>
#include <ros/ros.h>
namespace demo_behaviortree {
class BlackboardRead : public BT::SyncActionNode {
public:
BlackboardRead(const std::string& name, const BT::NodeConfiguration& config) : BT::SyncActionNode(name, config){};
// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
ports_list.insert(BT::InputPort<std::string>("aa_msg"));
ports_list.insert(BT::InputPort<std::string>("bb_msg"));
ports_list.insert(BT::InputPort<std::string>("cc_msg"));
return ports_list;
}
BT::NodeStatus tick() override {
auto aa_msg = getInput<std::string>("aa_msg");
auto bb_msg = getInput<std::string>("bb_msg");
auto cc_msg = getInput<std::string>("cc_msg");
ROS_INFO("aa msg: %s.", aa_msg->c_str());
ROS_INFO("bb msg: %s.", bb_msg->c_str());
ROS_INFO("cc msg: %s.", cc_msg->c_str());
return BT::NodeStatus::SUCCESS;
};
};
} // namespace demo_behaviortree
note:
主树和子树各自维护自己的blackboard，当需要使用对方blackboard里面的值的时候，需要做Remapping
参考资料： Remapping
2.4 ros server 的使用
这个服务用的是ros 官方server demo，client 发送两个int值给 server，server 返回两个int值相加的和。
#pragma once
#include <behaviortree_cpp_v3/action_node.h>
#include <ros/ros.h>
#include "demo_behaviortree/AddTwoInts.h"
namespace demo_behaviortree {
class Calculate : public BT::SyncActionNode {
public:
Calculate(const std::string& name, const BT::NodeConfiguration& config, const ros::NodeHandle& root_nh,
const ros::NodeHandle& tree_nh)
: BT::SyncActionNode(name, config) {
ros::NodeHandle nh(root_nh);
ati_client_ = nh.serviceClient<demo_behaviortree::AddTwoInts>("add_two_ints");
ros::NodeHandle calculate_nh(tree_nh, "calculate");
a_ = calculate_nh.param("a", 0);
b_ = calculate_nh.param("b", 0);
};
// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
return ports_list;
}
BT::NodeStatus tick() override {
demo_behaviortree::AddTwoInts srv;
srv.request.a = a_;
srv.request.b = b_;
if (ati_client_.call(srv)) {
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
} else {
ROS_ERROR("Failed to call service add_two_ints");
return BT::NodeStatus::FAILURE;
}
return BT::NodeStatus::SUCCESS;
};
private:
ros::ServiceClient ati_client_;
int a_, b_;
};
} // namespace demo_behaviortree
note:
这个ActionNode 是带参数的,参数是root_nh 和 tree_nh
2.5 ros action 的使用
Shoot.action
# Define the goal
int32 number_of_shots # 一共打几枪
---
# Define the result
uint8 SUCCESS = 0
uint8 FAILURE = 10
uint8 CANCELED = 11
uint8 res
---
# Define a feedback message
int32 which_shot # 正在开第几枪
这个action 的功能是，client 发送一个请求，告诉server一共要开几枪，server接收到请求之后，以0.5秒开一枪的速度开枪，并发布一个feedback，feedback的内容是现在正在开第几枪。
#pragma once
#include <actionlib/client/simple_action_client.h>
#include <ros/ros.h>
#include "demo_behaviortree/ShootAction.h"
namespace demo_behaviortree {
class Shoot : public BT::SyncActionNode {
public:
Shoot(const std::string& name, const BT::NodeConfiguration& config, const ros::NodeHandle& root_nh,
const ros::NodeHandle& tree_nh)
: BT::SyncActionNode(name, config) {
ros::NodeHandle nh(root_nh);
shoot_client_ = std::make_unique<actionlib::SimpleActionClient<demo_behaviortree::ShootAction>>("shoot", true);
feedback_sub_ = nh.subscribe<demo_behaviortree::ShootActionFeedback>("shoot/feedback", 1, &Shoot::feedbackCB, this);
};
// 这个必须要写，处理接口参数的
static BT::PortsList providedPorts() {
BT::PortsList ports_list;
return ports_list;
}
BT::NodeStatus tick() override {
// 这时是一个简单的不断设计的demo，一次action完毕直接发送新的设计请求
// 实际上的真正的行为树这里会判断状态，根据具体情况发送cancel 取消action，或者新的action goal
if (actionlib::SimpleClientGoalState::ACTIVE == shoot_client_->getState().state_) {
ROS_INFO("Shoot feedback which_shot: %d", fb_.feedback.which_shot);
} else {
int num_of_shoot = rand() % 10; // 随便打几枪
demo_behaviortree::ShootGoal shoot_goal;
shoot_goal.number_of_shots = num_of_shoot;
shoot_client_->sendGoal(shoot_goal);
ROS_INFO("Send a new goal, shoot_goal.num_of_shoot is %d in this time", num_of_shoot);
}
return BT::NodeStatus::SUCCESS;
};
private:
void feedbackCB(const demo_behaviortree::ShootActionFeedback::ConstPtr& msg) { fb_ = *msg; };
std::unique_ptr<actionlib::SimpleActionClient<demo_behaviortree::ShootAction>> shoot_client_;
demo_behaviortree::ShootActionFeedback fb_;
ros::Subscriber feedback_sub_;
};
} // namespace demo_behaviortree
note:
注意这个设计模式，在ros中 ，action采用的是client/server模式,所以它的客户端和服务端天生就是异步的，action 的client和行为树同步，在每个tick检查server的状态，并根据具体情况发送action请求，就可以达到一个异步控制的效果,而且还不会阻塞行为树的线程。如果是一些必须要阻塞的action,那直接在tick里面阻塞就达到阻塞行为树的效果。
额外的演示，这是使用了movebase 的action执行机器人自主巡逻的demo
基于行为树的自主巡逻
真机自主巡逻效果
2.5 运行行为树
run_bh_tree.cpp
#include <behaviortree_cpp_v3/bt_factory.h>
#include <ros/ros.h>
#include "demo_behaviortree/blackboard_read.h"
#include "demo_behaviortree/calculate.h"
#include "demo_behaviortree/self_intro.h"
#include "demo_behaviortree/shoot.h"
int main(int argc, char** argv) {
ros::init(argc, argv, "run_bh_tree");
ros::NodeHandle root_nh;
ros::NodeHandle bh_tree_nh("~");
BT::BehaviorTreeFactory factory;
// 1. 自我介绍,包含两个参数: my_name, age. 参数定义在行为树xml文件里面
factory.registerNodeType<demo_behaviortree::SelfIntro>("SelfIntroduction");
// 2. 读取行为树 blackboard里面数据的值
// note: MainTree 和 SubA 有各自的 blackboard， 这两个blackboard 想要相互读取对方的数据，需要 Remapping
// ref: https://www.behaviortree.dev/tutorial_06_subtree_ports/
factory.registerNodeType<demo_behaviortree::BlackboardRead>("BlackboardRead");
// 3. 调用 AddTwoInts 服务，计算两个int值相加的结果
// note: 使用了带参数(root_nh, bh_tree_nh)的注册方法，先创建 builder
BT::NodeBuilder builder_calculate = [&root_nh, &bh_tree_nh](const std::string& name,
const BT::NodeConfiguration& config) {
return std::make_unique<demo_behaviortree::Calculate>(name, config, root_nh, bh_tree_nh);
};
factory.registerBuilder<demo_behaviortree::Calculate>("AddTwoInts", builder_calculate);
// 4. 调用 Shoot 服务，随机设置射击次数
// note: 在ros 的action 使用的是client/server模式，因此BT::SyncActionNode 可以很简单地实现异步和同步 action
// 异步方式：在每次tick的时候使用action的client查看server 的状态和feedback，可以根据具体情况发送cancel 或者更改goal
// 同步方式：直接在tick里面去阻塞等待action的完成即可。
// ref: https://www.behaviortree.dev/asynchronous_nodes/
BT::NodeBuilder builder_shoot = [&root_nh, &bh_tree_nh](const std::string& name,
const BT::NodeConfiguration& config) {
return std::make_unique<demo_behaviortree::Shoot>(name, config, root_nh, bh_tree_nh);
};
factory.registerBuilder<demo_behaviortree::Shoot>("Shoot", builder_shoot);
// 5. 读取行为树的xml文件, 根据上面具体行为的实现方法生成行为树
// ref: https://www.behaviortree.dev/tutorial_06_subtree_ports/
// ref: https://www.behaviortree.dev/tutorial_07_multiple_xml/
std::string file_path = bh_tree_nh.param("file_path", std::string(" "));
auto tree = factory.createTreeFromFile(file_path);
ros::Rate loop_rate(5); // 5HZ,设置行为树的运行频率
while (ros::ok()) {
BT::NodeStatus status = tree.tickRoot();
std::cout << status << std::endl;
loop_rate.sleep();
ros::spinOnce();
}
return 0;
}
3、 运行行为树
roslaunch demo_behaviortree all_server.launch # 运行服务和action的server
roslaunch demo_behaviortree run_tree.launch # 启动行为树
1340
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
