Cartographer 建图到导航实战：从 .pbstream 到 ROS 地图的 2 种转换方法与 Nav2 集成

Cartographer 建图到导航实战：从 .pbstream 到 ROS 地图的 2 种转换方法与 Nav2 集成

当你在机器人项目中成功使用 Cartographer 完成环境建图后，接下来的关键挑战是如何将这些地图数据无缝集成到 ROS 导航栈中。本文将深入探讨两种将 .pbstream 格式转换为 ROS 标准栅格地图的方法，并详细解析如何与 Nav2 导航系统进行集成。

1. Cartographer 地图数据解析与转换原理

Cartographer 生成的 .pbstream 文件包含了比传统栅格地图丰富得多的信息。理解这些数据的组织结构是进行有效转换的基础。

.pbstream 文件本质上是一个 Protocol Buffers 序列化格式的二进制文件，主要包含以下核心数据：

• 子图集合：Cartographer 将地图分解为多个子图（submaps），每个子图都是局部环境的高精度表示
• 位姿图：记录所有扫描数据之间的约束关系，包含回环检测信息
• 轨迹数据：机器人运动过程中收集的传感器数据和位姿估计

转换的核心挑战在于如何将这些概率网格和位姿图数据降维到 ROS 导航栈能够处理的二维栅格地图格式（.pgm+.yaml）。以下是两种主流方法的对比：

提示：在大多数室内导航场景中，官方工具提供的转换质量已经足够。但对于需要利用 Cartographer 全部特性的高级应用，建议开发自定义转换流程。

2. 方法一：使用官方工具转换

Cartographer 自带的cartographer_pbstream_to_ros_map节点是最直接的转换方案。以下是完整的操作流程：

2.1 基础转换命令

rosrun cartographer_ros cartographer_pbstream_to_ros_map \ -map_filestem=${输出文件前缀} \ -pbstream_filename=${输入.pbstream文件} \ -resolution=${地图分辨率}

关键参数说明：

• -resolution：通常设置为 0.05（米/像素），对应 Cartographer 的默认子图分辨率
• -map_filestem：输出的 .pgm 和 .yaml 文件将使用这个前缀

2.2 自动化转换脚本

为提高效率，可以创建以下 Python 脚本实现一键转换：

#!/usr/bin/env python3 import os import subprocess from datetime import datetime def convert_pbstream(pbstream_path, output_dir, resolution=0.05): timestamp = datetime.now().strftime("%Y%m%d_%H%M%S") map_stem = os.path.join(output_dir, f"map_{timestamp}") cmd = [ 'rosrun', 'cartographer_ros', 'cartographer_pbstream_to_ros_map', '-map_filestem', map_stem, '-pbstream_filename', pbstream_path, '-resolution', str(resolution) ] try: subprocess.run(cmd, check=True) print(f"转换成功！地图文件已保存到: {map_stem}.{pgm/yaml}") except subprocess.CalledProcessError as e: print(f"转换失败: {e}") if __name__ == "__main__": import argparse parser = argparse.ArgumentParser() parser.add_argument("pbstream", help="输入的.pbstream文件路径") parser.add_argument("-o", "--output", default=".", help="输出目录") parser.add_argument("-r", "--resolution", type=float, default=0.05, help="地图分辨率（米/像素）") args = parser.parse_args() convert_pbstream(args.pbstream, args.output, args.resolution)

将此脚本保存为pbstream_converter.py后，可通过以下命令使用：

python3 pbstream_converter.py /path/to/your_map.pbstream -o ~/maps

3. 方法二：自定义栅格化流程

当需要更精细控制地图生成过程时，可以开发自定义转换脚本。以下是基于 Cartographer API 的实现框架：

3.1 提取子图数据

import cartographer_ros from cartographer.io.pbstream import read_pbstream def extract_submaps(pbstream_path): _, _, pose_graph = read_pbstream(pbstream_path) submaps = [] for submap_id in pose_graph.submaps: submap = pose_graph.submaps[submap_id] submaps.append({ 'id': submap_id, 'pose': submap.pose, 'grid': submap.grid }) return submaps

3.2 子图融合与栅格化

import numpy as np from PIL import Image def merge_submaps(submaps, resolution=0.05): # 计算全局地图边界 all_corners = [] for submap in submaps: corners = calculate_submap_corners(submap, resolution) all_corners.extend(corners) # 创建空白画布 min_x = min(c[0] for c in all_corners) max_x = max(c[0] for c in all_corners) min_y = min(c[1] for c in all_corners) max_y = max(c[1] for c in all_corners) width = int((max_x - min_x) / resolution) + 1 height = int((max_y - min_y) / resolution) + 1 global_grid = np.zeros((height, width), dtype=np.uint8) # 融合所有子图 for submap in submaps: insert_submap(global_grid, submap, min_x, min_y, resolution) # 转换为ROS兼容格式 image = Image.fromarray((255 - global_grid * 100).astype(np.uint8)) return image, (min_x, min_y, resolution) def insert_submap(global_grid, submap, origin_x, origin_y, resolution): # 实现子图插入逻辑 pass

4. Nav2 集成配置

成功转换地图后，需要正确配置 Nav2 以使用这些地图。以下是关键配置步骤：

4.1 地图服务器配置

创建cartographer_nav2.launch.py文件：

from launch import LaunchDescription from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): map_dir = os.path.join( get_package_share_directory('your_pkg'), 'maps', 'your_map.yaml' ) return LaunchDescription([ Node( package='nav2_map_server', executable='map_server', name='map_server', output='screen', parameters=[{'use_sim_time': True}, {'yaml_filename': map_dir}] ), Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_mapper', output='screen', parameters=[{'use_sim_time': True}, {'autostart': True}, {'node_names': ['map_server']}] ) ])

4.2 导航参数优化

Cartographer 地图需要特定的 Nav2 参数调整。在nav2_params.yaml中添加：

amcl: ros__parameters: alpha1: 0.2 # 降低旋转噪声模型 alpha4: 0.2 # 降低旋转噪声模型 laser_model_type: "likelihood_field" planner_server: ros__parameters: expected_planner_frequency: 5.0 planner_plugins: ["GridBased"] controller_server: ros__parameters: controller_frequency: 10.0 min_x_velocity_threshold: 0.1 min_y_velocity_threshold: 0.1 min_theta_velocity_threshold: 0.2

5. 两种导航模式性能对比

Cartographer 支持两种主要的导航集成方式，各有优缺点：

5.1 离线地图模式

实现步骤：

1. 完成环境建图并保存为 .pbstream
2. 转换为 ROS 标准格式
3. 通过 map_server 加载
4. 使用 AMCL 进行定位

性能特点：

• 系统资源占用低
• 定位精度依赖 AMCL 参数调优
• 无法利用 Cartographer 的全局一致性优化

5.2 在线建图模式

实现配置：

<node pkg="cartographer_ros" type="cartographer_node" name="cartographer_node"> <remap from="scan" to="/scan" /> <param name="use_sim_time" value="true" /> </node> <node pkg="cartographer_ros" type="occupancy_grid_node" name="occupancy_grid_node"> <param name="resolution" value="0.05" /> </node>

优势对比：

• 实时优化地图和位姿
• 自动处理回环检测
• 计算资源需求较高

注意：在线模式适合动态环境，但需要确保机器人计算平台有足够的处理能力。对于资源受限的平台，建议使用离线模式。