使用外部代价地图插件（STVL）

• 概述
• Costmap2D 与 STVL
• 教程步骤

概述

本教程展示如何加载和使用外部代价地图插件。 示例选用时空体素层（Spatio Temporal Voxel Layer）（STVL）这一 pluginlib 代价地图插件来演示。 STVL 只是众多可选插件之一，本教程的方法同样适用于其他代价地图插件，以及插件化的规划器、控制器和行为。

在开始之前，建议先阅读前面两篇关于仿真和实体硬件导航的教程。 本教程假定你已经具备导航基础知识，并对代价地图有一定了解。

注意：2021 年 12 月之前的 Ubuntu 20.04 用户可能遇到 OpenVDB 与 libjemalloc 的已知兼容问题。如果出现类似 Could not load library LoadLibrary error: /usr/lib/x86_64-linux-gnu/libjemalloc.so.2: cannot allocate memory in static TLS block 的错误，可在 OpenVDB 发布修复版本之前，通过执行 export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libjemalloc.so.2 来解决。

Costmap2D 与 STVL

Costmap2D 是一种数据对象，用于将传感器信息缓存为全局地图视图，机器人据此进行规划和控制。 Costmap2D 提供了 pluginlib 插件接口，支持在运行时加载自定义行为。 内置的 pluginlib 插件示例包括障碍物层（Obstacle Layer）、体素层（Voxel Layer）、静态层（Static Layer）和膨胀层（Inflation Layer）。

以上只是基础实现附带的示例插件。 Navigation2 还为 Costmap2D 提供了另一个可用的 pluginlib 插件——STVL。

STVL 是一款类似体素层的 3D 感知插件。 详细的工作原理可参阅该仓库；简而言之，它将深度相机、声呐、激光雷达等设备的 3D 数据缓存到一个稀疏体素世界模型中，并根据传感器模型和时间过期机制逐步清除过期的体素。 这对高度动态环境中的机器人尤其有用，可将 3D 传感器处理的资源消耗降低多达一半。 此外，STVL 原生支持 3D 激光雷达和雷达。 有关 STVL 的 ROSCon 演讲可观看这个视频。

教程步骤

0- 环境设置

按照与快速入门相同的流程安装和设置机器人，用于硬件测试或仿真。确保已安装 ROS 2、Navigation2 和 Gazebo。

1- 安装 STVL

首先，加载你的 ROS 2 安装目录的环境变量：

  source /opt/ros/<ros2-distro>/setup.bash

STVL 可以通过 ROS Build Farm 在 ROS 2 中安装：

  sudo apt install ros-$ROS_DISTRO-spatio-temporal-voxel-layer

也可以将仓库克隆到你的 Navigation2 工作空间中，从源码构建：

  git clone -b $ROS_DISTRO-devel git@github.com:stevemacenski/spatio_temporal_voxel_layer

1- 修改 Navigation2 参数

与所有插件一样，STVL 是 Costmap2D 的可选插件。Navigation2 中的代价地图插件通过各自代价地图中的 plugin_names 和 plugin_types 变量加载。 例如，下面的配置分别以 static_layer 和 obstacle_layer 的名称加载静态层和障碍物层插件：

    global_costmap:
      global_costmap:
        ros__parameters:
          plugins: ["static_layer", "obstacle_layer"]

注意：Galactic 及更高版本中，plugin_names 和 plugin_types 已被单个 plugins 字符串向量取代。插件类型现在在 plugin_name 命名空间下的 plugin: 字段中定义（例如 plugin: MyPlugin::Plugin）。代码块中的行内注释对此有详细说明。

要加载 STVL 插件，需要添加新的插件名称和类型。 例如，如果应用需要 STVL 层而非障碍物层，配置文件应为：

    global_costmap:
      global_costmap:
        ros__parameters:
          plugins: ["static_layer", "stvl_layer"]

与体素层类似，注册插件后，可在 stvl_layer 命名空间下添加 STVL 层的配置。 完整的 STVL 配置参数示例如下：

    stvl_layer:
      plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"
      enabled: true
      voxel_decay: 15.
      decay_model: 0
      voxel_size: 0.05
      track_unknown_space: true
      unknown_threshold: 15
      mark_threshold: 0
      update_footprint_enabled: true
      combination_method: 1
      origin_z: 0.0
      publish_voxel_map: true
      transform_tolerance: 0.2
      mapping_mode: false
      map_save_duration: 60.0
      observation_sources: pointcloud
      pointcloud:
        data_type: PointCloud2
        topic: /intel_realsense_r200_depth/points
        marking: true
        clearing: true
        obstacle_range: 3.0
        min_obstacle_height: 0.0
        max_obstacle_height: 2.0
        expected_update_rate: 0.0
        observation_persistence: 0.0
        inf_is_valid: false
        filter: "voxel"
        voxel_min_points: 0
        clear_after_reading: true
        max_z: 7.0
        min_z: 0.1
        vertical_fov_angle: 0.8745
        horizontal_fov_angle: 1.048
        decay_acceleration: 15.0
        model_type: 0

将上面的配置连同 plugin_names 和 plugin_types 注册信息一起复制到你的 nav2_params.yaml 中，即可在应用中启用 STVL。 注意需要同时修改局部代价地图和全局代价地图。

规划、行为和控制等其他 Navigation2 服务器的 pluginlib 插件也可以用相同的方式配置。

2- 启动 Navigation2

按照与快速入门相同的流程，在 Gazebo 中启动使用 Navigation2 的仿真机器人。 Navigation2 现在将使用 STVL 作为 3D 感知代价地图层。

3- RViz

打开 RViz，将 publish_voxel_map 设置为 true 后，即可通过 {local, global}_costmap/voxel_grid 话题可视化底层数据结构的 3D 网格。 建议在 RViz 中将 PointCloud2 的 Size 设置为你的体素大小，样式设置为 Boxes，并选用中性颜色以获得最佳可视化效果。