0. Map 地图模块指南

autonomy/map：环境地图子系统，对齐 nav2 nav2_costmap_2d 与 ETH grid_map。输出 OccupancyGrid（静态）、Costmap2D（规划/避障）、GridMap（地形/高程）。

本文 §0	其他文档
上手、配置、排错	§1 架构 · §2 组件 · §5 综述

0.1 文档地图

角色	阅读顺序
新手	§0.2 → §1 → §2
集成	§0.2、§0.5、§0.7 → §3 图层
算法	§5 → §2 → §3–§4

§	文档	内容
0	本指南	上手、形式化、配置、排错
1	架构	三条管线、数据流、线程
2	组件总览	MapServer / Costmap2D / GridMap 对比
3–4	Costmap2D · GridMap	实现专题
5	综述	历史、分类、选型

0.2 快速开始

Costmap2D（三步）

config/planner/planner.lua — 配置 costmap 块（图层、分辨率、footprint）
config/autonomy.lua — planning = AUTONOMY_PLANNER（PlannerServer 自动构造 Costmap2DWrapper）
确保 MapServer 发布静态地图，或 static_layer 订阅 /map

-- config/planner/planner.lua
costmap = {
    enabled = true,
    name = "global_map",
    frame_id = "map",
    resolution = 0.05,
    width = 20.0,
    height = 20.0,
    update_frequency = 5.0,
    robot_radius = 0.22,
    plugins = {"static_layer", "obstacle_layer", "inflation_layer"},
    static_layer = { enabled = true, map_topic = "map" },
    obstacle_layer = {
        enabled = true,
        sensor_sources = {
            scan = { topic = "scan", data_type = "LaserScan", marking = true, clearing = true },
        },
    },
    inflation_layer = { enabled = true, inflation_radius = 0.35, cost_scaling_factor = 3.0 },
}

#include "autonomy/map/costmap_2d/costmap_2d_wrapper.hpp"
auto costmap = std::make_shared<autonomy::map::costmap_2d::Costmap2DWrapper>(
    autonomy::map::CreateCostmap2DOptions("config"), "global_costmap");
costmap->Start();
costmap->updateRobotPose(x, y, yaw);

MapServer

auto server = std::make_shared<autonomy::map::MapServer>(
    autonomy::map::LoadOptions("config").map_server());
server->SetMapPublishCallback([](const auto& grid) { /* → static_layer */ });
server->Start();

GridMap

auto grid = std::make_shared<autonomy::map::grid_map::GridMapWrapper>(
    autonomy::map::CreateGridMapOptions("config"));
grid->Start();
// 应用层写入 elevation；查询见 §4

验证：MapServer::HasStaticMap() · costmap->isCurrent() · 自由区 ≈ 0、障碍 ≈ 254。

运行时序见 §1.3.4；组件对比见 §2。

0.3 环境形式化

平面环境 \(\mathcal{W} \subset \mathbb{R}^2\) 离散为栅格 \(\mathcal{M} = \{ c_{ij} \}\)，分辨率 \(\Delta\) 将 \(\mathbf{p}=(x,y)^\top\) 映射到索引 \((m_x, m_y)\)。Autonomy 提供两套坐标约定：

属性	Costmap2D	GridMap
原点	地图左下角 \((x_0, y_0)\)	地图中心 \(\mathbf{p}_{\text{map}}\)
索引	\(idx = m_y \cdot N_x + m_x\)	Eigen 矩阵 + 循环 buffer
数值	离散 0–255	连续 float，`NaN` 表无效

互转须显式处理原点差异；公式见 §3.5 · §4.6。

0.4 代价值语义

规划读 Costmap2D 快照；代价值为 Planning / Control 公共输入。

值	常量	含义
0	`FREE_SPACE`	完全自由
1–252	—	距障碍越近代价越高
253	`INSCRIBED_INFLATED_OBSTACLE`	内切膨胀区
254	`LETHAL_OBSTACLE`	致命障碍
255	`NO_INFORMATION`	未知

膨胀公式与图层见 §3.6–§3.8；Planning 侧解读见 Planning §0.4。

0.5 配置与 API

Costmap2D

项	值
配置	`config/planner/planner.lua` → `costmap`
加载	`map::CreateCostmap2DOptions("config")`
集成	`PlannerServer` 自动构造并 `Start()`

端到端（经 PlannerServer）

MapServer::Start() → PlannerServer 构造 Costmap2DWrapper
→ static_layer 收地图 → bridge 转发 /scan → TF 就绪 → isReady()

API	何时
`Start()`	系统启动
`applyOccupancyGrid`	静态地图到达
`feedLaserScan`	每次激光回调
`getCostmap()` + lock	规划前复制快照
`isReady()` / `isCurrent()`	规划前检查

图层参数要点见 §3.9；详细 API 见 §3.3。

GridMap

项	值
Proto	`map_grid_option.proto`
封装	`GridMapWrapper`
差异	无自动传感器管线；应用层投影点云到 `elevation`

详见 §4.3。

MapServer

API	用途
`Start()`	加载并发布
`GetStaticMapShared()`	获取 `OccupancyGrid`
`SetMapPublishCallback`	转发给 `applyOccupancyGrid`

0.6 自定义 Costmap 图层

继承 CostmapLayer 或 Layer，实现 updateBounds() / updateCosts()
编译为 .so，在 plugins 列表注册
Lua 中添加对应参数块

参考 autonomy/map/costmap_2d/layers/；架构见 §1.12。

0.7 故障排查

现象	常见原因	处理
`PlannerTimedOut`	costmap 未更新	`update_frequency`、`isCurrent()`
路径穿墙	膨胀不足	增大 `inflation_radius`
机器人被标为障碍	footprint 未清除	`footprint_clearing_enabled=true`
全未知 (255)	静态图未加载	MapServer、`static_layer.map_topic`
坐标偏移	TF / `frame_id`	统一 frame 与 TF 树
GridMap 查询 NaN	无数据或越界	检查 `move()` 后 buffer

检查清单：HasStaticMap() · isCurrent() · worldToMap 边界 · 可视化 costmap 快照。

性能建议：全局图 ≤ 2048² · 更新 5 Hz · 规划时复制 char map 避免长持锁。详见 §3.3.6。