ROS2与Navigation2入门教程-编写新的行为树（Behavior Tree）插件

说明：

• 介绍如何编写新的行为树（Behavior Tree）插件

概述

• 本教程将会说明如何创建自己的行为树（BT）插件。

• 在由BT导航仪（Navigator）处理的行为树XML中，BT插件会用作导航逻辑节点

要求

• 要求在本地机器上已经安装或构建好了以下软件包：

  • ROS 2（二进制安装或从源代码构建）
  • Nav2（包括依赖包）
  • Gazebo
  • Turtlebot3

具体步骤

创建一个新的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动作插件所需方法的更多信息。

• 本教程中仅会使用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()可以用来定义输入或输出端口。端口可以​​视作行为树节点能从行为树本身访问的参数。

• 对于本示例，只有一个输入端口，即可以在BT XML中为每个wait恢复器实例设置的 wait_duration。

• 这里设置了其类型int，默认值1，名称wait_duration，以及该端口的描述信息“wait time”。

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

• 当行为树勾选了特定节点时会调用on_tick()方法。对于wait BT节点，仅仅是想要通知黑板上的一个计数器，相应恢复器的一个动作插件被勾选了。

• 这对于保留某次具体导航运行期间执行的恢复器数量指标很有用。

• 如果是一个可变的输入，还可以记录或更新goal_等待持续时间。

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

• 本教程中不会使用其余方法，而且也不强制要求覆盖它们。只有一些BT节点插件要求覆盖on_wait_for_result()方法以检查抢占或检查超时。如果没有被覆盖，则success、aborted和cancelled方法将会分别返回默认值SUCCESS、FAILURE、SUCCESS。

导出BT插件

• 现在已经创建好了自定义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"动作。

• 最后会将builder交给一个factory进行注册。给factory的Wait是行为树XML文件中对应这个BT节点插件的名称。

• 下面是一个示例，其中BT XML节点Wait指定了5秒的非可变输入端口 wait_duration。

<Wait wait_duration="5"/>

将插件库名称添加到配置（config）中

• 为了让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

运行自定义插件

• 现在可以将行为树与您的自定义BT节点一起使用了。

• 例如，如下所示的navigate_w_replanning_and_recovery.xml文件。

• 可以在NavigateToPose中的特定导航请求中选择此BT XML文件，或选择此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>

参考：

• https://navigation.ros.org/plugin_tutorials/docs/writing_new_bt_plugin.html