添加平滑器（Smoother）

概述
环境要求
教程步骤

smoothing_path

概述

本教程介绍如何在行为树中添加平滑器（Smoother），对规划器输出的路径进行平滑处理。如果你刚开始接触 ROS 和 Nav2，建议先完成快速入门。

环境要求

需要安装 Nav2 和 TurtleBot3。如果尚未安装，请按照快速入门操作。

此外，还需要一份可供编辑的行为树（例如 Nav2 BT Navigator 包提供的行为树），以及系统所用的 nav2_params.yaml 文件副本。

教程步骤

0- 熟悉 Smoother BT 节点

SmoothPath（bt_smooth_action）是一个行为树节点，用于与平滑器任务服务器交互，使用方式与规划器和控制器服务器节点类似。它内部包含用于调用服务器的 action 客户端，并将相关参数和返回类型暴露为 BT 端口（port）。该节点通过 action 接口与服务器通信，其他服务器和客户端库也可以独立使用这一接口。

请查看该 BT 节点的配置页面以了解全部细节。重点关注两个端口：unsmoothed_path 输入端口接收规划算法输出的原始路径，smoothed_path 输出端口在平滑完成后写入平滑后的路径。此外还有其他端口，可完整实现平滑器服务器（Smoother Server）的 action API。

1- 指定平滑器插件

在行为树中使用平滑器之前，必须先配置平滑器服务器，加载所需的平滑器插件。这些插件实现了具体的平滑算法。

每个平滑器插件都需要指定一个名称（例如 simple_smoother、curvature_smoother），即它的 smoother_id。其他服务器通过该 ID 向平滑器服务器发起 action 请求来调用对应算法。

在每个名称下，需要配置该算法的具体参数，以及 pluginlib 加载所需的 plugin 名称。下面是一个包含 2 个平滑器插件的配置示例，可直接用于机器人的 nav2_params.yaml：

smoother_server:
  ros__parameters:
    costmap_topic: global_costmap/costmap_raw
    footprint_topic: global_costmap/published_footprint
    robot_base_frame: base_link
    transform_tolerance: 0.1
    smoother_plugins: ["simple_smoother", "curvature_smoother"]
    simple_smoother:
      plugin: "nav2_smoother::SimpleSmoother"
      tolerance: 1.0e-10
      do_refinement: True
    curvature_smoother:
      plugin: "nav2_smoother::SavitzkyGolaySmoother"

2- 修改 BT XML

平滑器服务器和插件配置好后，接下来就可以在行为树中调用平滑器来完成导航流程了。虽然行为树中很多位置都可以插入平滑器，但下面展示的是最常见的场景——先对规划器返回的路径进行平滑，再用平滑后的路径进行跟踪。

提示：如果只使用一种平滑算法，则无需在 BT XML 中指定 smoother_id，未显式指定插件名称的请求会自动使用该算法。但如果使用多个平滑器插件，则必须填写 smoother_id XML 端口。

行为树中会有一行调用规划器服务器、将路径写入 path 黑板变量：

<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>

将其替换为以下内容，依次完成路径计算、路径平滑，并将平滑后的路径写回 path 变量供后续使用：

<Sequence name="ComputeAndSmoothPath">
  <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased" error_code_id="{compute_path_error_code}" error_msg="{compute_path_error_msg}"/>
  <SmoothPath unsmoothed_path="{path}" smoothed_path="{path}" error_code_id="{smoother_error_code}" error_msg="{smoother_error_msg}"/>
</Sequence>

提示：在 PipelineSequence 节点内使用该序列时，建议将平滑路径映射到另一个黑板变量（例如 smoothed_path="{smoothed_path}"）。

PipelineSequence 在任一子节点返回 RUNNING 时会重新 tick 所有先前的子节点。如果 FollowPath 返回 RUNNING，PipelineSequence 会重新 tick ComputePathToPose，后者会尝试覆盖 FollowPath 正在使用的 path 变量。

如果需要对平滑器错误码进行恢复（recovery）处理，例如在行为树中触发系统恢复分支：

<Sequence name= "TryToResolveSmootherErrorCodes">
  <WouldASmootherRecoveryHelp error_code="{smoother_error_code}" error_msg="{smoother_error_msg}">
  <!-- recovery to resolve smoother error code goes here -->
<Sequence/>

完成上述修改后，即可编译并在系统中使用该行为树。此时规划路径已经过平滑，控制器会跟随平滑后的路径。本教程顶部的图片展示了 NavFn 的未平滑路径（红色）与平滑后路径（黑色）的对比，可以注意到平滑后路径在接近目标点、转弯处以及直线段过渡区域的变化。如果使用上下文相关错误码，请确保在 BT Navigator 配置中添加平滑器错误码前缀。

如果只想查看平滑效果但不跟随平滑后的路径，可以移除 smoothed_path="{path}" 部分，这样仍会计算平滑路径但不替换原始路径。此时，话题 /smoothed_path 会发布平滑器服务器的输出，可供可视化或系统其他部分使用。也可以将平滑路径映射到另一个黑板变量，在行为树的其他部分引用（例如 smoothed_path="{smoothed_path}"）。