为 BT 添加平滑器

概述
要求
教程步骤

概述

本教程介绍如何向行为树添加平滑器，以平滑路径规划器的输出路径。在完成本教程之前，强烈建议您先完成：ref:getting_started，特别是如果您是 ROS 和 Nav2 新手。

要求

您必须安装 Nav2、Turtlebot3。如果您尚未安装它们，请按照：ref:getting_started 操作。您还必须有一个工作行为树，例如 Nav2 BT Navigator 包提供的行为树，以供编辑。您还应该有一份 nav2_params.yaml 文件的副本，供您的系统编辑。

教程步骤

0-熟悉 Smoother BT 节点

SmoothPath BT 节点是一个行为树节点，它与更平滑的任务服务器交互，类似于您可能期望在规划器或控制器服务器中找到的节点。它包含调用服务器的操作客户端，并将其参数和返回类型指定为 BT 端口。它也通过操作接口调用服务器，该接口可以通过其他服务器和客户端库语言单独交互。

请参阅 BT 节点的配置页面以熟悉所有方面，但要注意的核心端口是“unsmoothed_path”输入端口和“smoothed_path”输出端口。前者从规划算法中获取原始路径，后者将设置平滑后输出路径的值。还有其他端口可用，可以完全实现 Smoother Server 的操作 API。

1- 指定更平滑的插件

为了在您的 BT 节点中使用平滑器，您必须首先配置平滑器服务器本身以包含感兴趣的平滑器插件。这些插件实现了您想要使用的特定算法。

对于您想要使用的每个平滑器插件，必须为其指定一个名称（例如“simple_smoother”，“curvature_smoother”）。此名称是其“smoother_id”，以便其他服务器从对平滑器服务器的操作界面的请求与此算法进行交互。

在每个名称下，必须指定该特定算法的参数以及 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_timeout: 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_ceres_costaware_smoother/CeresCostawareSmoother"

2- 修改您的 BT XML

现在您已经为给定的插件选择并配置了平滑器服务器，是时候在行为树中使用这些平滑器来实现导航行为了。虽然在 BT 中有很多地方/方法可以使用它，但下面显示的情况可能是您最可能想要使用平滑器的情况（平滑路径规划器返回的路径，然后使用该平滑路径进行路径跟踪）。

注意：如果您只使用一种类型的平滑算法，则无需在 BT XML 条目中指定“smoother_id”。由于只有一个选项，它将用于任何未明确请求平滑器插件的请求。但是，如果您利用多个平滑器插件，则**必须**填充“smoother_id”XML 端口。

给定的行为树将有一行：

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

这一行调用规划器服务器并返回行为树中“path”黑板变量的路径。我们将用下面的代码替换这一行，以计算路径、平滑路径，最后用系统现在将与之交互的新平滑路径替换“path”黑板变量：

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

如果您希望恢复更平滑的错误代码，例如触发行为树的系统恢复分支：

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

就这么简单！您现在可以在系统中编译或使用此行为树，并看到计划现在已平滑，控制器现在正在跟踪此平滑路径。本教程顶部的图像显示了 NavFn 的未平滑路径（红色）和平滑路径（黑色）。请注意直线段中更平滑的目标、转弯和过渡方法。

如果您希望看到差异，但不跟踪平滑路径，您可能希望删除 smoothed_path="{path}" 部分以计算平滑路径，但不将其替换为原始路径。相反，主题 /smoothed_path 包含平滑服务器发布的此信息，用于可视化或供系统的其他部分使用。您还可以将平滑路径重新映射到另一个黑板变量，以便在行为树的其他部分与其交互（例如 smoothed_path="{smoothed_path}"）。