接下来会记录如何在ROS2中使用BT（Behavior Tree）
1. 介绍 BT 框架
BT类似于 FSM 有着广泛的应用，BT框架可以集成到任何其他软件中，首先记录一下没有ROS的纯BT框架，
在理解核心原则之后，将BT程序的相同结构合并到ROS2环境中。
行为树（Behavior Tree，BT）是一种用于设计和管理机器人或自主智能的行为的框架。它可以将复杂的任务和行为分解成逻辑上连续的节点，使得整个系统的控制逻辑更加清晰、灵活和可扩展。
在BT中，整个行为树的结构类似一个树状图，从根节点开始，逐级向下展开，直到叶子节点为止。每个节点都代表着一个特定的行为或决策，并且可以根据节点之间的组合规则进行执行。
常见的BT节点类型包括：
序列节点（Sequence）： 顺序执行它们包含的子节点，一个接一个地执行，只有当所有子节点都成功执行时，整个序列才被认为成功。
选择节点（Fallback）： 依次执行它们的子节点，直到找到一个成功执行的节点，然后停止。它类似于“或”的关系。
行为节点（Action）： 这些节点代表具体的行为，例如“捡起物体”、“放置物体”等，它们是执行最终任务的节点。
条件节点（Condition）： 用于判断特定条件是否满足，例如“检测物体是否在视野范围内”、“是否已经抓住物体”等。
1.1 用C++ 实现简单 BT
在BT中，节点从子节点从左往右执行，使用符号 ⟶ 表示逻辑与操作（Sequence），而符号 “?” 表示逻辑或操作（Fallback），也称回退操作。通过这些逻辑运算符，我们可以推导机器人的行为。比如，机器人首先要找到、抓取并放置球。抓取操作需要执行特定的动作。
如图所示的行为树的逻辑是这样的：
1.寻找球的任务 (FindBall)。
2.依次执行两个回退节点，如果球没有靠近，则执行接近球的任务 (ApproachBall)；如果球没有被抓住，则执行抓住球的任务 (GraspBall)。
3.执行放置球的任务 (PlaceBall)。
代码如下：
-
首先定义四个行为节点类：ApproachBall、FindBall、PlaceBall和GripperInterface：
class ApproachBall: public BT::SyncActionNode
{
public:
ApproachBall(const std::string& name):BT::SyncActionNode(name,{}){} NodeStatus tick() override { std::count <<"ApproachBall: " << this->name() << std::endl; return BT::NodeStatus::SUCCESS; } }; //----------------Class FindBall------------------------------------------ class FindBall : public BT::SyncActionNode { public: FindBall(const std::string& name) : BT::SyncActionNode(name, {}) { } NodeStatus tick() override { std::cout << "FindBall: " << this->name() << std::endl; return BT::NodeStatus::SUCCESS; } }; //----------------Class PlaceBall------------------------------------------ class PlaceBall : public BT::SyncActionNode { public: PlaceBall(const std::string& name) : BT::SyncActionNode(name, {}) { } NodeStatus tick() override { std::cout << "PlaceBall: " << this->name() << std::endl; return BT::NodeStatus::SUCCESS; } };//----------------Class GripperInterface------------------------------------------
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; }};
BT::NodeStatus BallClose()
{
std::cout << “[ Close to ball: NO ]” << std::endl;
return BT::NodeStatus::FAILURE;
}BT::NodeStatus BallGrasped()
{
std::cout << “[ Grasped: NO ]” << std::endl;
return BT::NodeStatus::FAILURE;
}
其中BallClose()和BallGrasped()为简单的简单的条件节点函数
-
定义了一个 XML 文本，用于构建整个行为树的结构
-
static const char* xml_text = R"(
<BehaviorTree ID="MainTree"> <Sequence name="root_sequence"> <FindBall name="ball_ok"/> <Sequence> <Fallback> <BallClose name="no_ball"/> <ApproachBall name="approach_ball"/> </Fallback> <Fallback> <BallGrasped name="no_grasp"/> <GraspBall name="grasp_ball"/> </Fallback> </Sequence> <PlaceBall name="ball_placed"/> </Sequence> </BehaviorTree> </root>)";
根节点是root_sequence，是一个序列节点，它包含了一系列子节点。
首先是FindBall节点，表示机器人寻找球的任务。然后是另一个序列节点，其中包含两个回退节点：BallClose和ApproachBall，用于决定机器人是否接近球。
如果BallClose返回FAILURE，表示机器人没有靠近球，则会执行ApproachBall节点，表示机器人接近球的任务。同理，对于抓取动作，如果BallGrasped返回FAILURE，表示机器人没有成功抓住球，则会执行GraspBall节点，表示机器人抓住球的任务。
-
最后，在主函数创建和执行行为树：
首先，我们创建了一个BehaviorTreeFactory对象 factory，用于注册自定义的行为节点。
接着，我们通过 factory.registerNodeType 注册了自定义的行为节点类 ApproachBall、FindBall和PlaceBall。
然后，我们通过 factory.registerSimpleCondition 注册了两个简单的条件节点函数 BallClose 和 BallGrasped。
通过 factory.createTreeFromText(xml_text) 从 XML 文本中创建了整个行为树结构
最后，通过调用 tree.tickRoot() 来执行行为树的根节点（root_sequence）。从根节点开始，树的执行逻辑会根据节点之间的连接和条件判断，依次调用每个节点的 tick() 函数来决定下一步执行的动作。整个行为树的执行过程会根据具体的逻辑规则依次展开，直到任务完成或者出现其他条件导致中止。
int main()
{
BehaviorTreeFactory factory;factory.registerNodeType<ApproachBall>("ApproachBall"); factory.registerNodeType<FindBall>("FindBall"); factory.registerNodeType<PlaceBall>("PlaceBall"); factory.registerSimpleCondition("BallClose", std::bind(BallClose)); factory.registerSimpleCondition("BallGrasped", std::bind(BallGrasped)); GripperInterface gripper; factory.registerSimpleAction("GraspBall", std::bind(&GripperInterface::close, &gripper)); auto tree = factory.createTreeFromText(xml_text); tree.tickRoot(); return 0;}
记得添加头文件：
#include “behaviortree_cpp_v3/bt_factory.h”
using namespace BT;
最后执行的结果就是这样：
FindBall: ball_ok
[ Close to ball: NO ]
ApproachBall: approach_ball
[ Grasped: NO ]
GripperInterface::close
PlaceBall: ball_placed
1.2 BT节点类型
BT是一个有向的根树，如何组织节点间的联系，有四种类型：。
tick操作是行为树执行的核心操作，它驱动着整个行为树的逻辑和任务的执行，下面是他的执行规则：
从根节点开始，根据节点的类型和连接关系选择要执行的子节点。
执行选择的子节点的tick操作，并等待其返回状态。
根据子节点的返回状态，决定下一步要执行的节点。
如果子节点返回的是"Running"（正在执行）状态，表示该节点需要继续执行，此时tick操作会暂停并等待下一次tick继续执行。
如果子节点返回的是"Success"（成功）状态，表示该节点执行成功，tick操作会继续执行下一个节点。
如果子节点返回的是"Failure"（失败）状态，表示该节点执行失败，tick操作会继续执行下一个节点或者回溯到更高级的节点。
重复以上步骤，直到整个行为树执行完成或者遇到特定的终止条件。
1.2.1 Sequence节点
Sequence节点功能类似于“与”，顺序执行它们包含的子节点，一个接一个地执行，只有当所有子节点都成功执行时，整个序列才被认为成功
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Sequence>
</BehaviorTree>
</root>
假设执行task2失败，则显示task1执行的内容，并且返回FAILURE
1.2.2 Fallback节点
依次执行它们的子节点，执行失败就Fallback,直到找到一个成功执行的节点，然后停止。它类似于“或”的关系。
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<Fallback name="root_sequence">
<RobotTask1 name="task1"/>
<RobotTask2 name="task2"/>
<RobotTask3 name="task3"/>
</Fallback>
</BehaviorTree>
</root>
假设执行task1失败，task2成功，则显示task1和task2执行的内容，并且返回SUCCESS
1.2.3 Action节点
这些节点代表具体的行为，结合上文提到tick的规则。动作节点的执行方式取决于它的回调函数，同步动作会，如打印消息等，立即执行完毕并返回结果，
而异步动作，比如耗时操作，可能会在一段时间内持续执行，并在执行完成后返回结果。
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ReactiveFallback name="root">
<EatSandwich name="eat_sandwich"/>
<EatApple name="eat_apple"/>
<Sequence>
<OpenBanana name="open_banana"/>
<EatBanana goal="1;2;3"/>
<SaySomething message="banan is gone!" />
</Sequence>
</ReactiveFallback>
</BehaviorTree>
</root>
1.3 BT 与FSM
行为树（Behavior Tree）相比有限状态机（FSM）更灵活、可扩展，适用于复杂行为逻辑的建模和实现，它通过树状结构组织任务、处理并行和优先级，并让开发者专注于高层次的任务设计。
有限状态机（FSM）在现实世界中遇到许多问题，特别是当状态之间的转换和条件变得非常复杂时。FSM可能会有大量的状态转换，当状态数量增多时，设计变得非常困难，状态之间紧密连接，重用性低，且难以理解和预测系统的行为。
而行为树（Behavior Tree）解决了这些问题，提高了模块化和重用性。行为树是一种层次结构的树状模型，每个子树都可以被视为一个可重用的行为，开发者可以利用行为树提供的语言来应用常见的设计模式，使得文本和图形表示更易于理解。
行为树使用动作（Actions）而不是状态（States），更符合定义行为和软件接口的概念模型，使得设计更直观。行为树的层次结构让人们更容易阅读和理解，避免了状态机中状态数量增多所带来的复杂性和困扰。
通过行为树，设计师可以更轻松地处理复杂的行为逻辑，提高开发效率，让系统的行为更加可控和可预测。
这里给出实现机器人避障的FSM 和BT 的比较：
1688
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
