编写新的行为树插件

概述

本教程展示如何创建自己的行为树（BT）插件。BT插件用作行为树XML中由BT Navigator处理的导航逻辑中的节点。

要求

  • ROS 2（二进制或源码编译）

  • Nav2（包括依赖项）

  • Gazebo

  • Turtlebot3

教程步骤

1- 创建一个新的BT插件

我们将创建一个简单的BT插件节点，用于在另一个服务器上执行操作。在这个示例中，我们将分析``nav2_behavior_tree``软件包中最简单的行为树动作节点，即``wait``节点。除了动作BT节点的示例之外，您还可以创建自定义的装饰器、条件和控制节点。每种节点类型在行为树中扮演着独特的角色，用于执行诸如规划、控制BT的流程、检查条件的状态或修改其他BT节点的输出等操作。

本教程中的代码可以在 nav2_behavior_tree 包中找到，作为``wait_action``节点。该动作节点可以作为编写其他动作节点插件的参考。

我们的示例插件继承自基类``nav2_behavior_tree::BtActionNode``。该基类是BehaviorTree.CPP中``BT::ActionNodeBase``的包装器，简化了使用ROS 2动作客户端的BT动作节点。``BTActionNode``既是BT动作，又使用ROS 2动作网络接口调用远程服务器进行一些工作。

当使用其他类型的BT节点（例如装饰器、控制器、条件节点）时，请使用相应的BT节点，BT::DecoratorNode、BT::ControlNode``或``BT::ConditionNode。对于不使用ROS 2动作接口的BT动作节点，请直接使用``BT::ActionNodeBase``基类。

``BTActionNode``类提供了5个虚拟方法供使用，除了构造函数提供的信息之外。让我们更详细地了解编写BT动作插件所需的方法。

方法

方法描述

是否必需？

构造函数

构造函数用于指示与插件匹配的相应XML标记名称，使用插件调用的动作服务器的名称，以及任何需要的BehaviorTree.CPP特殊配置。

是

提供的端口()

一个函数，用于定义BT节点可能具有的输入和输出端口。这类似于通过硬编码值或其他节点的输出端口的值在BT XML中定义的参数。

是

每秒执行()

在执行期间，当行为树对此BT节点进行打勾时调用此方法。这应该用于获取动态更新，如新的黑板值、输入端口或参数。也可以重置动作的状态。

No

等待结果()

当行为树节点正在等待ROS 2动作服务器返回结果时调用此方法。这可以用于检查当前任务的更新情况，检查超时或在等待动作完成时进行计算等操作。

No

成功()

当ROS 2动作服务器返回成功结果时调用此方法。返回BT节点将向树报告的值。

No

中止()

当ROS 2动作服务器返回一个中止的结果时，调用该方法。返回BT节点将向树报告的值。

No

取消()

当ROS 2动作服务器返回一个取消的结果时，调用该方法。返回BT节点将向树报告的值。

No

在本教程中，我们只会使用``每秒执行()``方法。

在构造函数中，我们需要获取适用于行为树节点的任何非变量参数。在这个示例中，我们需要从行为树XML的输入端口获取睡眠持续时间的值。

WaitAction::WaitAction(
  const std::string & xml_tag_name,
  const std::string & action_name,
  const BT::NodeConfiguration & conf)
: BtActionNode<nav2_msgs::action::Wait>(xml_tag_name, action_name, conf)
{
  int duration;
  getInput("wait_duration", duration);
  if (duration <= 0) {
    RCLCPP_WARN(
      node_->get_logger(), "Wait duration is negative or zero "
      "(%i). Setting to positive.", duration);
    duration *= -1;
  }

  goal_.time.sec = duration;
}

在这里，我们提供了``xml_tag_name``的输入，它告诉BT节点插件与此节点对应的XML中的字符串。当我们将此BT节点注册为插件时，稍后会看到它。它还接受要调用以执行某些行为的动作服务器的字符串名称。最后，有一组配置，对于大多数节点插件的目的，我们可以安全地忽略它们。

然后我们调用``BTActionNode``构造函数。可以看到，它是由ROS 2动作类型进行模板化的，因此我们将其提供给``nav2_msgs::action::Wait``动作消息类型，并转发其他输入。当行为树从树中调用该节点时，BTActionNode``具有``tick()``方法，该方法直接被调用。然后，在发送动作客户端目标之前调用``on_tick()。

在构造函数的主体中，我们获取参数``wait_duration``的输入端口``getInput``，可以独立配置每个树中``wait``节点的实例。它设置在``duration``参数中，并插入到``goal_``中。``goal_``类变量是ROS 2动作客户端将发送到动作服务器的目标。因此，在此示例中，我们设置持续时间为所需等待的时间，以便动作服务器了解我们请求的具体内容。

providedPorts()``方法使我们有机会定义输入或输出端口。端口可以被视为行为树节点从行为树本身访问的参数。对于我们的示例，只有一个输入端口，即``wait_duration，可以在每个``wait``恢复的BT XML中设置。我们设置了类型为``int``，默认值为``1``，名称为``wait_duration``，端口的描述为``等待时间``。

static BT::PortsList providedPorts()
{
  return providedBasicPorts(
    {
      BT::InputPort<int>("wait_duration", 1, "Wait time")
    });
}

当行为树对特定节点进行刻度时，将调用``on_tick()``方法。对于等待BT节点，我们只想通知黑板上的一个计数器，即与恢复对应的动作插件已经刻度。这对于在特定导航运行期间执行的恢复次数进行度量非常有用。您还可以记录或更新``goal_``等待持续时间（如果它是可变输入）。

void WaitAction::on_tick()
{
  increment_recovery_count();
}

剩下的方法未被使用，也不是强制要求重写它们。只有一些BT节点插件需要重写``on_wait_for_result()``以检查抢占或检查超时。如果未重写，成功、中止和取消方法将分别默认为``SUCCESS``、FAILURE、SUCCESS。

2- 导出规划器插件

现在我们已经创建了自定义的BT节点，我们需要导出插件，以便在加载自定义的BT XML时，行为树能够看到它。插件在运行时加载，如果它们不可见，那么我们的BT导航服务器将无法加载或使用它们。在BehaviorTree.CPP中，导出和加载插件由``BT_REGISTER_NODES``宏处理。

BT_REGISTER_NODES(factory)
{
  BT::NodeBuilder builder =
    [](const std::string & name, const BT::NodeConfiguration & config)
    {
      return std::make_unique<nav2_behavior_tree::WaitAction>(name, "wait", config);
    };

  factory.registerBuilder<nav2_behavior_tree::WaitAction>("Wait", builder);
}

在此宏中，我们必须创建一个``NodeBuilder``，以便我们的自定义动作节点可以具有非默认的构造函数签名（用于动作和xml名称）。此lambda将返回我们创建的行为树节点的唯一指针。使用函数参数中给定的相关信息填写构造函数。然后定义此BT节点将调用的ROS 2动作服务器的名称，在本例中为``Wait``动作。

我们最终将构建器交给工厂进行注册。工厂中的“Wait”是与此BT节点插件对应的行为树XML文件中的名称。下面是一个示例，其中“Wait”BT XML节点指定了一个非变量输入端口“wait_duration”，持续时间为5秒。

<Wait wait_duration="5"/>

3- 将插件库名称添加到配置中

为了使BT Navigator节点能够发现我们刚刚注册的插件，我们需要在配置的YAML文件中的bt_navigator节点下列出插件库的名称。配置应该类似于下面所示的内容。请注意plugin_lib_names下列出的nav2_wait_action_bt_node。

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
    plugin_lib_names:
    - nav2_back_up_action_bt_node # other plugin
    - nav2_wait_action_bt_node    # our new plugin

4- 运行您的自定义插件

现在您可以在自定义BT节点中使用行为树。例如，下面显示了``navigate_w_replanning_and_recovery.xml``文件。

在``NavigateToPose``中的具体导航请求中选择此BT XML文件，或者将其作为BT导航器配置yaml文件中的默认行为树。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
      <PipelineSequence name="NavigateWithReplanning">
        <RateController hz="1.0">
          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
            <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
            <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
          </RecoveryNode>
        </RateController>
        <RecoveryNode number_of_retries="1" name="FollowPath">
          <FollowPath path="{path}" controller_id="FollowPath"/>
          <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
        </RecoveryNode>
      </PipelineSequence>
      <ReactiveFallback name="RecoveryFallback">
        <GoalUpdated/>
        <SequenceStar name="RecoveryActions">
          <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
          <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
          <Spin spin_dist="1.57"/>
          <Wait wait_duration="5"/>
        </SequenceStar>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>