ROS2与Navigation2入门教程-使用外部的STVL成本地图插件进行导航

说明：

• 介绍如何使用外部的STVL成本地图插件进行导航

概述：

• 本教程将会说明如何加载和使用外部插件。

• 本示例将会使用时空体素（即体积像素Volume Pixel）图层（STVL，Spatio Temporal Voxel Layer）成本地图插件库（pluginlib）插件作为示例。

• 在本教程中，STVL 是一个示范性的插件库插件，其它成本地图插件以及规划器、控制器和恢复器插件都可以遵循相同的过程。

• 在进行本教程之前，请阅读前面两个关于仿真和物理机器人硬件（如果有的话）导航的教程。

• 本教程假定您具备导航知识和对成本地图有基本的了解。

Costmap2D和STVL

• Costmap2D是用于将传感器信息缓冲到全局视图中的数据对象，机器人会使用该全局视图创建路径规划和控制工作。

• 在Costmap2D中，有可用的插件库（pluginlib）插件界面来创建可在运行时加载的自定义行为。

• Costmap2D中包含的插件库插件示例包括Obstacle Layer（障碍物图层）插件、Voxel Layer（体素图层）插件、Staic Layer（静态图层）插件和Inflation Layer（膨胀图层）插件。

• 但是，这些只是Costmap2D基本实现中提供的示例插件。

• Navigation2中Costmap2D的另一个可用插件库插件就是STVL。

• STVL是另一个类似于Voxel Layer（体素图层）插件的3D感知插件。

• 可以在这个存储库中找到有关其工作原理的更详细概述，而STVL会将来自深度相机、声纳、激光雷达等的3D数据缓冲到一个稀疏体积世界模型中，并且会随着时间流逝以与传感器模型和时间到期成比例地移除体素。

• 这对位于高度动态环境中的机器人尤其有用，并且可以将3D传感器处理的资源利用率降低多达2倍。

• STVL还对3D激光雷达和雷达提供最高级别的支持

具体步骤

• 设置
• 按照与“开始使用Nav2”教程中相同的过程安装和设置用于硬件测试（如果有硬件的话）或者仿真的机器人。
• 请确保已经安装好了ROS 2、Navigation2和Gazebo软件。

安装STVL

• 在ROS 2中，可以通过ROS Build Farm来安装STVL，命令为：

sudo apt install ros-<ros2-distro>-spatio-temporal-voxel-layer

• 也可以从源代码编译构建STVL，这里采用源代码编译安装方法，这需要将其存储库克隆到您的Navigation2工作空间src目录中：

git clone https://github.com/SteveMacenski/spatio_temporal_voxel_layer.git --branch foxy-devel

• 然后退回到src目录的上一级目录，并执行colcon build命令：

colcon build --symlink-install --packages-select spatio_temporal_voxel_layer

• 编译安装过程中可能会提示尚未安装pcl_conversions和OpenVDB软件包，如果在本地机器上尚未安装这两个软件包的话。

• 这样，需要先从源代码编译安装这两个软件包，其中pcl_conversions目前已经包含在perception_pcl元软件包中

• 因此需要先编译安装其依赖包pcl_msgs然后再编译安装perception_pcl。

• 而OpenVDB的安装方法可以在其官网https://www.openvdb.org/download/上找到，

• 在Linux系统中从源代码编译安装的命令如下（先安装依赖包Boost、TBB和Blosc，再编译构建OpenVDB）：

apt-get install -y libboost-iostreams-dev
apt-get install -y libboost-system-dev
apt-get install -y libtbb-dev
git clone https://github.com/Blosc/c-blosc.git
cd c-blosc
git checkout tags/v1.5.0 -b v1.5.0
mkdir build
cd build
cmake ..
make -j4
make install
cd ../..

git clone https://github.com/AcademySoftwareFoundation/openvdb.git
cd openvdb
mkdir build
cd build
cmake ..
make -j4
make install

• 此时再执行colcon build命令来编译安装STVL就应该能成功了。

修改Navigation2参数

• 在Costmap2D中，与所有其它插件一样，STVL也是一个可选插件。在ROS 2 Foxy及更高版本的发行版中，Navigation2中的成本地图插件是在其各自成本地图中以plugins变量（早期发行版则为plugin_names和plugin_types两个变量）来加载的。

• 例如，下面的配置会分别将静态图层插件和障碍物图层插件加载到名称static_layer和obstacle_layer中：

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: True
      plugin_names: ["static_layer", "obstacle_layer"] # For Eloquent and earlier
      plugins: ["static_layer", "obstacle_layer"] # For Foxy and later
      plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer"] # For Eloquent and earlier:

• 对于Foxy发行版或者更高版本，plugin_names和 plugin_types已被替换为用于插件名称的单个plugins字符串向量。

• 现在插件类型是在“plugins”字段的plugin_name命名空间中定义的（例如 plugin: MyPlugin::Plugin）。

• 代码块中的行内注释将帮助指导您完成此一点。

• 要加载STVL插件，必须添加新的插件名称和类型。

• 例如，如果应用程序需要的是STVL图层而不是障碍物图层，则该文件将会如下所示：

global_costmap:
  global_costmap:
    ros__parameters:
      use_sim_time: True
      plugin_names: ["static_layer", "stvl_layer"] # For Eloquent and earlier
      plugins: ["static_layer", "stvl_layer"] # For Foxy and later
      plugin_types: ["nav2_costmap_2d::StaticLayer", "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer"] # For Eloquent and earlier

• 与体素图层插件Voxel Layer类似，注册该插件后，可以在命名空间stvl_layer下添加STVL图层插件的配置。
• 一个STVL配置的完整描述参数化示例如下所示：

stvl_layer:
  plugin: "spatio_temporal_voxel_layer/SpatioTemporalVoxelLayer" # For Foxy and later
  enabled: True
  voxel_decay: 15.
  decay_model: 0
  voxel_size: 0.05
  track_unknown_space: True
  max_obstacle_height: 2.0
  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
    voxel_filter: False
    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

• 请将上面配置中带有plugins（或者plugin_names和plugin_types）注册项的文本复制粘贴到您的nav2_params.yaml文件中，以在您的应用程序中启用 STVL。

• 请确保同时更改该文件中的local_costmap和global_costmap部分。

• Navigation2的其他服务器（如规划器、恢复器和控制器）的插件库插件也可以采用同样的方式进行设置。

启动Navigation2

• 请按照与“开始使用Nav2”教程中相同的过程在启动Navigation2（启用SLAM制图功能）的同时在Gazebo中启动仿真机器人。
• 启动命令为：

ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True

• 现在Navigation2就会使用STVL作为其3D传感成本地图图层插件。

RVIZ

• 在STVL配置中将publish_voxel_map字段设置为True的情况下，执行上面的命令打开RViz后，就可以使用RViz中的{local, global}_costmap/voxel_grid话题对底层数据结构的3D网格进行可视化。

• 注意：建议在RViz中将PointCloud2大小设置为您的体素大小即size变量（上面的STVL完整参数配置示例中为0.05），并将样式（style）设置为具有中性颜色（例如[125,125,125]）的“Boxes”以获得最佳的可视化效果。

参考：　

• 参考1