建图与定位

传感器配置完成后，接下来可以利用传感器数据构建环境地图，并在地图中对机器人进行定位。slam_toolbox 是一组用于 ROS 2 的 2D SLAM 工具，能够处理大规模地图。它是 Nav2 官方支持的 SLAM 库之一，建议在需要 SLAM 的场景下优先使用。除了 slam_toolbox，也可以用 nav2_amcl 包进行定位——该包实现了自适应蒙特卡洛定位（AMCL），用于估计机器人在地图中的位置和朝向。其他可用技术请参阅 Nav2 文档。

slam_toolbox 和 nav2_amcl 都依赖激光扫描传感器的数据来感知环境，因此必须确保它们订阅了正确的、发布 sensor_msgs/LaserScan 消息的话题。只需将它们的 scan_topic 参数设置为该话题即可。按照惯例，sensor_msgs/LaserScan 消息发布到 /scan 话题，因此 scan_topic 默认值就是 /scan。上一节为 sam_bot 添加激光雷达时，已将发布 sensor_msgs/LaserScan 消息的话题设为 /scan。

完整配置参数较为复杂，不在本教程范围内。建议参考下方链接中的官方文档。

注意：

• 有关 slam_toolbox 的完整配置参数列表，请参阅 slam_toolbox 的 GitHub 仓库。
• 有关 nav2_amcl 的完整配置参数列表和示例配置，请参阅 AMCL 配置指南。

还可以参考边建图边导航（SLAM）指南了解如何将 Nav2 与 SLAM 配合使用。在 RViz 中可视化地图和机器人位姿（pose），即可验证 slam_toolbox 和 nav2_amcl 是否配置正确，方法与上一节类似。

代价地图 2D

代价地图 2D 包利用传感器信息，以占据栅格（occupancy grid）的形式表示机器人所处的环境。占据栅格中的每个单元格存储 0 到 254 之间的代价值，表示通过该区域的通行代价。代价值 0 表示该单元格空闲，254 表示被致命障碍物占据。介于两者之间的值则被导航算法用作势场，引导机器人远离障碍物。Nav2 中的代价地图通过 nav2_costmap_2d 包实现。

代价地图由多个层组成，每层负责特定功能，共同决定单元格的最终代价值。该包基于插件架构，支持自定义和扩展，内置以下层：静态层（static layer）、膨胀层（inflation layer）、距离层（range layer）、障碍物层（obstacle layer）和体素层（voxel layer）。静态层表示代价地图中来自 /map 话题（如 SLAM 生成）的地图数据。障碍物层包含由发布 LaserScan 和/或 PointCloud2 消息的传感器检测到的物体。体素层与障碍物层类似，同样可使用 LaserScan 和/或 PointCloud2 数据，但处理的是 3D 信息。距离层用于纳入声呐和红外传感器的数据。最后，膨胀层在致命障碍物周围增加代价值，使机器人根据自身几何形状避免碰撞障碍物。下一小节将讨论 nav2_costmap_2d 中各层的基本配置。

这些层通过插件接口集成到代价地图中。若膨胀层启用，则按用户指定的膨胀半径进行膨胀。有关代价地图概念的深入讨论，可参考 ROS 1 costmap_2D 文档。nav2_costmap_2d 包基本上是 ROS 1 导航栈的 ROS 2 移植版本，做了少量适配改动，并新增了一些层插件。

配置 nav2_costmap_2d

本小节将展示一个 nav2_costmap_2d 的示例配置，使用 sam_bot 的激光雷达传感器数据。配置中将用到静态层、障碍物层、体素层和膨胀层，其中障碍物层和体素层均使用激光雷达发布到 /scan 话题的 LaserScan 消息。同时设置一些基本参数，定义检测到的障碍物如何反映到代价地图中。此配置应包含在 Nav2 的配置文件中。

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.22
      resolution: 0.05
      track_unknown_space: false
      rolling_window: false
      plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
      static_layer:
        plugin: "nav2_costmap_2d::StaticLayer"
        map_subscribe_transient_local: True
      obstacle_layer:
        plugin: "nav2_costmap_2d::ObstacleLayer"
        enabled: True
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
          raytrace_max_range: 3.0
          raytrace_min_range: 0.0
          obstacle_max_range: 2.5
          obstacle_min_range: 0.0
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 5.0
      publish_frequency: 2.0
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.22
      plugins: ["voxel_layer", "inflation_layer"]
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        enabled: True
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        cost_scaling_factor: 3.0
        inflation_radius: 0.55
      always_send_full_costmap: True

上面的配置中为两个不同的代价地图设置了参数：global_costmap（全局代价地图）和 local_costmap（局部代价地图）。之所以使用两个代价地图，是因为 global_costmap 用于整个地图上的长期规划，而 local_costmap 用于短期规划和避障。

配置中使用的层在 plugins 参数中定义，如 global_costmap 的第 13 行和 local_costmap 的第 50 行所示。该参数是一个层名称列表，每个名称同时作为对应层参数的命名空间。列表中的每个层都必须有 plugin 参数（如第 14、17、31、50 和 66 行所示），用于指定该层加载的插件类型。

静态层（第 13-15 行）中，map_subscribe_transient_local 设为 True，用于配置地图话题的 QoS 策略。静态层的另一个重要参数是 map_topic，用于指定订阅的地图话题，未定义时默认为 /map。

障碍物层（第 16-29 行）中，通过 observation_sources 参数（第 19 行）将传感器源定义为 scan，其参数在第 21-29 行设置。topic 参数设置为该传感器源发布数据的话题，data_type 根据所用传感器设置。在本配置中，障碍物层使用激光雷达发布到 /scan 的 LaserScan 消息。

障碍物层和体素层的 data_type 可设为 LaserScan 和/或 PointCloud2，默认为 LaserScan。下面的代码片段展示了同时使用两者作为传感器源的示例，配置实体机器人时可能会用到。

obstacle_layer:
  plugin: "nav2_costmap_2d::ObstacleLayer"
  enabled: True
  observation_sources: scan pointcloud
  scan:
    topic: /scan
    data_type: "LaserScan"
  pointcloud:
    topic: /depth_camera/points
    data_type: "PointCloud2"

障碍物层的其他参数：max_obstacle_height 设置传感器读数返回到占据栅格的最大高度；min_obstacle_height 设置最小高度，配置中未设此项，默认值为 0。clearing 设置是否从代价地图中清除障碍物，清除操作通过对栅格进行光线追踪（raytracing）实现。raytrace_max_range 和 raytrace_min_range 分别设置光线追踪清除的最大和最小距离。marking 设置是否将检测到的障碍物标记到代价地图中。obstacle_max_range 和 obstacle_min_range 分别设置标记障碍物的最大和最小距离。

膨胀层（第 31-34 行和第 67-70 行）中，cost_scaling_factor 设置代价值在膨胀半径内的指数衰减因子，inflation_radius 定义围绕致命障碍物的膨胀半径。

体素层（第 51-66 行）中，publish_voxel_map 设为 True 以启用 3D 体素栅格的发布。z_resolution 定义体素在高度方向的分辨率，z_voxels 定义每列的体素数量。mark_threshold 设置一列中至少需要多少体素才在占据栅格中标记为占用。体素层的 observation_sources 设为 scan，扫描参数（第 61-66 行）与障碍物层类似。根据 topic 和 data_type 的设置，体素层使用激光雷达发布到 /scan 话题的 LaserScan 消息。

本配置未使用距离层（range layer），但在实际机器人配置中可能会用到。距离层的基本参数包括 topics、input_sensor_type 和 clear_on_max_reading。topics 定义要订阅的距离话题；input_sensor_type 可设为 ALL、VARIABLE 或 FIXED；clear_on_max_reading 为布尔参数，设置是否在最大距离处清除传感器读数。如需使用距离层，请参考下方链接中的配置指南。

注意：有关 nav2_costmap_2d 及层插件参数的完整列表，请参阅 代价地图 2D 配置指南。

构建、运行与验证

首先启动 display.launch.py，它会启动机器人状态发布器（提供 URDF 中的 base_link => sensors 变换）、启动 Gazebo 物理仿真器，并提供来自差速驱动插件或 ekf_node 的 odom => base_link 变换。同时启动 RViz，用于可视化机器人和传感器信息。

然后启动 slam_toolbox，它会向 /map 话题发布地图数据，并提供 map => odom 变换。回顾前文，map => odom 变换是 Nav2 系统的核心要求之一。发布到 /map 话题的消息随后会被 global_costmap 的静态层使用。

正确设置好机器人描述、里程计传感器和必要的变换后，最后启动 Nav2 系统。目前只需关注 Nav2 的代价地图生成功能。启动 Nav2 后，将在 RViz 中可视化代价地图以验证输出。

启动描述节点、RViz 和 Gazebo

通过启动文件 display.launch.py 来启动机器人描述节点、RViz 和 Gazebo。打开一个新终端，执行以下命令。

colcon build
. install/setup.bash
ros2 launch sam_bot_description display.launch.py

RViz 和 Gazebo 应已启动，sam_bot 同时出现在两者中。此时，base_link => sensors 变换由 robot_state_publisher 发布，odom => base_link 变换由 Gazebo 插件发布。两个变换都应在 RViz 中正常显示，无报错。

启动 slam_toolbox

启动 slam_toolbox 前，请确保已安装该包：

sudo apt install ros-$ROS_DISTRO-slam-toolbox

使用该包内置的启动文件来启动 slam_toolbox 的 async_slam_toolbox_node。打开一个新终端，执行以下命令：

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=true

slam_toolbox 此时应该正在向 /map 话题发布地图数据，并提供 map => odom 变换。

可以在 RViz 中验证 /map 话题是否正在发布。在 RViz 窗口中，点击左下方的 Add 按钮，切换到 By topic 选项卡，在 /map 话题下选择 Map。此时应能看到从 /map 接收到的消息，如下图所示。

还可以在新终端中执行以下命令，检查变换是否正确：

ros2 run tf2_tools view_frames

该命令会生成一个 frames.pdf 文件，展示当前的变换树。变换树应与下图类似：

启动 Nav2

首先，确保已安装 Nav2 相关包：

sudo apt install ros-$ROS_DISTRO-navigation2
sudo apt install ros-$ROS_DISTRO-nav2-bringup

使用 nav2_bringup 内置的启动文件 navigation_launch.py 来启动 Nav2。打开一个新终端，执行以下命令：

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=true

上一小节讨论的 nav2_costmap_2d 参数已包含在 navigation_launch.py 的默认参数中。除 nav2_costmap_2d 参数外，还包含 Nav2 中其他节点的参数。

Nav2 正确设置并启动后，/global_costmap 和 /local_costmap 话题应处于活动状态。

注意：要使代价地图正常显示，请按以下顺序依次执行 3 个命令：

1. 启动描述节点、RViz 和 Gazebo——稍等片刻，等待所有组件启动完成
2. 启动 slam_toolbox——等待日志中出现「Registering sensor」
3. 启动 Nav2——等待日志中出现「Creating bond timer」

在 RViz 中可视化代价地图

global_costmap、local_costmap 以及检测到的障碍物的体素表示均可在 RViz 中可视化。

在 RViz 中可视化 global_costmap：点击 RViz 窗口左下方的 Add 按钮，切换到 By topic 选项卡，在 /global_costmap/costmap 话题下选择 Map。global_costmap 将显示在 RViz 窗口中，如下所示。图中黑色区域即机器人在 Gazebo 仿真世界中导航时应避开的部分。

可视化 local_costmap：在 /local_costmap/costmap 话题下选择 Map，并将 RViz 的 color scheme 设为 costmap，效果如下图所示。

若要可视化检测到的障碍物的体素表示，请打开一个新终端，执行以下命令：

ros2 run nav2_costmap_2d nav2_costmap_2d_markers voxel_grid:=/local_costmap/voxel_grid visualization_marker:=/my_marker

该命令将标记发布话题设为 /my_marker。要在 RViz 中查看标记，请在 /my_marker 话题下选择 Marker，如下所示。

然后将 RViz 中的 fixed frame 设为 odom，即可看到体素表示——它们对应 Gazebo 世界中的立方体和球体：

结论

本节机器人设置指南讨论了传感器信息在 Nav2 各项任务中的重要性，涵盖建图（SLAM）、定位（AMCL）和感知（代价地图）等方面。随后为 nav2_costmap_2d 包配置了不同的层，生成了全局和局部代价地图，并通过在 RViz 中可视化这些代价地图验证了配置效果。