Nav2通用教程-4

Groot 是 BehaviorTree.CPP 库的伴侣应用程序，用于创建、编辑和可视化行为树。行为树被深度整合到Nav2中，作为在复杂导航和自治堆栈中编排任务服务器逻辑的主要方法。行为树简称为BTs，由许多节点组成，完成不同的任务并控制逻辑流程，类似于层次结构或有限状态机，但以树形结构组织。这些节点的类型包括：动作（Action）、条件（Condition）、控制（Control）或修饰器（Decorator），详细描述在导航概念 Navigation Concepts 和 BehaviorTree.CPP 中。

如果对创建新的BT节点感兴趣，编写新的行为树插件Writing a New Behavior Tree Plugin 提供了一个良好的示例，展示了如何创建一个简单的动作节点。本教程将专注于启动Groot，可视化行为树，并根据特定的定制修改该树，假设有一个BT节点库。幸运的是，Nav2提供了大量的BT节点供您直接使用，在导航插件Navigation Plugins中列举了这些节点。

BehaviorTree.CPP中的BT配置文件是一个XML文件。这用于在运行时动态加载BT节点插件，从相应的库中映射到它们的名称。XML格式在这里被详细定义。因此，Groot需要一个节点列表，以及关于它们的重要元数据，例如它们的类型和端口（或参数）。在教程后期我们称之为节点的“调色板”（Groot作为一个行为树编辑和可视化工具，需要获取关于这些行为树节点的列表，以及有关这些节点的重要元数据，例如它们的类型、端口或参数等信息。这些信息形成了Groot中所谓的“调色板”，用于在编辑和构建行为树时供用户选择和配置节点）。

在上面的视频中，您可以看到Groot与RVIz和一个100%配备了来自SIEMENS的ROS启用硬件的测试平台并排。Groot不仅在机器人运行时显示当前的行为树。注意：在ROS 2 Humble之前，Nav2支持在执行过程中实时监视Groot行为树。由于BT.CPP / Groot在动态更改行为树方面存在问题，这一功能已在后续版本中移除，请参阅Galactic到Humble Galactic to Humble 了解更多详情。

Visualize Behavior Trees

要显示类似图3 Figure 3 中的行为树，首先要启动Groot可执行文件。Groot默认情况下只能显示BT.CPP中默认的行为树和节点，因为它不了解Nav2或您的其他项目。因此，我们必须将Groot指向我们的调色板或Nav2/自定义行为树节点的索引：

1. 在编辑器模式下打开Groot。此时，Groot的界面应该类似于图1 Figure 1 中的样子。

2. 或菜单栏顶部中间的导入图标，选择“从文件加载调色板”选项。

3. 打开文件/path/to/navigation2/nav2_behavior_tree/nav2_tree_nodes.xml，导入用于导航的所有自定义行为树节点。这是Nav2自定义行为树节点的调色板。此时，Groot的界面应该类似于图2 Figure 2 中的样子。

4. 在左上角附近选择“加载树”选项。

5. 浏览要可视化的行为树，然后选择“确定”。Nav2中的行为树位于/path/to/navigation2/nav2_bt_navigator/behavior_trees/。

这样您就可以在Groot中加载和可视化所需的行为树了。

如果选择默认的树 navigate_w_replanning_and_recovery.xml，那么 Groot 编辑器应该会呈现如图3所示的界面。

如果某个树无法被可视化，可能是因为调色板中缺少某些节点，你可能需要将其添加到调色板中。尽管我们努力保持 Nav2 的 BT 节点和调色板同步，但如果你发现某个节点丢失，请提交工单或拉取请求，我们会尽快进行更新。

Edit Behavior Trees

现在你已经在编辑模式下打开了 Groot 中的 Nav2 BT，你可以轻松地使用图形界面进行修改。从类似于图3 Figure 3 所示的屏幕开始，你可以从侧边栏拖拽新的节点添加到工作区。然后，你可以使用“拖放”操作将节点的输入和输出端口连接起来，将新节点组装到树中。

如果选择特定节点，你可以更改有关该节点的元数据，比如名称或可参数化端口的值。完成修改后，只需保存新的配置文件，下次在机器人上使用即可！

Adding A Custom Node

行为树中的每个节点都具有特定的功能。有时，在设计过程中创建新节点并将其添加到调色板中是很有用的，也许在实现本身存在之前。这有助于设计者将节点的实现细节从树本身的高级逻辑中抽象出来，以及他们希望如何与给定节点交互（例如类型、端口等）。在 Groot 中，你可以创建新的自定义节点并将其添加到树中，然后将这些新节点导出到调色板中。节点的实现需要单独完成，这在《编写新的行为树插件》 Writing a New Behavior Tree Plugin 中有所描述。

创建新的自定义节点可以在 Groot 处于编辑模式时，点击图 4 Figure 4 中标记为橙色的图标开始。这会打开一个类似图 5 Figure 5 的新窗口。在这个新窗口中，它会询问您填写关于这个新节点的元数据，以便创建它。它会询问您标准信息，如名称（绿色框），节点类型（橙色框），以及任何可选的端口，用于参数化或访问黑板变量（蓝色框）。

完成后，在图 5 Figure 5 中选择“确定”，新的自定义节点应该会出现在 TreeNode 调色板中，如图 6 Figure 6 中的蓝色部分所示。

在开始创建基于新自定义节点的新行为树之前，建议将新创建的节点导出，以防 Groot 崩溃时丢失。这可以通过图 6 Figure 6 中突出显示的绿色标记图标来执行。下面是从图 5 Figure 5 中创建的节点生成的 XML 输出示例。您可以在 Nav2 的 BT 节点调色板 XML Nav2’s BT Node Palette XML 中查看更多示例。

<root>
    <TreeNodesModel>
        <Action ID="MyAwesomeNewNode">
            <input_port name="key_name" default="false">coffee</input_port>
            <output_port name="key_name2" default="42">Sense of life</output_port>
            <inout_port name="next_target" default="pancakes">rolling target</inout_port>
        </Action>
    </TreeNodesModel>
</root>

Camera Calibration相机标定

Overview

这个教程展示了如何获取单目摄像头的校准参数。

Requirements

1- 使用操作系统的软件包管理器安装摄像头校准解析器（Camera Calibration Parser）、摄像头信息管理器（Camera Info Manager）和启动测试 Ament Cmake。

sudo apt install ros-<ros2-distro>-camera-calibration-parsers
sudo apt install ros-<ros2-distro>-camera-info-manager
sudo apt install ros-<ros2-distro>-launch-testing-ament-cmake

2- 需要在你的工作空间中从源代码构建图像流水线（Image Pipeline），方法如下：

git clone – b <ros2-distro> git@github.com:ros-perception/image_pipeline.git

同时，请确保你拥有以下内容：

一个已知尺寸的大棋盘格。本教程使用一个7x9的格子，每个格子尺寸为20mm。校准使用棋盘格的内部顶点点，因此“8x10”板使用内部顶点参数“7x9”，就像下面的示例一样。带有设定尺寸的棋盘格可以从这里下载。

一个光线充足的区域，没有遮挡物和其他棋盘格图案

一个通过ROS发布图像的单目摄像头

Tutorial Steps

1- 在你的图形用户界面中启动一个终端。

2- 启动你特定摄像头的ROS驱动器。

3- 确保相机正在通过ROS发布图像。可以通过运行以下命令进行测试：

ros2 topic list

4- 这将显示所有发布的话题，请确保存在一个名为 /camera/image_raw 的图像原始话题。要确认它是一个真实存在的话题并且正在发布，请检查话题的频率：

ros2 topic hz /camera/image_raw

5- 启动相机标定节点。

ros2 run camera_calibration cameracalibrator --size 7x9 --square 0.02 --ros-args -r image:=/my_camera/image_raw -p camera:=/my_camera

--size 7x9: 棋盘格的尺寸，指定棋盘格内部顶点的行和列数。
--square 0.02: 棋盘格的方块大小（以米为单位），即每个方块的边长。
--ros-args: 指示后面的参数是ROS特定的参数。
-r image:=/my_camera/image_raw: 这个参数是ROS的重映射参数。image:=/my_camera/image_raw 重映射了相机标定器节点期望的图像话题到实际相机发布图像消息的话题上。
-p camera:=/my_camera: 这个参数也是一个ROS的重映射参数。camera:=/my_camera 重映射了相机标定器节点期望的相机信息话题到实际相机发布相机信息消息的话题上。

Camera Name:

-c, --camera_name
        name of the camera to appear in the calibration file


Chessboard Options:

You must specify one or more chessboards as pairs of --size and--square options.

  -p PATTERN, --pattern=PATTERN
                    calibration pattern to detect - 'chessboard','circles', 'acircles','charuco'
  -s SIZE, --size=SIZE
                    chessboard size as NxM, counting interior corners (e.g. a standard chessboard is 7x7)
  -q SQUARE, --square=SQUARE
                    chessboard square size in meters

ROS Communication Options:

 --approximate=APPROXIMATE
                    allow specified slop (in seconds) when pairing images from unsynchronized stereo cameras
 --no-service-check
                    disable check for set_camera_info services at startup

Calibration Optimizer Options:

 --fix-principal-point
                    fix the principal point at the image center
 --fix-aspect-ratio
                    enforce focal lengths (fx, fy) are equal
 --zero-tangent-dist
                    set tangential distortion coefficients (p1, p2) to
                    zero
 -k NUM_COEFFS, --k-coefficients=NUM_COEFFS
                    number of radial distortion coefficients to use (up to
                    6, default 2)
 --disable_calib_cb_fast_check
                    uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners

    This will open a calibration window which highlight the checkerboard.

6- 为了获得良好的标定，您需要在相机框架中移动棋盘，以确保：

摄像机视野中的棋盘格：

- X 条：位于视野的左侧或右侧
- Y 条：位于视野的顶部或底部
- 大小条：朝向/远离相机，并倾斜
- 填满整个视野的棋盘格
- 倾斜到左侧、右侧、顶部和底部（倾斜）

7- 当棋盘格在视野中移动时，校准侧边栏上的4个条形图长度会增加。当所有4个条形图变为绿色且有足够的数据可用于校准时，“CALIBRATE”按钮将亮起。单击按钮以查看结果。校准过程大约需要一分钟。

8- 校准完成后，保存和提交按钮会点亮。您也可以在终端中看到校准结果。

9- 点击保存按钮查看结果。数据将保存到“/tmp/calibrationdata.tar.gz”。

10- 要使用校准文件，请解压缩 calibration.tar.gz。

tar -xvf calibration.tar.gz

11- 在包含用于校准的图像的文件夹中，还会有“ost.yaml”和“ost.txt”文件。您可以使用包含校准参数的 yaml 文件，按照摄像头驱动程序的指示进行操作。

Get Backtrace in ROS 2 / Nav2ROS 2 / Nav2 中获取回溯信息

Overview

这份文档介绍了一套方法，用于在 ROS 2 和 Nav2 中获取回溯信息。有许多方法可以做到这一点，但对于没有 GDB 经验的新 C++ 开发者来说，这是一个很好的起点。

以下步骤向 ROS 2 用户展示如何修改 Nav2 栈，以在遇到问题时从特定服务器获取跟踪。本教程适用于仿真和实际机器人。

内容包括如何使用 ros2 run 从特定节点获取回溯，如何使用 ros2 launch 表示单个节点的启动文件获取回溯，以及如何从更复杂的节点协作中获取回溯。通过本教程，您应该能够在注意到 ROS 2 中服务器崩溃时获取回溯。

未能有帮助！以后再看！

Profiling in ROS 2 / Nav2
ROS 2 / Nav2 中的性能分析

未能有帮助！以后再看！

Using VIO to Augment Robot Odometry利用视觉惯性里程计（VIO）来增强机器人的里程计

Overview

这个教程重点介绍如何将视觉惯性里程计（VIO）集成到基于 Nav2 和 ROS 2 的机器人系统中，以增强机器人的里程计。

许多现代机器人平台对于高质量的轮式里程计存在不理想的配置。例如，采用履带、麦克纳姆轮或多向轮的轮式机器人，以及腿部机器人，往往会产生次优质量的里程计。这些类型的平台在机器人领域越来越常见，需要增强或替代里程计的来源。此外，一些机器人技术的应用涉及对现有设备进行改装，这些设备可能根本没有里程计。

计算机视觉和机器人学的一个子领域专注于如何利用视觉（例如摄像头）和惯性（例如 IMU）数据，以便计算相对高速的运动，而与机器人的机械结构无关。这对于没有准确固有里程计传感能力的无人机或具有较差固有里程计测量的移动机器人尤其有用。

因此，本教程将引导您将VIO集成到机器人系统中，以替代或增强轮式里程计，使您的机器人能够自主导航，并获得高质量的状态估计，这对于一个能够准确可靠地完成任务的良好设计的系统而言是必要的。

在本教程中，我们将使用Stereolabs Stereolabs SDK的Position Tracking Position Tracking 功能作为我们首选的VIO解决方案，配合新的 ZED X 摄像头。当使用ZED相机模块时，这种VIO解决方案易于使用，并且提供了生产级的性能，免费提供使用（Stereolabs SDK的Position Tracking功能指的是使用立体摄像头（例如ZED相机）的计算机视觉技术，结合惯性测量单元（IMU）数据，实现对相机位置和姿态进行跟踪和估计的功能。这种功能可以将相机相对于其初始位置的运动实时转换为三维坐标，并提供相对于初始位置的旋转和转换信息）。

尽管我们使用Stereolabs SDK和ZED X相机，但本教程也可以广泛适用于其他解决方案。然而，我们推荐这个选项作为一个优化的解决方案，它与相机硬件和Jetson计算架构紧密耦合，具有高性能和快速的开箱即用结果。根据Open Navigation的经验，尝试使用开源的VIO解决方案并解决立体相机ROS驱动程序的时间同步问题可能需要数月的测试，才能达到实用品质的结果。而这个解决方案非常方便地提供了一站式集成解决方案。

（这段话强调了使用Stereolabs SDK和ZED X相机的优势，将其作为一种高效的解决方案，紧密结合相机硬件和Jetson计算架构，能够快速获得性能优异、即插即用的结果。作者指出，使用开源的VIO解决方案和尝试解决立体相机ROS驱动程序的时间同步问题可能需要花费数月的时间来达到实用水平，而所提供的解决方案则方便且集成度高）

我们在本教程中使用ZED X相机，原因如下：

        1. 尺寸较小，类似于其他自主移动机器人（AMR）深度传感器。
        2. 在移动机器人和操作中具有相关范围的高质量深度信息。
        3. 具备硬件同步的IMU（惯性测量单元）。
        4. 在大多数情况下只需要一个摄像头的宽广视场。
        5. 使用GMSL2连接器而非USB（耶！）。
        6. 全局快门相机可提高质量并消除运动模糊。

然而，任何带有IMU的其他ZED相机（ZED X相机具备拍摄深度图像的功能，并且内置IMU（惯性测量单元））也可以使用（例如ZED2、ZED2i、ZED mini等）。如果特别使用ZED X，请查看YouTube上的这个播放列表，逐步了解如何为ROS 2设置Nvidia Jetson和ZED X this playlist on YouTube showing step by step how to setup the Nvidia Jetson and ZED X for ROS 2，或者查看ZED X入门页面 ZED X Getting Started page 以安装SDK、ZED X驱动程序和ROS 2驱动程序以进行后续操作。

到目前为止，您应该能够运行以下命令之一来启动驱动程序并在Rviz中可视化传感器数据。请注意，这些是ROS 2组件节点，也可以加载到系统的组件管理器中以减少由于序列化而产生的延迟。

$ ros2 launch zed_wrapper zedx.launch.py
$ ros2 launch zed_wrapper zedxm.launch.py

截至2023年9月，该驱动程序在默认情况下会自动生成完整的map->odom->base_link->camera树结构。这是因为Pose SDK不仅可以生成VIO，还可以生成闭环VSLAM，表示完整的状态估计TF树。

我们希望能够将其他信息融合到我们的局部或全局状态估计中，例如外部IMU、轮式测距、GPS或其他传感器，因此我们需要禁用map->odom和odom->base_link的TF发布，以便由我们的融合输出提供这些信息。特别是考虑到ZED X相机并不了解base_link帧的性质。但是，如果您想要将ZED的状态估计用于整个系统而不需要进一步的传感器融合，当然可以！（ZED本身就比较完整，但也不是最好的选择，当我们需要进行传感器的融合时，就需要禁用map->odom和odom->base_link的TF发布，以便由我们的融合输出提供这些信息，特别是考虑到ZED X相机并不了解base_link帧的性质）

Setting Up ZED ROS

为了单独运行VIO，我们需要执行以下操作：

停止计算VSLAM的map->odom TF变换（TF（Transform）变换是ROS中用于处理坐标系之间关系的系统。TF库允许用户跟踪多个坐标系之间的关系，例如机器人的底盘坐标系、摄像头坐标系、全局地图坐标系等。TF库提供的变换功能使得用户可以轻松地在不同坐标系之间进行转换和处理，使得机器人能够理解和感知其周围环境的准确位置和方向）。这部分TF树的信息由我们的全局定位解决方案（例如AMCL、GPS、融合的全局状态估计）提供。

禁用VIO发布的odom->camera，并改为发布VIO姿态解的nav_msgs/Odometry消息以供融合使用。默认情况下，zed_wrapper（zed_wrapper是一个ROS包，用于与Stereolabs ZED系列相机进行交互。它提供了与ZED相机通信的ROS节点，允许用户通过ROS操作ZED相机，获取图像、深度数据和相机的姿态信息等。通过zed_wrapper，用户可以将ZED相机集成到ROS系统中，并利用ROS的功能进行数据处理、导航和其他机器人应用）会将其发布在odom主题下，但建议将其重映射到非保留的主题名称（例如，camera_odom）。

重新配置ZED Wrapper的参数，以获得最佳的VIO效果。

        - two_d_mode将强制姿态跟踪在二维尺寸（例如X、Y、偏航）上进行，用于室内或二维应用。
        - pos_tracking_enabled将禁用或启用姿态跟踪，如果您需要的话（我们在这里需要！）。
        - path_max_count将设置姿态随时间的可视化最大大小。默认情况下为无限制。我们应该将其设置为有限值。
        - qos_depth将设置整个驱动程序的QoS深度。将其设置为3-5适用于所有选项。设置为1可能会导致在计算中出现非常临时的消息丢失。3-5允许我们缓冲少量的测量数据，以处理非常短暂的干扰，但在CPU频繁调用时同样会清除它们。

因此，请对zed_wrapper的默认参数进行以下参数更新：

pos_tracking:
    publish_tf: false # 禁用 odom -> base_link 的 TF 变换
    publish_map_tf: true # 禁用 map -> odom 的 TF 变换
    area_memory: false # 禁用回环闭合计算，但仍发布用于全局 VSLAM 的 Pose 主题（现在是纯 VIO）

    # 可选的优化
    two_d_mode: false # 或 true，根据需要选择！
    pos_tracking_enabled: true # 当然！
    path_max_count: 30
    qos_depth: 5 # 质量服务深度设置为 5

可选择地，将zed的odom话题重新映射到一个不受保留或其他系统常用的话题。在您的ZED启动文件中，向节点/启动文件添加：

remappings=[('odom', 'camera_odom')]

ZED驱动程序会发布两个姿态跟踪话题，pose和odom。Pose是具有闭环（如果area_memory：false，则没有）的完整V-SLAM姿态。odom话题包含实际的VIO，我们希望使用它，它以帧捕获速率发布。Pose话题的发布频率可能会根据闭环的情况而不规则变化。因此，我们希望使用odom进行本地融合。

（

这段文字解释了如何在运行Visual-Inertial Odometry（VIO）时进行配置。它涉及到停止计算VSLAM（Visual Simultaneous Localization and Mapping）中map到odom的TF变换。这一部分的TF树由全局定位解决方案提供（例如AMCL、GPS、融合的全局状态估计）。此外，要禁用VIO的odom到camera的发布，转而发布VIO姿态解的nav_msgs/Odometry，供融合使用。建议将其重新映射到一个非保留或其他系统不常用的话题名称上（例如camera_odom）。还需重新配置ZED Wrapper的参数，以获取最佳的VIO效果。设置一些参数，如将pose tracking（姿态跟踪）配置为2D模式、启用或禁用pose tracking、设置路径可视化的最大大小、以及设置QoS深度等。

）

Fusing VIO Into Local State Estimate

现在我们已经设置好了ZED ROS 2驱动程序，将我们的VIO发布到一个话题上，并将TF树留给融合算法和机器人状态发布器（机器人状态发布器（Robot State Publisher）是一个ROS软件包，它的主要作用是根据机器人描述文件（通常是URDF文件）发布机器人模型的状态信息。它利用URDF文件中描述的机器人结构和几何形状信息，发布TF变换和关节状态数据，以便其他ROS节点可以使用这些信息执行传感器融合、导航、控制等任务）（例如URDF），我们终于准备好使用robot_localization软件包将VIO融入到更广泛的状态估计中。

该软件包是一个通用的EKF（扩展卡尔曼滤波器）和UKF（无迹卡尔曼滤波器）解决方案，用于对可能具有不同类型和不同发布速率的多个不同话题进行状态估计。如果您对robot_localization（robot_localization软件包是用于多传感器融合的工具，可以实现在ROS中执行状态估计。尽管它在融合多个传感器提供的数据时会创建和发布TF变换信息，但它并不是专门用来计算TF的。它的主要功能是执行传感器数据的融合，以提供机器人的状态估计，如位置、姿态和速度等）不熟悉，请查看我们的首次机器人设置指南中 First-Time Robot Setup Guide的里程计页面以获取基本信息，以及该软件包的广泛文档package’s extensive documentation。

大多数用户在这一点上已经在其机器人系统中拥有一个robot_localization配置文件，用于将现有的传感器进行融合，例如轮式里程计（即使质量较差）和机器人IMU。我们将向我们的配置文件中添加一个新的odom字段，即odom1，以将VIO的位置和方向融入到我们的滤波器中。如果这是您的第一个里程计字段，请使用odom0，并可以以 ekf.yaml 为基础创建您的文件。

odom1: camera_odom # 如果ZED相机命名空间不同，调整此处名称（例如，/zed/odom）
odom1_config: [true,  true,  true,     # X, Y, Z
               true,  true,  true,     # Roll, Pitch, Yaw
               false, false, false,    # Vx, Vy, Vz
               false, false, false,    # Vroll, Vpitch, Vyaw
               false, false, false]   # Ax, Ay, Az
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2

我们正在融合Roll（横滚）、Pitch（俯仰）和Yaw（偏航）。如果您的扩展卡尔曼滤波器（EKF）或ZED相机处于2D模式下运行，请将Roll和Pitch字段设置为false。如果您认为VIO中可能会发生跳变，请考虑使用`odom1_pose_rejection_threshold`，该参数设置了一个阈值，如果相对于最近的更新来说非常离谱，则拒绝更新。在这种情况下，将`differential`设置为true也可能有所帮助，以便单个错误的更新不会移动整个坐标系。

请确保评估您的扩展卡尔曼滤波器（EKF）的频率、two_d_mode、publish_tf 和关键帧是否适用于您的应用场景。通常情况下，当在仅限于平坦室内环境的导航时，我们希望发布TF并开启2D模式。

Fusing VSLAM Into Global State Estimate融合VSLAM到全局状态估计

尽管超出本教程的范围，但仍然有可能继续使用VSLAM（视觉和惯性测距）结果进行全局定位，包括使用Stereolabs Position Tracking SDK。整合的步骤与上一节类似，但是：

继续禁用map->odom的TF树，但全局姿态主题将继续在pose下发布，以用于融合。

将该主题与其他信息（例如外部IMU、AMCL、GPS等）一起融合到全局定位扩展卡尔曼滤波器（EKF）中，世界框架是map。

多个全局定位技术的融合应该谨慎进行。最可靠的来源应设置为_differential: false，以使用实际的姿态信息。所有其他后续系统应设置为_differential: true，以确保不同的坐标系不会创建出现跳动的解决方案。相反，这将将一个系统作为绝对姿态进行融合，而将另一个系统作为迭代之间姿态变化的融合。

Testing it Out!

在下面的示例中，我们将Stereolabs SDK的Pose Tracking VIO解决方案与机器人的外部IMU和里程计（例如robot_localization有odom0、odom1和imu0）（机器人的内部里程计（通常是基于腿部运动的）、来自相机的视觉里程计以及IMU的惯性里程计）进行融合，以提高在户外环境中腿部机器人平台的导航性能。机器人基于腿部运动的内部里程计相当糟糕，导致机器人的自主导航性能普遍较差。

在这些数据集上，视觉惯性里程计（VIO是视觉惯性里程计（Visual-Inertial Odometry）的缩写，它结合了视觉（通常是相机）和惯性（通常是惯性测量单元IMU）传感器的数据，利用这些数据计算和估计机器人或车辆的运动轨迹、姿态和位置信息。通过结合相机和IMU的数据，VIO可以提供更准确的运动估计，特别是在传统里程计（比如轮式编码器）无法提供准确数据的情况下，比如在复杂环境中或者特殊机器人结构下的导航中）的误差为70米路径上的4.1%。通常对于由轮式编码器 + IMU 提供的“良好”里程计，我希望看到2-3%的全面调整（或者对于“出色”里程计，小于1%），因此这是一个很好的数据源！与腿部机器人的里程计融合在一起，可以将整体性能提高到可接受的水平！

Steve正在使用手柄操控他的机器狗穿过旧金山加利福尼亚州的金门公园收集数据。Steve并不是一个很好的机器人司机（他不玩视频游戏），你看到的蛇行是由于他操控手柄不熟练，再加上四足机器狗上有很多额外的不对称重量。这不代表Nav2，应该被嘲笑。这是为了测试在更恶劣条件下VIO解决方案的准确性……是的……我们就这么说吧。

Dynamic Object Following

Overview

这个教程展示了如何使用 Nav2 来执行不同于从点 A 到点 B 的任务。在这种情况下，我们将使用 Nav2 来无限跟随一个移动对象。

这个任务在跟随人或其他机器人等场景下非常有用。以下是一些示例视频，展示了可能利用这项功能创建的应用场景。包括“Carry My Luggage” RoboCup @ Home 测试，其中 CATIE Robotics 团队成功完成了测试，以及这个真实（未来）世界的应用示例。

这个任务的要求如下：

更改仅限于用于导航的行为树。当需要时，可以在“NavigateToPose”操作中选择这个行为树，或者它可以是默认的行为树。这个行为树由运行时可配置的插件组成。

不会修改规划器和控制器的配置。

该操作将一直运行，直到由启动它的人取消。

检测要跟随的动态对象（如人）不在本教程的范围内。正如下图所示，您的应用程序应该为感兴趣的对象提供一个检测器，将初始姿势发送到“NavigateToPose”操作，并在任务持续时间内更新该姿势的主题。存在许多不同类型的检测器，您可以利用这些检测器进行这个应用程序的开发：

Tutorial Steps

0- Create the Behavior Tree

让我们从这个简单的行为树开始。该行为树以1赫兹的频率重新规划新的路径，并将该路径传递给控制器以进行跟踪：

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      </RateController>
      <FollowPath path="{path}" controller_id="FollowPath"/>
    </PipelineSequence>
  </BehaviorTree>
</root>

首先，让我们让这个行为持续运行直到出现故障。为此，我们将使用KeepRunningUntilFailure控制节点。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

然后，我们将使用修饰器GoalUpdater来接受动态物体姿势的更新，该姿势是我们正在尝试跟踪的。该节点接受当前目标作为输入，并订阅主题/goal_update。如果接收到该主题上的新目标，它将将新目标设置为updated_goal。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
          <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
        </GoalUpdater>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{path}" controller_id="FollowPath"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

为了保持与目标的特定距离，我们将使用动作节点TruncatePath。该节点修改路径，使其变短，以便我们不会尝试导航到感兴趣的对象内部。我们可以使用输入端口distance设置到目标的期望距离。

KeepRunningUntilFailure：一种控制节点，用于持续运行直到出现故障。
GoalUpdater：一个修饰器，用于接受动态物体姿势的更新，通过订阅特定主题并将新的目标设置为更新后的目标。
TruncatePath：一个动作节点，用于调整路径长度，以确保机器人或程序保持在特定距离内追踪目标物体，避免过于靠近或进入目标物体。

<root main_tree_to_execute="MainTree">
  <BehaviorTree ID="MainTree">
    <PipelineSequence name="NavigateWithReplanning">
      <RateController hz="1.0">
        <Sequence>
          <GoalUpdater input_goal="{goal}" output_goal="{updated_goal}">
            <ComputePathToPose goal="{updated_goal}" path="{path}" planner_id="GridBased"/>
          </GoalUpdater>
         <TruncatePath distance="1.0" input_path="{path}" output_path="{truncated_path}"/>
        </Sequence>
      </RateController>
      <KeepRunningUntilFailure>
        <FollowPath path="{truncated_path}" controller_id="FollowPath"/>
      </KeepRunningUntilFailure>
    </PipelineSequence>
  </BehaviorTree>
</root>

现在，你可以保存这个行为树并在我们的导航任务中使用它。

作为参考，在nav2_bt_navigator软件包中，你可以直接使用包含完整行为树的模块。

1- Setup Rviz clicked point

我们将使用RViz而不是完整的应用程序，这样你就可以在家里进行测试，而不需要找到一个检测器来开始。我们将使用工具栏上的“clicked point”按钮来代替对象检测，以提供目标更新给Nav2。这个按钮允许你在主题/clicked_point中发布坐标。这个点需要发送到行为树，使用这个存储库中的程序clicked_point_to_pose。在你的工作空间中克隆这个存储库，构建，并在终端中输入：

ros2 run nav2_test_utils clicked_point_to_pose

可选地，你可以在你的rviz配置文件中重新映射这个主题为goal_updates。

2- Run Dynamic Object Following in Nav2 Simulation

在一个终端中启动 Nav2：

ros2 launch nav2_bringup tb3_simulation_launch.py default_bt_xml_filename:=/path/to/bt.xml

打开 RViz，在初始化机器人位置后，命令机器人导航到任何位置。使用 clicked point 按钮模拟对感兴趣的对象进行新的检测，就像教程开头的视频中演示的那样。

当你的检测器以更高的频率（1赫兹、10赫兹、100赫兹）检测到障碍物时，你会看到机器人更快地跟随检测到的感兴趣对象！

Navigating with Keepout Zones带着禁止区域导航

未能有帮助！以后再看！

Navigating with Speed Limits

未能有帮助！以后再看！

Using Rotation Shim Controller使用旋转隔垫控制器

未能有帮助！以后再看！

Adding a Smoother to a BT给行为树添加平滑器

Overview

这个教程展示了如何向你的行为树添加平滑器，以平滑路径规划器输出的路径。在完成这个教程之前，强烈建议完成 Getting Started ，特别是如果你对ROS和Nav2还不太熟悉的话。

Requirements

你必须安装Nav2和Turtlebot3。如果你还没有安装它们，请按照 Getting Started 进行安装。你还必须有一个可用的行为树，比如由Nav2 BT Navigator软件包提供的行为树，供进行编辑。你还应该有一个用于你系统的nav2_params.yaml文件（nav2_params.yaml是Nav2中用于配置导航和行为的参数文件。这个文件包含了许多参数和设置，用于定义机器人导航和行为执行的方式。你可以在这个文件中配置路径规划器、控制器、传感器配置、全局和局部代价地图等内容，以便根据你的特定需求和机器人设置进行调整和优化）的副本进行编辑。

Tutorial Steps

0- Familiarization with the Smoother BT Node熟悉平滑器BT节点

SmoothPath 行为树节点是与平滑器任务服务器进行交互的行为树节点，类似于你可能会在路径规划器或控制器服务器中找到的功能。它包含调用服务器的动作客户端，并指定了其参数和返回类型作为BT端口。它也通过动作接口调用服务器，可以通过其他服务器和客户端库语言单独进行交互。

请查看BT节点的配置页面，以熟悉所有方面，但需要注意的核心端口是unsmoothed_path输入端口和smoothed_path输出端口。前者接收来自规划算法的原始路径，而后者将设置平滑后输出路径的值。还有其他端口可完全实现平滑器服务器的动作API。

1- Specifying a Smoother Plugin指定一个平滑器插件

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

（

在这个上下文中，行为树的节点（例如SmoothPath节点）是用于在行为树中执行某些操作的组件，而算法则是指用于路径平滑（如平滑输出路径）的具体方法或技术。

行为树的节点不直接执行算法，而是通过调用其他部分或外部服务来实现特定的功能。比如，SmoothPath节点是一个行为树节点，它可能会调用平滑器服务器（即包含各种平滑算法的服务器）来平滑路径。这些算法可能由用户编写或者从库中选择。

因此，在这个情境中，行为树节点是执行控制和调度的部分，而算法是用于执行特定任务（如平滑路径）的具体技术或方法。行为树节点可以调用特定的算法或服务来完成任务，但节点本身并不是实现算法的地方。

）

对于每个你想使用的平滑器插件，必须给它指定一个名称（例如simple_smoother、curvature_smoother）。这个名称是它在其他服务器中与该算法进行交互时的smoother_id。

（

平滑器插件（Smoother Plugins）：这些是用于实现具体平滑算法的模块，它们提供了不同的路径平滑技术，比如简单平滑、曲率平滑等。这些插件通常被加载到平滑器服务器中，供导航系统调用和使用。
平滑器服务器（Smoother Server）：这是一个提供路径平滑功能的服务或组件。平滑器服务器包含了各种平滑器插件，负责管理和执行这些插件中的平滑算法。它允许其他系统组件（如行为树节点）通过调用相应的平滑器插件来实现路径的平滑处理。

）

在每个名称下，必须指定该特定算法的参数，以及用于pluginlib加载给定算法库的插件名称。下面是两个平滑器插件的示例配置，可以用在你机器人的nav2_params.yaml文件中。

smoother_server:  # 平滑器服务器节点配置
  ros__parameters:  # ROS参数
    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- Modifying your BT XML

现在你已经选择并配置了平滑器服务器来使用特定的插件，接下来是在行为树中应用这些平滑器来实现导航行为。尽管有许多地方和方式可以在行为树中使用这些平滑器，在下面展示的情况可能是你想要使用平滑器的最常见情况之一（对路径规划器返回的路径进行平滑，然后使用平滑后的路径进行路径跟踪）。

注：如果你只使用单一类型的平滑算法，那么在BT XML条目中无需指定smoother_id。因为只有一个选项，它将用于任何不明确请求特定平滑器插件的请求。但是，如果你使用了多个平滑器插件，就必须填写smoother_id XML端口。

给定的行为树将有一行：

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

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

<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}"）。

（

这段话指的是如果你想看到平滑路径与原始路径的差异，但又不想使用平滑路径替代原始路径进行跟踪。在这种情况下，你可以移除在代码中的部分，即smoothed_path="{path}"，这样计算出的平滑路径就不会替换原始路径。

相反，通过主题/smoothed_path，你可以获取到平滑路径的信息，这个信息是由平滑器服务器发布的。你可以利用这个信息进行可视化或者让系统的其他部分使用。如果你希望在行为树的其他部分与这个平滑路径进行交互，你也可以将平滑路径重新映射到另一个黑板变量，比如smoothed_path="{smoothed_path}"，这样其他部分就可以使用这个平滑路径的信息。

）

Using Collision Monitor碰撞监视器

Overview

这个教程展示了如何在Nav2堆栈中使用碰撞监视器。根据这个教程，你可以为你的环境和需求设置碰撞监视器。

Requirements

这个假设前提是ROS2和Nav2相关的软件包已经被安装或者在本地进行了构建。请确保Nav2项目也像在“构建和安装 Build and Install ”部分中描述的那样被在本地构建了。

Configuring Collision Monitor

碰撞监视器节点有自己的collision_monitor_node.launch.py启动文件，并在collision_monitor_params.yaml文件中设置了预设参数，用于演示。尽管在实际使用中，将其添加到Nav2的主要启动文件中非常简单。在演示中，将创建两个形状 - 一个内部停止和一个较大的减速边界框，放置在机器人的前方：

如果在减速区域内出现超过3个点，机器人将把速度减少到其值的30%。对于障碍物与机器人危险接近的情况，内部停止区域将起作用。为了这个设置，以下行应该被添加到collision_monitor_params.yaml参数文件中。停止区域被命名为PolygonStop，减速边界框被命名为PolygonSlow：

polygons: ["PolygonStop", "PolygonSlow"]  # 定义多边形的列表，包括PolygonStop和PolygonSlow

PolygonStop:  # PolygonStop的配置
  type: "polygon"  # 形状类型为多边形
  points: [0.4, 0.3, 0.4, -0.3, 0.0, -0.3, 0.0, 0.3]  # 多边形的顶点坐标
  action_type: "stop"  # 动作类型为停止
  min_points: 4  # 最少顶点数为4，Humble为3
  visualize: True  # 可视化为真
  polygon_pub_topic: "polygon_stop"  # 发布多边形的话题名称

PolygonSlow:  # PolygonSlow的配置
  type: "polygon"  # 形状类型为多边形
  points: [0.6, 0.4, 0.6, -0.4, 0.0, -0.4, 0.0, 0.4]  # 多边形的顶点坐标
  action_type: "slowdown"  # 动作类型为减速
  min_points: 4  # 最少顶点数为4，Humble为3
  slowdown_ratio: 0.3  # 减速比例为0.3
  visualize: True  # 可视化为真
  polygon_pub_topic: "polygon_slowdown"  # 发布多边形的话题名称

圆形形状可以用于代替多边形，例如对于全向机器人，碰撞可能发生在任何方向。然而，对于教程的需要，让我们把注意力集中在多边形上。出于相同的原因，我们暂时不涉及“Approach”模型。这两种情况都可以轻松地通过参考碰撞监视器配置指南来启用。

教程中的两个多边形形状都是静态设置的。然而，有能力通过包含多边形或足迹顶点点的主题消息，随时间动态调整它们。有关更多信息，请参考配置指南。

对于工作配置，至少应添加一个数据源。在当前的演示中，使用了激光扫描仪（虽然也可以使用PointCloud2和Range/Sonar/IR传感器），碰撞监视器节点的描述如下：


observation_sources: ["scan"] # 观测源列表，这里使用了激光扫描仪

scan: # 激光扫描仪的配置
type: "scan" # 类型为扫描
topic: "scan" # 激光扫描仪的话题名称

将话题名称、帧ID和超时时间设置为与默认的Nav2设置相匹配，此时整个`nav2_collision_monitor/params/collision_monitor_params.yaml`文件将如下所示：

collision_monitor:
  ros__parameters:
    use_sim_time: True  # 使用仿真时间
    base_frame_id: "base_footprint"  # 基础坐标系ID
    odom_frame_id: "odom"  # 里程计坐标系ID
    cmd_vel_in_topic: "cmd_vel_smoothed"  # 平滑后的命令速度输入话题
    cmd_vel_out_topic: "cmd_vel"  # 命令速度输出话题
    transform_tolerance: 0.5  # 转换容差
    source_timeout: 5.0  # 数据源超时时间
    stop_pub_timeout: 2.0  # 停止发布超时时间
    polygons: ["PolygonStop", "PolygonSlow"]  # 多边形列表
    PolygonStop:  # PolygonStop的配置
      type: "polygon"
      points: [0.4, 0.3, 0.4, -0.3, 0.0, -0.3, 0.0, 0.3]
      action_type: "stop"
      min_points: 4  # Humble最大顶点数为3
      visualize: True  # 可视化为真
      polygon_pub_topic: "polygon_stop"  # 多边形发布话题
    PolygonSlow:  # PolygonSlow的配置
      type: "polygon"
      points: [0.6, 0.4, 0.6, -0.4, 0.0, -0.4, 0.0, 0.4]
      action_type: "slowdown"
      min_points: 4  # Humble最大顶点数为3
      slowdown_ratio: 0.3
      visualize: True  # 可视化为真
      polygon_pub_topic: "polygon_slowdown"  # 多边形发布话题
    observation_sources: ["scan"]  # 观测源为激光扫描仪
    scan:  # 激光扫描仪的配置
      type: "scan"
      topic: "scan"  # 激光扫描仪话题

Preparing Nav2 stack

碰撞监视器被设计为在 Nav2 下作为独立的安全节点运行。它作为控制器发送的 cmd_vel 消息的过滤器，以避免潜在的碰撞。如果没有触发这样的区域，那么就会使用 cmd_vel 消息。否则，它会根据需要进行缩放或停止。

默认情况下，碰撞监视器被配置为与 Nav2 的启动包一起使用，与 navigation_launch.py 启动文件并行运行。为了让碰撞监视器与速度平滑器正确地运行，需要在 navigation_launch.py 启动脚本中移除速度平滑器的 cmd_vel_smoothed 重映射，如下所示。这将使速度平滑器的输出话题保持不变，成为新添加的碰撞监视器的输入话题（意味着你需要编辑名为 navigation_launch.py 的启动脚本文件，并删除对速度平滑器的 cmd_vel_smoothed 重映射。这样做的结果是，速度平滑器的输出话题将保持不变，它会成为新添加的碰撞监视器节点的输入话题。这种调整确保碰撞监视器能够接收到速度平滑器发布的 cmd_vel_smoothed 消息，并根据安全区域的触发情况进行相应的处理，以避免碰撞）：

Node(
    package='nav2_velocity_smoother',
    executable='velocity_smoother',
    name='velocity_smoother',
    output='screen',
    respawn=use_respawn,
    respawn_delay=2.0,
    parameters=[configured_params],
    arguments=['--ros-args', '--log-level', log_level],
    remappings=remappings +
-           [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+           [('cmd_vel', 'cmd_vel_nav')]),
...
ComposableNode(
    package='nav2_velocity_smoother',
    plugin='nav2_velocity_smoother::VelocitySmoother',
    name='velocity_smoother',
    parameters=[configured_params],
    remappings=remappings +
-              [('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
+              [('cmd_vel', 'cmd_vel_nav')]),

如果你改变了碰撞监视器的默认 `cmd_vel_in_topic` 和 `cmd_vel_out_topic` 配置，请确保速度平滑器的默认输出话题 `cmd_vel_smoothed` 应该与碰撞监视器节点的输入速度 `cmd_vel_in_topic` 参数值匹配，而输出速度 `cmd_vel_out_topic` 参数值应该是实际的 `cmd_vel` 以适应替换的情况。这样设置确保了碰撞监视器能够正确地接收速度平滑器发布的平滑速度信息，并且将适当处理后的速度信息发布为实际的速度命令，以适应下游的处理。

由于碰撞监视器充当安全节点，它必须是速度消息后处理链中的最后一环，这使得它成为发布到 `cmd_vel` 话题的节点。它可以放置在经过平滑的速度之后，就像我们的演示中一样，或者放置在来自控制器服务器的未经过平滑的速度之后，例如，如果系统中未启用速度平滑器，或者在自定义配置中放置在产生最终速度的任何其他模块之后。因此，在任何自定义的 Nav2 启动配置中，发布到 `cmd_vel` 话题的最后一个节点，应该重新映射为发布到碰撞监视器的输入话题，该话题由 `cmd_vel_in_topic` ROS 参数进行配置（默认为 `cmd_vel_smoothed`）。

Demo Execution

一旦碰撞监视器节点被调整，并且 `cmd_vel` 话题被调整，碰撞监视器节点就准备好运行了。为此，请按照 Getting Started 中的说明运行 Nav2 stack。

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

在并行的控制台中，使用碰撞监视器节点的启动文件来启动碰撞监视器节点。

ros2 launch nav2_collision_monitor collision_monitor_node.launch.py

由于 PolygonStop 和 PolygonSlow 多边形将有它们自己的发布者，它们可以像下面图片中显示的那样添加到可视化中：

设置初始姿态，然后在地图上放置 Nav2 目标点。机器人将开始移动，在靠近障碍物时减速，并在靠近障碍物时停止。

Adding a New Nav2 Task ServerNav2任务服务器

添加一个新的 Nav2 任务服务器涉及到服务器端的逻辑，用于完成不同类型的请求，通常由自主系统或通过行为树导航器调用。在本指南中，我们将讨论向 Nav2 添加新任务服务器所需的核心组件（例如控制器、行为、平滑器、规划器服务器）。主要涉及如何设置新的生命周期组件节点以进行启动和状态管理，以及传达语义上有意义的错误代码（如果有必要）。

虽然本教程不涵盖如何添加与此新任务服务器交互的补充行为树节点，但在Writing a New Behavior Tree Plugin中详细介绍了此内容，这样该任务服务器就可以在行为树导航器中被调用。

如果你创建了一个可能会为社区提供一般性重用的新任务服务器，请考虑与维护者联系，将其添加到 Nav2 项目中！Nav2 得益于像你这样的用户的贡献而变得更好！

Lifecycle Nodes

生命周期节点是 nav2 任务服务器的首要组件。生命周期节点是在 ROS 2 中引入的，用于系统化地管理机器人操作中涉及的不同节点的启动和关闭。使用生命周期节点确保所有节点在开始执行之前都能成功实例化，并且如果有任何无响应的节点，Nav2 将关闭所有节点。

生命周期节点包含状态机转换，可在 ROS 2 服务器中实现确定性行为。Nav2 中的生命周期节点转换由生命周期管理器处理。生命周期管理器会转换生命周期节点的状态，并对系统的状态提供更大的控制。

生命周期节点的主要状态包括未配置、非活动、活动和已完成。生命周期节点在实例化后处于未配置状态。生命周期管理器通过实现配置转换将节点从未配置转换为非活动状态。配置转换设置所有配置参数，并准备所需的任何设置，例如内存分配以及静态发布和订阅话题的设置。非活动状态的节点允许重新配置其参数，但不能执行任何处理。从非活动状态开始，生命周期管理器实施激活转换状态，将节点从非活动转换为活动状态，即主要状态。活动状态的节点允许执行任何处理操作。如果节点崩溃，生命周期管理器将关闭系统以防止任何关键故障。在关闭过程中，进行必要的清理操作，并通过停止、清理和关闭转换状态将节点转换为已完成状态。

有关生命周期管理的更多信息，请参阅关于 Managed Nodes 的文章。

您可能希望将自己的节点集成到 Nav2 框架中，或者向您的系统添加新的生命周期节点。例如，我们将添加一个名为 sensor_driver 的新的虚构生命周期节点，并通过 Nav2 生命周期管理器控制其，以确保在激活导航之前传感器数据可用。您可以通过在您的启动文件中添加 sensor_driver 节点，并将其添加到 lifecycle_manager 在导航之前激活节点列表中来实现，示例如下所示。

lifecycle_nodes = ['sensor_driver',
                   'controller_server',
                   'smoother_server',
                   'planner_server',
                   'behavior_server',
                   'bt_navigator',
                   'waypoint_follower']

...

Node(
    package='nav2_sensor_driver',
    executable='sensor_driver',
    name='sensor_driver',
    output='screen',
    parameters=[configured_params],
    remappings=remappings),

Node(
    package='nav2_lifecycle_manager',
    executable='lifecycle_manager',
    name='lifecycle_manager_navigation',
    output='screen',
    parameters=[{'autostart': autostart},
                {'node_names': lifecycle_nodes}]),

以上片段中，要由生命周期管理器处理的节点是使用 node_names 参数设置的。node_names 参数接受一个有序的节点列表，通过生命周期转换进行启动。如片段所示，node_names 参数接受 lifecycle_nodes，其中包含要添加到生命周期管理器中的节点列表。生命周期管理器逐个按顺序实施启动转换（配置和激活）节点，而对于关闭转换，则按相反的顺序处理节点。因此，sensor_driver 在其他导航服务器之前列出，以确保在激活导航服务器之前可用传感器数据。

生命周期管理器的另外两个参数是 autostart 和 bond_timeout。如果想要在启动时将转换节点设置为活动状态，则将 autostart 设置为 true。否则，您需要手动触发生命周期管理器以转换系统。bond_timeout 设置等待时间，用于确定何时将所有节点转换为非响应状态。

更多有关生命周期管理器参数的信息可以在Configuration Guide of Lifecycle Manager中找到。

Composition

Composition（组合）是导航2任务服务器中的第二个关键组件，其引入旨在通过将多个节点放置在单个进程中来减少内存和CPU资源的使用。在Nav2中，可以使用Composition来将所有Nav2节点组合在单个进程中，而不是将它们分别启动。这对于部署在嵌入式系统上的情况非常有用，因为开发人员需要优化资源使用。

here 可以找到有关Composition的更多信息。

在以下部分，我们举例说明如何向我们的系统中添加一个新的Nav2服务器，我们将其暂时称为route_server。

我们利用launch文件将不同的服务器组合成单个进程。该进程由ComposableNodeContainer容器建立，通过ComposableNode填充组合节点。然后，该容器可以像任何其他Nav2节点一样启动和使用。

1、在你的launch文件中添加一个新的ComposableNode()实例，指向你选择的组件容器。

container = ComposableNodeContainer(  # 创建组件容器
    name='my_container',  # 容器名称
    namespace='',  # 命名空间
    package='rclcpp_components',  # 包名
    executable='component_container',  # 可执行文件
    composable_node_descriptions=[  # 组件节点描述列表
        ComposableNode(  # 创建组件节点
            package='nav2_route_server',  # 包名
            plugin='nav2_route_server::RouteServer',  # 插件名称
            name='nav2_route_server'),  # 节点名称
    ],
    output='screen',  # 输出类型
)
#这个组件容器中只包含了一个名为 nav2_route_server 的组件节点。
#在这个示例中，只添加了一个节
#但您可以根据需要向 composable_node_descriptions 列表中添加更多的组件节点，以便在容器中运行多个节点。

请参考组合演示中的示例 composition_demo.launch.py 。

2、将包含该服务器的包添加到您的 package.xml 文件中。

<exec_depend>nav2_route_server</exec_depend>

Error codes

您的 nav2 任务服务器可能希望在其动作响应中返回一个“error_code”（虽然不是必需的）。如果对于您的系统有语义上有意义且可操作的失败类型，那么这是一种系统化的方式来传达这些失败，这些失败可能会自动汇总到导航系统对您的应用程序的响应中。

需要注意的是，错误代码从 0 到 9999 为内部 nav2 服务器保留，每个服务器的偏移量为 100，而外部服务器从 10000 开始，结束于 65535。下表显示了当前服务器以及预期的错误代码结构。

错误代码附加在动作消息的响应中。下面是路由服务器的示例。请注意，按照惯例，我们在消息定义中将错误代码字段设置为 error_code。

# Error codes
# Note: The expected priority order of the errors should match the message order
uint16 NONE=0 # 0 is reserved for NONE
uint16 UNKNOWN=10000 # first error code in the sequence is reserved for UNKNOWN

# User Error codes below
int16 INVILAD_START=10001
int16 NO_VALID_ROUTE=10002

#goal definition
route_msgs/PoseStamped goal
route_msgs/PoseStamped start
string route_id
---
#result definition
nav_msgs/Route route
builtin_interfaces/Duration route_time
uint16 error_code
---

正如消息中所述，错误的优先顺序应与消息顺序匹配，0 保留为“NONE”，序列中的第一个错误代码保留为“UNKNOWN”。由于路由服务器是一个外部服务器，错误代码从 10000 开始，最多到 10099。

为了将服务器的错误代码传播到系统的其他部分，必须将其添加到 nav2_params.yaml 文件中。BT 导航器中的 error_code_id_names 定义了服务器在黑板上要查找的错误代码。然后返回序列中最低的错误代码——而代码枚举会逐渐增加，越高层的软件堆栈会给予更低级别的失败更高的优先级。

error_code_id_names:
    - compute_path_error_code_id
    - follow_path_error_code_id
    - route_error_code_id

Conclusion

在本指南的这一部分中，我们讨论了 ROS 2 中的新概念和重要概念，包括生命周期节点、组合和错误代码。我们还展示了如何将这些概念应用于你使用 Nav2 创建的新节点/服务器。这三个概念有助于有效地运行你的系统，因此建议在 Nav2 中全面采用它们。

Filtering of Noise-Induced Obstacles噪声引起的障碍物过滤

Overview

嘈杂的传感器测量可能导致体素层或障碍物层出现误差。结果是，在代价地图上可能出现椒盐噪声。这种噪声会产生虚假的障碍物，阻止机器人找到地图上最佳的路径。虽然上述图像展示了椒盐噪声以及由定位错误引起的错误，但这个图层只会移除传感器噪声，而不是定位错误，导致与静态地图不对齐的伪装物件。本教程展示了如何配置过滤因噪声引起的虚假障碍物。这项功能由 DenoiseLayer 代价地图层插件提供，在本文档中将被启用和使用。

Requirements

这假定了 ROS 2、Gazebo 和 TurtleBot3 包已在本地安装或构建。请确保 Nav2 项目也像“构建和安装 Build and Install ”中所述一样在本地构建了。

Tutorial Steps

1. Enable Denoise Layer

Denoise Layer是Costmap2D插件。你可以通过在nav2_params.yaml文件中的plugins参数中添加denoise_layer来启用DenoiseLayer插件。你可以将其放置在global_costmap和/或local_costmap中，以在全局或局部地图上过滤噪声。DenoiseLayer插件应该有以下参数定义：

- plugin: 插件的类型。在我们的情况下是nav2_costmap_2d::DenoiseLayer。

DenoiseLayer支持的参数的完整列表在 Denoise Layer Parameters 页面中列出。

需要注意的是，DenoiseLayer通常应该放置在膨胀层之前。这是为了防止膨胀层受到噪声导致的障碍物的影响。此外，DenoiseLayer只处理costmap中的障碍物信息。INSCRIBED_INFLATED_OBSTACLE、LETHAL_OBSTACLE和可选的NO_INFORMATION值将被解释为障碍单元。在处理时，任何其他值的单元都将被解释为FREE_SPACE（不会在成本图中失真）。如果被识别为噪声的障碍单元，它将在处理后被替换为FREE_SPACE。

要为全局和局部costmap启用DenoiseLayer，请使用以下配置：

global_costmap:
  global_costmap:
    ros__parameters:
      ...
      plugins: ["static_layer", "obstacle_layer", "denoise_layer", "inflation_layer"]
      ...
      denoise_layer:
        plugin: "nav2_costmap_2d::DenoiseLayer"
        enabled: True
...
local_costmap:
  local_costmap:
    ros__parameters:
      ...
      plugins: ["voxel_layer", "denoise_layer", inflation_layer"]
      ...
      keepout_filter:
        plugin: "nav2_costmap_2d::DenoiseLayer"
        enabled: True

成功过滤噪声的关键在于理解其类型并选择合适的DenoiseLayer参数。默认参数专注于快速移除孤立的障碍物。更正式地说，如果相邻的八个单元中没有障碍物，则会丢弃障碍物。在典型情况下，这应该足够了。

如果某些传感器生成了相互关联的噪声诱导障碍物，并且世界中很少有小障碍物，则可以删除小组的障碍物。要配置DenoiseLayer以适应这种情况，并了解其工作原理，请参考“ How it works ”部分。

使用此插件谨慎过滤全局costmap。它可能引入潜在的性能问题。例如，在典型的高范围激光雷达（20米以上）情况下，更新窗口可能非常大，导致处理时间不可接受的长。作为应用设计者，考虑到这一点是值得的。

2. Run Nav2 stack

当启用了全局/局部costmap的去噪层后，按照 Getting Started 中的步骤运行Nav2堆栈：

ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

然后检查过滤器是否正常工作：使用默认参数，costmap 上不应该留下单独的障碍物。可以在 RViz 主窗口中检查，显示局部和全局costmap，同时移除不必要的粒子（如本教程顶部所示）。

How it works

这个插件基于两个算法。

当参数 minimal_group_size = 2 时，会启用第一个算法。它应用侵蚀（ erosion ）函数，并使用以下图像中的内核（如果 group_connectivity_type = 4，则使用左侧图像；如果 group_connectivity_type = 8，则使用右侧图像）处理costmap。内核像素的白色意味着使用该值，黑色意味着忽略它。

侵蚀函数的结果是创建了邻域图像。内核在costmap上的每个可能位置对应于邻域图像的一个像素。该图像的像素值等于与掩模的白色像素相对应的4/8个costmap像素的最大值。换句话说，邻域图像的像素等于如果附近有障碍物则为障碍物代码，否则为自由空间代码。然后，对于邻域图像中对应自由空间代码的障碍物进行移除。

这个过程如下图所示。图像的左侧是costmap，右侧是邻域图像。白色像素是自由空间，黑色像素是障碍物，group_connectivity_type = 4。动画末尾标记的障碍物将被移除。

当参数 minimal_group_size > 2 时，将执行第二种算法。这是一种通用解决方案，允许您移除相邻障碍物的群组，如果它们的总数小于 minimal_group_size。为了选择相邻障碍物的群组，该算法执行它们的分割。一个段内的单元格连接类型由参数 group_connectivity_type 确定。接下来，计算每个段的大小。障碍物段的大小小于 minimal_group_size 的将被替换为空单元格。这个算法大约比第一个慢10倍，因此请谨慎使用，只在必要时使用。它的执行时间取决于处理的地图片段的大小（而不依赖于 minimal_group_size 的值）。

下面的动画演示了该算法（group_connectivity_type = 8）。动画末尾标记的障碍物将被移除（小于3个的群组）。