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

一、说明

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

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

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

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

二、行为树的基本概念 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

三、BT 的节点类型

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

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

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

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

        请记住，在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 
#include 

using namespace BT;

class SaySomething : public SyncActionNode
{
public:
    SaySomething(const std::string& name, const NodeConfiguration& config)
        : SyncActionNode(name, config)
    {
    }

    static PortsList providedPorts()
    {
        return { InputPort("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"(

  
    
  

)";

int main()
{
    BehaviorTreeFactory factory;
    factory.registerNodeType("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 

#include 
#include 

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 node
    NodeStatus 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 node
    NodeStatus 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 node
    NodeStatus 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"(
 

     
        
            
                
                    
                        
                        
                    
                    
                        
                        
                    
                
            
        
     
 
 )";

int main()
{    
    BehaviorTreeFactory factory;

    // Register custom action nodes with the factory
    factory.registerNodeType("FindBall");
    factory.registerNodeType("PickBall");
    factory.registerNodeType("PlaceBall");

    // Register custom condition nodes with the factory
    factory.registerSimpleCondition("BallClose", std::bind(BallClose));
    factory.registerSimpleCondition("BallGrasped", std::bind(BallGrasped));

    // Create an instance of GripperInterface
    GripperInterface gripper;

    // Register a simple action node using the GripperInterface instance
    factory.registerSimpleAction("GraspBall", std::bind(&GripperInterface::close, &gripper));

    // Create the behavior tree using the XML description
    auto tree = factory.createTreeFromText(xml_text);

    // Run the behavior tree until it finishes
    tree.tickWhileRunning();

    return 0;
}

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

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

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

        此代码提供了机器人如何使用行为树执行一系列任务的实际演示，并在此过程中进行检查。但是，请注意，当前条件始终返回 。为了使此示例更加逼真，您可能需要根据机器人的实际状态及其环境更新这些条件。NodeStatus::FAILURE

        这是这篇文章的总结！您已经了解了行为树、其结构和组件，甚至使用拾取和放置示例实现了复杂的树。

        请继续关注我们的下一篇文章，我们将把事情提升一个档次。我们将介绍 ROS2（机器人操作系统 2）与行为树的集成，为您的机器人应用程序创建复杂、健壮和自适应行为开辟更多可能性。下一篇文章见！

        我希望这些信息对您有所帮助，并且您发现它很有用。如果您有任何问题或意见，请随时告诉我。感谢您的反馈，并很乐意帮助回答您可能遇到的任何问题。感谢您抽出宝贵时间阅读本文。

        我总是在LinkedIn上分享有趣的文章和更新，所以如果您想随时了解情况，请随时在平台上关注我。杰加特桑·尚穆甘

参考资料：
1.行为树官方文档
2.机器人和人工智能中的行为树：简介
3.导航2行为树

Nav2 Behavior Trees — Nav2 1.0.0 documentation (ros.org)