ROS2 的行为树 — 第 1 部分：解锁高级机器人决策和控制

一、说明

在复杂而迷人的机器人世界中，行为树（BT）已成为决策过程中不可或缺的一部分。它们提供了一种结构化、模块化和高效的方法来对机器人的行为进行编程。BT起源于视频游戏行业，用于控制非玩家角色，他们在机器人领域找到了归宿，他们擅长管理无数的任务和条件。

用于机器人导航的Nav2和操作框架MoveIt等机器人软件使用行为树来处理复杂的动作和条件。

在这篇文章中，我们将探索行为树以彻底理解它们。我们将首先分解行为树的基本部分，并清楚地了解它们的工作原理。然后，我们将进入实际领域，编写代码来创建和使用行为树，独立于任何特定的平台或框架。

但我们不会止步于此。一旦我们掌握了基础知识，我们将更进一步，将我们的行为树与ROS2（机器人操作系统2）集成，ROS<>是一个用于编写机器人软件的灵活框架。这将展示如何在现实世界的机器人环境中使用行为树，增强我们自主系统的决策能力。

二、行为树的基本概念

将行为树（BT）视为视频游戏角色、机器人或任何其他需要决定下一步操作的自主代理的决策图。

想象一下，你正在导演一部戏剧，但你的演员是机器人。作为导演，你需要一种方法来指导你的演员，告诉他们该做什么以及什么时候做。行为树（BT）就像你的脚本，详细说明你的机器人演员需要做出的每一个动作。就像在真实的戏剧中一样，当导演叫“动作”（或者，在这种情况下，“滴答”）时，节目（或任务）就开始了。

那么，什么是“刻度”？在BT的上下文中，tick是从树的根节点（或起点）到其子节点的信号，敦促它们执行任务。节点仅在收到即时报价时“采取行动”。

现在，这个系统的美妙之处在于它的反馈机制。节点开始执行其任务后，它会使用以下三种状态之一与控制器（其父节点）通信其进度：正在运行、成功或失败。

正在运行：这种状态有点像在说，“我在上面，但我还没有完成。
成功：这相当于，“我已经尽了自己的一份力量，而且进展顺利。
失败：这就像说，“我试过了，但我做不到。

根据这些状态，导演决定将下一个即时报价发送到何处。这是一个持续到任务完成或不再有效的循环。

行为树由两种主要类型的节点组成：控制流节点和执行节点。让我们更深入地了解它们。

控制流节点就像导演的助手，他们帮助决定演员下一步应该做什么。主要有四种类型：

序列（→）节点：这就像一个严格的助手，他要求每个任务都必须完美地完成，顺序，然后才能继续下一个任务。如果任何任务失败，序列将停止并报告失败。

回退（？节点（也称为选择器）：这是一个更灵活的助手，他有一个任务列表并逐个尝试，直到一个成功。如果所有任务都失败，则回退报告失败。

并行（⇒）节点： 这是一个可以同时处理多项任务的助手，指导多个演员同时执行他们的任务。并行节点根据一组必须成功的任务来定义其成功。

装饰器节点：这个助手有点特别，因为它只处理一个演员，但可以通过各种方式修改演员的行为。有不同种类的装饰器，每种装饰器都为任务提供了独特的转折。

执行节点是参与者，即执行任务（操作）或评估某些条件（条件）的参与者。

操作节点： 当此参与者收到勾号时，它会启动其任务，使控制器保持“正在运行”状态的更新，最后报告“成功”或“失败”。

条件节点： 这个演员更像是一个抽查者。它评估是否满足某些条件，如果条件为真，则立即报告“成功”，如果条件为假，则立即报告“失败”。

现在您已经了解了基础知识，您可以使用行为树指导您的机器人游戏。请记住，此框架旨在提高灵活性和适应性，使您的机器人参与者能够快速响应变化并执行复杂、细致入微的任务。

三、BT 的节点类型

让我们考虑一个机器人手臂上下文中的行为树，它可以捡起球并将其放置在球门位置。

高级BT执行一项任务，包括首先找到，然后挑选并最终放置球

BT 以序列节点作为主控制器开始。该节点对机器人有两个主要任务：“查找球”和“放置球”。
第一个任务“找球”涉及一个机器人，该机器人被分配来定位球。
第二个任务“放置一个球”更为复杂。此任务由另一个“序列”节点管理，该节点可确保按顺序执行一系列操作。此序列涉及任务“挑选球”。
“拾球”任务由两个回退节点管理。这些节点中的每一个都有不同的方法来引导机器人。

第一个回退节点确保机器人检查“球是否接近”，如果是，则指示机器人“接近球”。
第二个回退节点负责检查“球是否被抓住”，如果没有，则指示机器人“抓住球”。

请记住，在BT中，节点仅在收到信号或“滴答”时才起作用。一旦任务启动，机器人就会将其状态传达回其父节点。循环一直持续到所有任务完成或无法完成。

到目前为止，我们已经详细讨论了行为树的概念和机制。现在，是时候将这些想法付诸行动了。

在深入研究实际编码之前，我们需要设置行为树包。在以下各节中，我们将引导您完成设置此包的过程，之后我们将探索将行为树变为现实的代码。

要在 Ubuntu 20.04 上设置并运行我们的第一个 BehaviorTree.CPP 示例，请执行以下步骤：

# Install the required dependencies:
# Open a terminal and run the following commands to install the necessary 
# dependencies:
sudo apt-get update
sudo apt-get install -y libzmq3-dev libboost-dev libboost-system-dev libboost-filesystem-dev libboost-thread-dev libprotobuf-dev protobuf-compiler libmsgsl-dev libgtest-dev cmake# Build and install BehaviorTree.CPP:
# Clone the BehaviorTree.CPP repository, build, and install the library:
git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git
cd BehaviorTree.CPP
mkdir build
cd build
cmake ..
make -j$(nproc)
sudo make install

让我们从使用 BehaviorTree 的简单示例开始.CPP将帮助您了解基础知识并熟悉语法。

mkdir ~/example1
cd ~/example1
touch main.cpp && touch CMakeLists.txt

// main.cpp#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>using namespace BT;class SaySomething : public SyncActionNode
{
public:SaySomething(const std::string& name, const NodeConfiguration& config): SyncActionNode(name, config){}static PortsList providedPorts(){return { InputPort<std::string>("message") };}NodeStatus tick() override{std::string message;getInput("message", message);std::cout << "Robot says: " << message << std::endl;return NodeStatus::SUCCESS;}
};static const char* xml_text = R"(
<root><BehaviorTree><SaySomething message="Hello, World!" /></BehaviorTree>
</root>
)";int main()
{BehaviorTreeFactory factory;factory.registerNodeType<SaySomething>("SaySomething");auto tree = factory.createTreeFromText(xml_text);tree.tickWhileRunning();return 0;
}

# CMakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(bt_example)set(CMAKE_CXX_STANDARD 17)find_package(behaviortree_cpp REQUIRED)add_executable(bt_example main.cpp)
target_link_libraries(bt_example BT::behaviortree_cpp)

# Now, build and run the example:
mkdir build
cd build
cmake ..
make
./bt_example

太棒了，您已经获得了第一个BehaviorTree.CPP 代码片段！该脚本代表了一个基本的行为树，其中机器人（您的演员）有一个简单的任务：说些什么。让我们来分解一下：

该类扩展了该类，该类是 BehaviorTree.CPP 提供的执行节点类型之一。这是一个同步操作节点，这意味着它不返回“正在运行”状态，而只返回“成功”或“失败”。在我们的例子中，它将始终返回“成功”。SaySomethingSyncActionNodeSyncActionNode

该方法是在勾选此节点时调用的主要方法。在这里，它检索输入消息并将其输出到控制台。由于它每次都成功执行其任务，因此它会返回 .tick()NodeStatus::SUCCESS

定义为的 XML 文本表示行为树的结构。它包含一个节点，带有消息“Hello， World！xml_textSaySomething

最后，该函数创建一个，注册节点类型，从 XML 文本创建树，并在树运行时勾选树。这让机器人说“你好，世界！mainBehaviorTreeFactorySaySomething

现在，有了基础知识，您就可以转向更复杂的行为树，例如拾取和放置树。在选取和放置树中，您将引入更多控制流节点和执行节点，将它们交织在一起以实现所需的行为。此处提供了此示例的完整代码。

#include <iostream>#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>using namespace BT;// Define FindBall action node
class FindBall : public BT::SyncActionNode 
{public:FindBall(const std::string& name) : BT::SyncActionNode(name, {}){}// Define the tick() function for the FindBall nodeNodeStatus tick() override{std::cout << "[⚓ FindBall ] => \t" << this->name() << std::endl; return BT::NodeStatus::SUCCESS;}
};// Define PickBall action node
class PickBall : public BT::SyncActionNode
{public:PickBall(const std::string& name) : BT::SyncActionNode(name, {}){}// Define the tick() function for the PickBall nodeNodeStatus tick() override{std::cout << "[⚓ PickBall ] => \t" << this->name() << std::endl;return BT::NodeStatus::SUCCESS;}
};// Define PlaceBall action node
class PlaceBall : public BT::SyncActionNode
{public:PlaceBall(const std::string& name) : BT::SyncActionNode(name, {}){}// Define the tick() function for the PlaceBall nodeNodeStatus tick() override{std::cout << "[⚓ PlaceBall ] => \t" << this->name() << std::endl;return BT::NodeStatus::SUCCESS;}
};// Define GripperInterface class for interacting with the gripper
class GripperInterface
{private:bool _opened;public:GripperInterface() : _opened(true){}NodeStatus open(){_opened = true;std::cout << "GripperInterface::open" << std::endl;return BT::NodeStatus::SUCCESS;}NodeStatus close(){std::cout << "GripperInterface::close" << std::endl;_opened = false;return BT::NodeStatus::SUCCESS;}
};// Define BallClose condition function
BT::NodeStatus BallClose()
{std::cout << "[ Close to ball: NO ]" << std::endl;return BT::NodeStatus::FAILURE;
}// Define BallGrasped condition function
BT::NodeStatus BallGrasped()
{std::cout << "[ Grasped: NO ]" << std::endl;return BT::NodeStatus::FAILURE;
}// Define the behavior tree structure in XML format
static const char* xml_text = R"(<root main_tree_to_execute = "MainTree" ><BehaviorTree ID="MainTree"><Sequence name="root_sequence"><FindBall   name="found_ok"/><Sequence><Fallback><BallClose   name="no_ball"/><PickBall    name="approach_ball"/></Fallback><Fallback><BallGrasped   name="no_grasp"/><GraspBall  name="grasp_ball"/></Fallback></Sequence><PlaceBall   name="ball_placed"/></Sequence></BehaviorTree></root>)";int main()
{    BehaviorTreeFactory factory;// Register custom action nodes with the factoryfactory.registerNodeType<FindBall>("FindBall");factory.registerNodeType<PickBall>("PickBall");factory.registerNodeType<PlaceBall>("PlaceBall");// Register custom condition nodes with the factoryfactory.registerSimpleCondition("BallClose", std::bind(BallClose));factory.registerSimpleCondition("BallGrasped", std::bind(BallGrasped));// Create an instance of GripperInterfaceGripperInterface gripper;// Register a simple action node using the GripperInterface instancefactory.registerSimpleAction("GraspBall", std::bind(&GripperInterface::close, &gripper));// Create the behavior tree using the XML descriptionauto tree = factory.createTreeFromText(xml_text);// Run the behavior tree until it finishestree.tickWhileRunning();return 0;
}

# Output
[⚓ FindBall ] =>       found_ok
[ Close to ball: NO ]
[⚓ PickBall ] =>       approach_ball
[ Grasped: NO ]
GripperInterface::close
[⚓ PlaceBall ] =>      cube_placed

太好了，您创建了一个更复杂的行为树！此脚本表示机器人的拾取和放置操作，由查找、拾取和放置球组成。让我们剖析一下：

您已经创建了三个操作节点：、和。这些类中的每一个都扩展并实现函数，该函数记录操作的名称并返回 .FindBallPickBallPlaceBallSyncActionNodetick()NodeStatus::SUCCESS
您已经定义了一个类，该类表示机器人的抓手。它可以打开和关闭夹持器，两个操作返回。GripperInterfaceNodeStatus::SUCCESS
您已经定义了两个条件函数：和。这些功能表示机器人在捡球之前需要进行的检查。他们目前总是返回，模拟球没有接近并且机器人还没有抓住球的场景。BallCloseBallGraspedNodeStatus::FAILURE
XML 字符串定义行为树。它从一个序列节点开始，该节点首先尝试找到球（）。找到球后，它会尝试拾取球，这是由两个回退节点组成的另一个序列节点。第一个回退节点检查球是否接近（），如果没有，机器人将接近球（）。第二个回退节点检查球是否被抓住（），如果没有，机器人会抓住球（）。一旦球被捡起，机器人就会放置球（）。xml_textFindBallBallClosePickBallBallGraspedGraspBallPlaceBall
最后，该函数注册自定义操作和条件节点，创建的实例，使用注册操作，从 XML 文本创建树，并在树运行时勾选树。mainGripperInterfaceGraspBallGripperInterface