ROS中如何访问YAML格式栅格地图数据用于定位节点开发

直接读取YAML关联的地图数据 #

YAML文件本身仅存储地图元数据（分辨率、原点、尺寸、关联的.pgm栅格文件路径等），真正的栅格数值存在对应的.pgm图像中，步骤如下：

• 解析YAML文件，提取关键参数：resolution（分辨率，单位m/像素）、origin（地图原点坐标(x, y, yaw)）、width/height（地图宽高，单位像素）、image（.pgm文件路径）
• 读取.pgm文件：将图像像素值转换为占据栅格标准值——通常0代表自由空间，100代表完全占据，-1代表未知区域（注意.pgm的像素值为0-255，需映射：255对应-1，0对应100，中间值按比例转换）
• 坐标转换：将世界坐标(10,1)转为图像像素坐标：

  pixel_x = int((world_x - origin_x) / resolution)
  pixel_y = int((origin_y - world_y) / resolution)
  

  （图像坐标系y轴向上，ROS世界坐标系y轴向下，需反转y方向）
• 从图像的(pixel_y, pixel_x)位置读取像素值，再映射为占据栅格数值

无法直接访问YAML时的替代方案 #

如果无法直接读取本地YAML文件，可通过以下方式获取数据：

• 订阅ROS话题获取实时数据：在定位节点中订阅/map话题，接收nav_msgs/OccupancyGrid类型消息。该消息包含完整的栅格数据数组data、分辨率info.resolution、原点info.origin.position、尺寸info.width/info.height。计算目标坐标对应的数组索引：

  index = int((world_y - info.origin.position.y) / info.resolution) * info.width + int((world_x - info.origin.position.x) / info.resolution)
  

  检查索引在0到info.width*info.height-1范围内后，直接从data[index]获取占据数值
• 离线保存地图后处理：离线场景下，先通过rosrun map_server map_saver命令将/map话题的地图保存为本地YAML+PGM文件，再按第一种方法读取
• 使用Costmap库查询：基于ROS的costmap_2d库，初始化Costmap后可直接调用getCost(x, y)方法获取指定世界坐标的占据成本，适合实时定位中的动态地图查询

内容的提问来源于stack exchange，提问作者peyman movlani