编写新的行为树插件

• 概述

• 要求

• 教程步骤

概述

本教程将介绍如何创建自己的行为树 (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 动作插件所需的方法。

method	Method description	Required?
Constructor	Constructor to indicate the corresponding XML tag name to that matches the plugin, the name of the action server to call using the plugin, and any BehaviorTree.CPP special configurations required.	Yes
providedPorts()	A function to define the input and output ports a BT node may have. These are analogous to parameters that are defined in the BT XML by hardcoded values or by the value of output ports of other nodes.	Yes
on_tick()	Method is called when this BT node is ticked by the behavior tree while executing. This should be used to get dynamic updates like new blackboard values, input ports, or parameters. May also reset state for the action.	No
on_wait_for_result()	Method is called when the behavior tree node is waiting for a result from the ROS 2 action server it called. This could be used to check for updates to preempt the current task, check for a timeout, or anything to compute while waiting for the action to complete.	No
on_success()	Method is called when the ROS 2 action server returns a successful result. Returns the value the BT node will report back to the tree.	No
on_aborted()	Method is called when the ROS 2 action server returns an aborted result. Returns the value the BT node will report back to the tree.	No
on_cancelled()	MMethod is called when the ROS 2 action server returns a cancelled result. Returns the value the BT node will report back to the tree.	No

对于本教程，我们将仅使用“on_tick()”方法。

在构造函数中，我们需要获取适用于行为树节点的任何非变量参数。 在此示例中，我们需要从行为树 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”，可以在 BT XML 中为“wait”恢复的每个实例设置。 我们设置了类型“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 Navigator 服务器将无法加载或使用它们。 在 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 将返回一个指向我们创建的行为树节点的唯一指针。 在构造函数中填写相关信息，为其提供函数参数中给出的``name``和``config``。 然后定义此 BT 节点将调用的 ROS 2 动作服务器的名称，在本例中，它是``Wait``动作。

我们最终将构建器交给工厂进行注册。 给工厂的``Wait``是行为树 XML 文件中与此 BT 节点插件相对应的名称。 下面可以看到一个示例，其中``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 Navigator 的配置 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>