编写新的行为树插件 [待校准@1988]

概述
必要条件
教程步骤

概述

本教程演示如何创建你自己行为树 (BT) 插件。BT插件作为节点行为树XML处理BT导航仪导航逻辑。 [待校准@1993]

必要条件

ROS 2 (二进制或从源代码构建)
Nav2 (包括依赖项) [待校准@1996]
Gazebo [待校准@1997]
Turtlebot3 [待校准@1998]

教程步骤

1创建新BT插件 [待校准@2000]

我们将创建一个简单的BT插件节点来在另一个服务器上执行操作。在这个例子中，我们将分析 nav2_behavior_tree 包中最简单的行为树动作节点， wait 节点。除了操作BT节点的示例之外，您还可以创建自定义装饰器、条件和控制节点。每个节点类型在行为树中都具有独特的作用，以执行诸如规划、控制BT流、检查条件状态或修改其他BT节点的输出之类的操作。 [待校准@2001]

本教程中的代码可以在 nav2_behavior_tree 包中找到，作为 wait_action 节点。可以将此操作节点视为编写其他操作节点插件的参考。 [待校准@2002]

我们的示例插件继承自基类 “nav2 _ 行为 _ 树::BtAction节点 . The base class is a wrapper on the BehaviorTree.CPP `` ::Action节点基本”，简化了利用ROS 2动作客户端的BT动作节点。 ``BTActionNode 既是BT动作，也是使用ROS 2动作网络接口调用远程服务器进行一些工作。 [待校准@2003]

与其它类型BT节点 (e。g.装饰、控制、状态) 使用相应BT节点，''BT: DecoratorNode''，''BT: ControlNode''，或 ''BT: ConditionNode''。BT动作节点 * 不 * 利用ROS 2动作接口，使用 ''BT: ActionNodeBase'' 基类本身。 [待校准@2004]

的 BTActionNode 类提供虚拟方法使用，除了信息提供构造函数。让学习更方法需要写BT动作插件。 [待校准@2005]

方法 [待校准@2006]	方法描述	Required需要吗？** [待校准@2008]
构造函数 [待校准@2009]	构造函数，用于指示与插件匹配的相应XML标记名称、使用插件调用的操作服务器的名称以及所需的任何行为树.CPP特殊配置。 [待校准@2010]	是 [校准@小鱼]
providedPorts() [待校准@2012]	函数定义输入和输出端口BT节点可能。这些类似于参数定义BT XML硬编码值或其他输出端口其他节点。 [待校准@2013]	是 [校准@小鱼]
On_tic() [待校准@2014]	方法当BT节点勾选行为时执行。这应该使用动态更新如新黑板值，输入端口，或参数。可能也复位状态动作。 [待校准@2015]	否 [校准@小鱼]
on_wait_for_result() [待校准@2017]	当行为树节点等待其调用的ROS 2操作服务器的结果时，将调用方法。这可用于检查更新以抢占当前任务、检查超时或等待操作完成时要计算的任何内容。 [待校准@2018]	否 [校准@小鱼]
on_success() [待校准@2019]	方法当ROS 2动作服务器返回成功结果。返回值BT节点报告树。 [待校准@2020]	否 [校准@小鱼]
on_aborted() [待校准@2021]	当ROS 2操作服务器返回中止结果时，将调用方法。返回BT节点将向树报告的值。 [待校准@2022]	否 [校准@小鱼]
on_cancelled() [待校准@2023]	MMethod当ROS 2动作服务器返回取消结果。返回BT节点将向树报告的值。 [待校准@2024]	否 [校准@小鱼]

在本教程中，我们将仅使用 on_tick() 方法。 [待校准@2025]

在构造函数中，我们需要获取应用于行为树节点的任何非变量参数。在此示例中，我们需要从行为树XML的输入端口获取睡眠时间的值。 [待校准@2026]

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节点插件stringXML中对应节点。这将看到后我们登记这BT节点插件。它也在string名称动作服务器将调用执行一些行为。最后，配置，我们可以忽略的最节点插件。 [待校准@2027]

然后我们调用 BTActionNode 构造函数。可以看出，它是由ROS 2操作类型模板化的，所以我们给它 “nav2_msgs::action:: wait” 操作消息类型，并转发我们的其他输入。 BTActionNode 作为 tick() 方法，当从树中调用该节点时，行为树直接调用该方法。然后， on_tick() 与行动客户目标一起被调用。 [待校准@2028]

在构造函数的主体，我们输入端口 getInput 参数 wait_duration 可独立配置的每个例证 wait 节点树。它是在 duration 参数插入 goal_ 。的 goal_ 类变量目标ROS 2行动客户端将发送到操作服务器。所以在这个例子，我们设置时间我们想等使动作服务器知道细节的请求。 [待校准@2029]

的 providedPorts() 方法给我们机会定义输入或输出端口。端口可以认为参数行为树节点访问行为树本身。例如，只有一个输入端口， wait_duration 可以把它BT XML每个实例 wait 恢复。我们设置类型， int ，默认 1 ， wait_duration ，描述港口 Wait time 。 [待校准@2030]

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

当行为树勾选特定节点时，将调用 on_tick() 方法。对于等待BT节点，我们只想通知黑板上的计数器，勾选了与恢复相对应的操作插件。这对于保持特定导航运行期间执行的恢复数量的度量很有用。如果是可变输入，您也可以记录或更新 goal_ 的等待时间。 [待校准@2031]

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

不使用其余方法，也不强制重写它们。只有一些BT节点插件需要覆盖 on_wait_for_result() 来检查抢占或检查超时。如果不重叠，成功、中止和取消的方法将分别默认为 SUCCESS 、 FAILURE 、 SUCCESS 。 [待校准@2032]

2-导出planner插件

现在我们创建定制BT节点，我们需要导出插件也能见的行为树时加载定制BT XML。插件加载运行时如果不可见，然后我们BT导航仪服务器将无法加载或使用它。在BehaviorTree.CPP，导出和加载插件由 BT_REGISTER_NODES 微距。 [待校准@2034]

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动作服务器的名称，在这种情况下，是 wWait 的动作。 [待校准@2035]

我们最后给生成器工厂登记。 Wait 给工厂名称在行为树XML文件对应BT节点插件。可以看到一个示例下面， Wait BT XML节点指定非变量输入端口 wait_duration 5秒。 [待校准@2036]

<Wait wait_duration="5"/>

3-将插件库名称添加到配置 [待校准@2037]

为了使BT Navigator节点发现我们刚刚注册的插件，我们需要在configuration YAML文件的bt_navigator节点下列出插件库名称。配置应类似于下面显示的配置。请注意plugin_lib_names下列出的nav2_wait_action_bt_node。 [待校准@2038]

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运行定制插件 [待校准@2039]

现在，您可以在自定义BT节点上使用行为树。例如， navigate_w_replanning_and_recovery.xml 文件如下所示。 [待校准@2040]

选择此BT XML文件特定导航请求 NavigateToPose 或默认行为树BT导航仪配置yaml文件。 [待校准@2041]

<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>