编写新的 BT 插件

• 概述
• 需求
• 教程步骤
• 为输入/输出端口使用自定义类型
• 在 Groot 2 (PRO) 中可视化黑板的内容

概述

本教程演示如何创建自定义行为树（Behavior Tree，BT）插件。BT 插件作为行为树 XML 中的节点，由 BT Navigator 加载并执行，用于实现导航逻辑。

需求

• ROS 2（二进制安装或源码编译）
• Nav2（包括依赖项）
• Gazebo
• TurtleBot3

教程步骤

1- 创建新的 BT 插件

本示例将创建一个简单的 BT 插件节点，用于在远程服务器上执行动作。我们以 nav2_behavior_tree 包中最简单的动作节点——wait 节点为例进行分析。除动作节点外，还可以创建自定义的装饰器（decorator）、条件（condition）和控制（control）节点。每种节点类型在行为树中扮演不同角色：执行动作、控制流程、检查条件状态，或修改其他节点的输出。

本教程的代码位于 nav2_behavior_tree 包中，即 wait_action 节点。它可作为编写其他动作节点插件的参考。

示例插件继承自基类 nav2_behavior_tree::BtActionNode。该基类是对 BehaviorTree.CPP 中 BT::ActionNodeBase 的封装，简化了基于 ROS 2 动作客户端的 BT 动作节点开发。BtActionNode 既是 BT 动作节点，又通过 ROS 2 动作接口调用远程服务器来完成任务。

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

BtActionNode 类除构造函数外，还提供了 5 个可重写的虚方法。下面介绍编写 BT 动作插件时需要了解的方法。

方法	方法描述	必需？
构造函数（Constructor）	构造函数用于指明与插件对应的 XML 标签名、使用该插件调用的动作服务器名称，以及任何所需的 BehaviorTree.CPP 特殊配置。	是
providedPorts()	用于定义 BT 节点可能拥有的输入和输出端口的函数。这些端口类似于在 BT XML 中通过硬编码值或其他节点的输出端口值定义的参数。	是
on_tick()	行为树执行过程中 tick 到该节点时调用。可用于获取动态更新（如新的黑板值、输入端口值或参数），也可重置动作状态。	否
on_wait_for_result()	节点等待 ROS 2 动作服务器返回结果时调用。可用于检查抢占（preempt）请求、检查超时，或在等待期间执行其他计算。	否
on_success()	ROS 2 动作服务器返回成功结果时调用。返回值即 BT 节点向行为树报告的状态。	否
on_aborted()	ROS 2 动作服务器返回中止（aborted）结果时调用。返回值即 BT 节点向行为树报告的状态。	否
on_cancelled()	ROS 2 动作服务器返回取消（cancelled）结果时调用。返回值即 BT 节点向行为树报告的状态。	否

本教程仅使用 on_tick() 方法。

在构造函数中，需要获取行为树节点的非变量参数。本示例从输入端口获取休眠（sleep）时长。

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 中对应的标签名，稍后注册插件时会用到。构造函数还接收要调用的动作服务器名称字符串。最后是一组配置参数，对于大多数节点插件可以安全地忽略。

随后调用 BtActionNode 构造函数。它以 ROS 2 动作类型为模板参数，因此传入 nav2_msgs::action::Wait 动作消息类型并转发其他参数。BtActionNode 内部实现了 tick() 方法，行为树 tick 该节点时直接调用它，而 on_tick() 则在发送动作目标之前被调用。

在构造函数体内，通过 getInput 获取 wait_duration 输入端口的值——该参数可在每个 wait 节点实例上独立配置。读取的值存入 duration 变量，再赋给 goal_。goal_ 是 ROS 2 动作客户端发送给服务器的目标对象，因此这里将时长设为期望的等待时间，让动作服务器了解具体请求。

providedPorts() 方法用于定义 BT 节点的输入和输出端口。端口可理解为行为树节点从树中访问的参数。本示例只有一个输入端口 wait_duration，可在 BT XML 中为每个 wait 恢复（recovery）实例单独设置。端口类型为 int，默认值 1，名称 wait_duration，描述为 Wait time。

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

行为树 tick 到该节点时调用 on_tick() 方法。对于 wait BT 节点，这里只需递增黑板上的恢复计数器，表示一个恢复（recovery）动作节点被触发。该计数器可用于统计某次导航过程中执行恢复行为的次数。如果输入是动态变量，也可在此记录或更新 goal_ 的等待时长。

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

其余方法本教程未使用，也不强制重写。只有部分 BT 节点插件需要重写 on_wait_for_result() 来检查抢占或超时。若不重写，on_success()、on_aborted() 和 on_cancelled() 将分别默认返回 SUCCESS、FAILURE、SUCCESS。

2- 导出 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 返回一个指向所创建行为树节点的 unique_ptr。构造函数中填入相关信息，并传入函数参数中的 name 和 config，同时定义该 BT 节点要调用的 ROS 2 动作服务器名称——本例中为 Wait 动作。

最后将 builder 交给 factory 注册。传给 factory 的 Wait 是行为树 XML 文件中与此 BT 节点插件对应的名称。下面的示例中，Wait BT XML 节点指定了一个固定值 5 秒的输入端口 wait_duration。

<Wait wait_duration="5"/>

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

要让 BT Navigator 节点发现刚注册的插件，需在配置 YAML 文件的 bt_navigator 节点下列出插件库名称。配置示例如下，注意 plugin_lib_names 中列出的 nav2_wait_action_bt_node。

bt_navigator:
  ros__parameters:
    global_frame: map
    robot_base_frame: base_link
    odom_topic: /odom
    default_nav_to_pose_bt_xml: replace/with/path/to/bt.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_to_pose_bt.xml
    default_nav_through_poses_bt_xml: replace/with/path/to/bt.xml # or $(find-pkg-share my_package)/behavior_tree/my_nav_through_poses_bt.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/>
        <SequenceWithMemory 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"/>
        </SequenceWithMemory>
      </ReactiveFallback>
    </RecoveryNode>
  </BehaviorTree>
</root>

为输入/输出端口使用自定义类型

除标准类型外，nav2_msgs 或 geometry_msgs 中的自定义类型也可用于输入/输出端口。

例如，可在 providedPorts 函数中为端口定义自定义类型：

static PortsList providedPorts()
{
  return providedBasicPorts(
    BT::OutputPort<geometry_msgs::msg::Point>("position", "Position of the robot")
  });
}

在行为树 XML 中使用自定义类型的端口时，需要从字符串进行转换，因为 XML 中的端口值均为字符串，必须转换为代码中对应的数据类型。

例如，自定义类型 geometry_msgs::msg::Point 的转换方式如下：

namespace BT
{
template<>
inline geometry_msgs::msg::Point convertFromString(const StringView key)
{
  // three real numbers separated by semicolons
  auto parts = BT::splitString(key, ';');
  if (parts.size() != 3) {
    throw std::runtime_error("invalid number of fields for point attribute)");
  } else {
    geometry_msgs::msg::Point position;
    position.x = BT::convertFromString<double>(parts[0]);
    position.y = BT::convertFromString<double>(parts[1]);
    position.z = BT::convertFromString<double>(parts[2]);
    return position;
  }
}
}  // namespace BT

有关自定义类型转换的更多信息，可以参考 bt_utils.hpp 或 BT.CPP 文档：Parsing a string。

在 Groot 2 (PRO) 中可视化黑板的内容

使用 Groot 2 Pro 付费版时，可查看 BT 节点黑板（blackboard）的内容。为此，需将自定义输入/输出类型转换为 JSON 格式，转换示例如下：

namespace geometry_msgs::msg
{
BT_JSON_CONVERTER(geometry_msgs::msg::Point, msg)
{
  add_field("x", &msg.x);
  add_field("y", &msg.y);
  add_field("z", &msg.z);
}
}  // namespace geometry_msgs::msg

宏 BT_JSON_CONVERTER 必须放在待转换自定义类型的命名空间内。此外，若自定义类型由其他自定义类型组成，需先转换子类型，再转换父类型。

定义转换后，还需在 providedPorts 函数中注册自定义类型，注册示例如下：

static PortsList providedPorts()
{
  // Register JSON definitions for the types used in the ports
  BT::RegisterJsonDefinition<geometry_msgs::msg::Point>();
  return providedBasicPorts(
    BT::OutputPort<geometry_msgs::msg::Point>("position", "Position of the robot")
  });
}

有关自定义类型转换的更多信息，可以参考 json_utils.hpp 或 BT.CPP 文档：Visualize custom types in the Blackboard

注意：Nav2 中使用的所有自定义类型都已注册在 json_utils.hpp 文件中，可直接使用，无需再次注册。