编写新的导航器插件

本教程演示如何基于 nav2_core::BehaviorTreeNavigator 基类创建自定义行为树导航器插件。

本教程将以「Navigate to Pose」行为树导航器插件为例进行分析。它是 Nav2 的基础导航器，也是 ROS 1 Navigation 的对应实现，用于完成点到点导航。教程基于 ROS 2 Iron 版本分析其代码和结构。后续版本可能有细微调整，但足以帮助你开始编写自定义导航器——我们预计该系统不会出现重大 API 变更。

当你需要使用自定义动作消息定义而非现成的 NavigateToPose 或 NavigateThroughPoses 接口时（例如全覆盖导航或需要额外约束信息），编写自定义导航器会很有用。导航器的职责是从请求中提取信息并传递给行为树/黑板（blackboard），填充反馈和响应，并在必要时维护行为树的状态。实际的导航逻辑由行为树 XML 定义。

环境要求

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

教程步骤

1- 创建新的导航器插件

本示例实现纯点到点导航行为。代码位于 Nav2 的 BT Navigator 软件包 中，即 NavigateToPoseNavigator。该软件包可作为编写自定义插件的参考。

示例插件类 nav2_bt_navigator::NavigateToPoseNavigator 继承自基类 nav2_core::BehaviorTreeNavigator。基类提供了一套用于实现导航器插件的虚方法，由 BT Navigator 服务器在运行时调用或作为对 ROS 2 动作的响应来处理导航请求。

需要注意的是，该类还有一个上层基类 NavigatorBase，用于提供一个非模板化的基类，以便将插件加载到向量中存储，并调用生命周期节点的基本状态转换。它的成员（如 on_XYZ）已在 BehaviorTreeNavigator 中实现并标记为 final，因此用户无法覆盖。编写导航器时需要实现的是 BehaviorTreeNavigator 中的虚方法，而非 NavigatorBase 中的。这些 on_XYZ 方法在 BehaviorTreeNavigator 中处理行为树和动作服务器的样板逻辑，最大限度地减少各导航器实现间的代码重复。例如，on_configure 会创建动作服务器、注册回调、向黑板填充基本信息，然后调用用户定义的 configure 函数来满足额外的自定义需求。

下表列出了这些方法、其描述以及必要性：

虚方法	方法描述	需要重写？
getDefaultBTFilepath()	初始化时调用，获取用于导航的默认 BT 文件路径。可通过参数、硬编码逻辑、哨兵文件等方式指定。	是
configure()	BT 导航器服务器进入 on_configure 状态时调用。应实现导航器激活前的必要操作，如获取参数、设置黑板等。	否
activate()	BT 导航器服务器进入 on_activate 状态时调用。应实现导航器激活前的必要操作，如创建客户端和订阅。	否
deactivate()	BT 导航器服务器进入 on_deactivate 状态时调用。应实现导航器进入非激活状态前的必要操作。	否
cleanup()	BT 导航器服务器进入 on_cleanup 状态时调用。应清理为导航器创建的资源。	否
goalReceived()	动作服务器收到新目标时调用。通过返回值决定接受或拒绝该目标。若接受，可能需要从请求中加载相应参数（如使用哪个 BT）、将请求参数添加到黑板供程序使用，或重置内部状态。	是
onLoop()	行为树循环时周期性调用，用于检查状态，更常见的是向客户端发布动作反馈。	是
onPreempt()	新目标请求抢占当前正在处理的目标时调用。若新目标可行，应对 BT 和黑板进行相应更新，使新请求立即开始处理，无需硬性取消原任务。	是
goalCompleted()	目标完成时调用，用于填充动作结果对象或在任务结束时执行额外检查。	是
getName()	获取此导航器类型的名称。	是

在 Navigate to Pose 导航器中，configure() 方法需要确定存储目标和路径的黑板参数名称。这些参数是 onLoop 中处理反馈的关键值，也是行为树节点之间传递信息的途径。此外，该导航器还有一个特点：会创建一个指向自身的客户端，并订阅 goal_pose 话题，用于处理来自 RViz2 中 Goal Pose 工具的请求。

    bool NavigateToPoseNavigator::configure(
      rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
      std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
    {
      start_time_ = rclcpp::Time(0);
      auto node = parent_node.lock();

      if (!node->has_parameter("goal_blackboard_id")) {
        node->declare_parameter("goal_blackboard_id", std::string("goal"));
      }

      goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();

      if (!node->has_parameter("path_blackboard_id")) {
        node->declare_parameter("path_blackboard_id", std::string("path"));
      }

      path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();

      // Odometry smoother object for getting current speed
      odom_smoother_ = odom_smoother;

      self_client_ = rclcpp_action::create_client<ActionT>(node, getName());

      goal_sub_ = node->create_subscription<geometry_msgs::msg::PoseStamped>(
        "goal_pose",
        rclcpp::SystemDefaultsQoS(),
        std::bind(&NavigateToPoseNavigator::onGoalPoseReceived, this, std::placeholders::_1));
      return true;
    }

黑板 ID 的值与 BT Navigator 提供的里程计平滑器（odometry smoother）一起存储，用于后续填充有意义的反馈。cleanup 方法则负责重置这些资源。本导航器中 activate 和 deactivate 方法未使用。

    bool NavigateToPoseNavigator::cleanup()
    {
      goal_sub_.reset();
      self_client_.reset();
      return true;
    }

在 getDefaultBTFilepath() 中，通过参数 default_nav_to_pose_bt_xml 获取默认行为树 XML 文件——当导航请求未提供 BT 文件时使用该默认文件，并用热加载的行为树初始化 BT Navigator。若参数文件中未提供，则从 nav2_bt_navigator 软件包中获取一个已知合理的默认 XML 文件：

    std::string NavigateToPoseNavigator::getDefaultBTFilepath(
      rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
    {
      std::string default_bt_xml_filename;
      auto node = parent_node.lock();

      if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
        std::string pkg_share_dir =
          ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
        node->declare_parameter<std::string>(
          "default_nav_to_pose_bt_xml",
          pkg_share_dir +
          "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
      }

      node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);

      return default_bt_xml_filename;
    }

收到目标后，需要判断其是否有效以及是否应处理。goalReceived 方法接收 goal 参数并返回是否处理该目标，该返回值会发回动作服务器以通知客户端。这里需确保目标的行为树有效，否则无法继续。若有效，则将目标位姿初始化到黑板上并重置部分状态，以便干净地处理新请求。

    bool NavigateToPoseNavigator::goalReceived(ActionT::Goal::ConstSharedPtr goal)
    {
      auto bt_xml_filename = goal->behavior_tree;

      if (!bt_action_server_->loadBehaviorTree(bt_xml_filename)) {
        RCLCPP_ERROR(
          logger_, "BT file not found: %s. Navigation canceled.",
          bt_xml_filename.c_str());
        return false;
      }

      initializeGoalPose(goal);

      return true;
    }

目标完成后，若有必要则填充动作结果。本导航器在导航请求成功完成时不包含任何结果信息，因此该方法为空。其他类型的导航器可填充 result 对象中的内容。

    void NavigateToPoseNavigator::goalCompleted(
      typename ActionT::Result::SharedPtr /*result*/,
      const nav2_behavior_tree::BtStatus /*final_bt_status*/)
    {
    }

当目标被抢占时（例如现有请求处理过程中收到新的动作请求），会调用 onPreempt() 方法来判断新请求是否适合抢占当前目标。例如，若抢占请求与现有行为树任务在本质上完全不同，或现有任务优先级更高，则接受抢占请求可能并不合适。

    void NavigateToPoseNavigator::onPreempt(ActionT::Goal::ConstSharedPtr goal)
    {
      RCLCPP_INFO(logger_, "Received goal preemption request");

      if (goal->behavior_tree == bt_action_server_->getCurrentBTFilename() ||
        (goal->behavior_tree.empty() &&
        bt_action_server_->getCurrentBTFilename() == bt_action_server_->getDefaultBTFilename()))
      {
        // if pending goal requests the same BT as the current goal, accept the pending goal
        // if pending goal has an empty behavior_tree field, it requests the default BT file
        // accept the pending goal if the current goal is running the default BT file
        initializeGoalPose(bt_action_server_->acceptPendingGoal());
      } else {
        RCLCPP_WARN(
          logger_,
          "Preemption request was rejected since the requested BT XML file is not the same "
          "as the one that the current goal is executing. Preemption with a new BT is invalid "
          "since it would require cancellation of the previous goal instead of true preemption."
          "\nCancel the current goal and send a new action request if you want to use a "
          "different BT XML file. For now, continuing to track the last goal until completion.");
        bt_action_server_->terminatePendingGoal();
      }
    }

这里可以看到 initializeGoalPose 方法的调用。该方法在黑板上设置目标参数，并重置重要的状态信息，以便干净地复用行为树而不受旧状态影响，代码如下：

    void
    NavigateToPoseNavigator::initializeGoalPose(ActionT::Goal::ConstSharedPtr goal)
    {
      RCLCPP_INFO(
        logger_, "Begin navigating from current location to (%.2f, %.2f)",
        goal->pose.pose.position.x, goal->pose.pose.position.y);

      // Reset state for new action feedback
      start_time_ = clock_->now();
      auto blackboard = bt_action_server_->getBlackboard();
      blackboard->set<int>("number_recoveries", 0);  // NOLINT

      // Update the goal pose on the blackboard
      blackboard->set<geometry_msgs::msg::PoseStamped>(goal_blackboard_id_, goal->pose);
    }

恢复计数器和开始时间是客户端了解当前任务状态的重要反馈项（如任务是否失败、遇到问题或耗时异常）。目标设置到黑板上后，会被 ComputePathToPose BT 动作节点用于规划到达目标的路径（随后路径通过先前设置的黑板 ID 传递给 FollowPath BT 节点）。

实现的最后一个函数是 onLoop，为了教程目的下面做了简化。虽然在这个方法中可以做任何事情（它在 BT 循环遍历树时被调用），但通常利用这个机会来填充客户端可能感兴趣的关于导航请求、机器人或元数据的任何必要反馈。

    void NavigateToPoseNavigator::onLoop()
    {
      auto feedback_msg = std::make_shared<ActionT::Feedback>();

      geometry_msgs::msg::PoseStamped current_pose = ...;
      auto blackboard = bt_action_server_->getBlackboard();
      nav_msgs::msg::Path current_path;
      blackboard->get<nav_msgs::msg::Path>(path_blackboard_id_, current_path);

      ...

      feedback_msg->distance_remaining = distance_remaining;
      feedback_msg->estimated_time_remaining = estimated_time_remaining;

      int recovery_count = 0;
      blackboard->get<int>("number_recoveries", recovery_count);
      feedback_msg->number_of_recoveries = recovery_count;
      feedback_msg->current_pose = current_pose;
      feedback_msg->navigation_time = clock_->now() - start_time_;

      bt_action_server_->publishFeedback(feedback_msg);
    }

2- 导出导航器插件

既然我们已经创建了自定义导航器，就需要导出插件，以便 BT Navigator 服务器能够发现它。插件在运行时动态加载，如果未正确导出，服务器将无法加载。在 ROS 2 中，插件的导出和加载由 pluginlib 处理。

回到本教程，类 nav2_bt_navigator::NavigateToPoseNavigator 将作为基类 nav2_core::NavigatorBase 的子类被动态加载。

1. 导出导航器需要以下两行代码：

 #include "pluginlib/class_list_macros.hpp"
 PLUGINLIB_EXPORT_CLASS(nav2_bt_navigator::NavigateToPoseNavigator, nav2_core::NavigatorBase)

这里需要借助 pluginlib 来导出插件类。pluginlib 提供的宏 PLUGINLIB_EXPORT_CLASS 会完成所有导出工作。

建议将这些代码放在文件末尾，但放在文件顶部也可以。

1. 下一步在软件包的根目录中创建插件描述文件。例如本教程包中的 navigator_plugin.xml 文件，该文件包含以下信息：

• library path：插件库的名称及其位置。
• class name：类的名称（可选）。如果未设置，将默认为 class type。
• class type：类的类型。
• base class：基类的名称。
• description：插件的描述。

  <library path="nav2_bt_navigator">
    <class type="nav2_bt_navigator::NavigateToPoseNavigator" base_class_type="nav2_core::NavigatorBase">
      <description>
        This is pure point-to-point navigation
      </description>
    </class>
  </library>

1. 下一步在 CMakeLists.txt 中使用 CMake 函数 pluginlib_export_plugin_description_file() 导出插件。该函数将插件描述文件安装到 share 目录，并设置 ament 索引使其可被发现。

  pluginlib_export_plugin_description_file(nav2_core navigator_plugin.xml)

1. 插件描述文件也应添加到 package.xml 中：

  <export>
    <build_type>ament_cmake</build_type>
    <nav2_core plugin="${prefix}/navigator_plugin.xml" />
  </export>

1. 编译后插件即被注册。可通过以下命令验证是否注册成功：

  $ ros2 plugin list

你应该会看到类似于下面的输出：

  nav2_bt_navigator:
        Plugin(name='nav2_bt_navigator::NavigateToPoseNavigator', type='nav2_bt_navigator::NavigateToPoseNavigator', base='nav2_core::NavigatorBase')

接下来，我们将使用这个插件。

3- 通过参数文件传入插件名称

要启用该插件，需按如下方式修改 nav2_params.yaml 文件：

  bt_navigator:
    ros__parameters:
        global_frame: map
        robot_base_frame: base_link
        transform_tolerance: 0.1
        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
        goal_blackboard_id: goal
        goals_blackboard_id: goals
        path_blackboard_id: path
        navigators: ['navigate_to_pose', 'navigate_through_poses']
        navigate_to_pose:
          plugin: "nav2_bt_navigator::NavigateToPoseNavigator" # In Iron and older versions, "/" was used instead of "::"
        navigate_through_poses:
          plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" # In Iron and older versions, "/" was used instead of "::"

在上面的代码片段中，可以看到 nav2_bt_navigator::NavigateToPoseNavigator 导航器映射到其 ID navigate_to_pose。要传递插件专属参数，使用 <plugin_id>.<plugin_specific_parameter> 格式。

4- 运行插件

运行启用 Nav2 的 TurtleBot3 仿真。详细步骤请参阅快速入门，快捷命令如下：

  $ ros2 launch nav2_bringup tb3_simulation_launch.py params_file:=/path/to/your_params_file.yaml

然后进入 RViz，点击顶部的“2D Pose Estimate”按钮，按照 快速入门 中描述的方式在地图上指定位置。机器人完成定位后，点击“Nav2 Goal”，再点击目标位姿。导航器随后接管，按照所提供的行为树 XML 文件中定义的逻辑执行导航。