# Map 模块说明

`Map` 为粗路径规划提供只读、可复用的规划地图快照。它只负责把外部障碍物投影并栅格化，生成占据图和保守障碍距离场；不读取传感器、定位、UI 或系统时钟，也不写入 AMR 自身占据和安全外扩。

规划器应长期持有一个 `PlanningMapFactory`，通过一次 `Create` 调用取得 `PlanningGridMap`。

## 文件结构

```text
Map/
├── README.md                                      # 本模块说明：结构、数据流、单位和调用方式
├── PlanningMapRequest.cs                          # 公开建图输入：边界、分辨率、障碍来源、空图策略
├── PlanningMapBuildResult.cs                      # 公开建图输出：状态、地图、来源结果、失败原因、缓存命中类型
├── PlanningMapFactory.cs                          # 唯一公开建图门面；负责两级缓存和快照编号
├── Core/
│   ├── MapBoundsMm.cs                             # 世界地图范围及行列尺寸计算
│   ├── MapBuildRequest.cs                         # EnvironmentMapBuilder 的内部建图输入
│   ├── EnvironmentMapBuildResult.cs               # 环境栅格构建结果
│   ├── EnvironmentGridMap.cs                      # 构建期可写的环境占据栅格
│   └── EnvironmentMapBuilder.cs                   # 汇总障碍来源并事务性创建环境图
├── Obstacles/
│   ├── IMapObstacle.cs                            # 世界坐标障碍物几何契约
│   ├── CircleObstacle.cs                          # 圆形障碍物几何
│   ├── AxisAlignedRectangleObstacle.cs            # 与坐标轴平行的矩形障碍物几何
│   └── MapObstacleRasterizer.cs                   # 唯一允许写入环境栅格的障碍物栅格化器
├── Sources/
│   ├── IMapObstacleSource.cs                      # 统一障碍来源接口
│   ├── ObstacleSourceStatus.cs                    # 来源投影状态：已应用、空、不可用、无效
│   ├── ObstacleProjectionResult.cs                # 单个来源的世界几何和诊断结果
│   ├── ManualObstacleSource.cs                    # 手工输入的圆形/矩形障碍来源
│   ├── TwoLegProjectionInput.cs                   # 检测时刻的 TwoLeg 纯数据快照
│   ├── TwoLegObstacleProjector.cs                 # 将 TwoLeg 局部坐标投影为世界坐标圆障碍物
│   └── TwoLegObstacleSource.cs                    # 将 TwoLeg 快照包装为统一障碍来源
├── Planning/
│   ├── PlanningGridMap.cs                         # 不可变规划快照；规划查询使用米
│   ├── PlanningMapAdapter.cs                       # 环境图到规划快照与距离场的适配器
│   ├── EuclideanDistanceTransform.cs              # 二值栅格的精确平方欧氏距离变换
│   ├── ObstacleDistanceField.cs                   # 对规划器暴露的保守障碍净距
│   └── PlanningMapCache.cs                         # 容量为 4 的输入/占据两级 LRU 缓存
└── Test/
    ├── MovementTest.MapTest.cs                    # Clumsy 手工建图测试入口与终端调试开关
    └── Visualization/
        ├── PlanningMapImageExportRequest.cs       # PNG 导出输入：规划快照和输出目录
        ├── PlanningMapImageExportResult.cs        # PNG 导出状态、路径、尺寸和诊断
        ├── PlanningMapImageExporter.cs            # 可选 PNG 导出门面和输出保护
        ├── PlanningMapImageRenderer.cs            # 只读快照到 RGBA 像素的渲染器
        └── ValidatedPngWriter.cs                  # 写入并校验 PNG 结构和 CRC
```

## 建图数据流

```text
PlanningMapRequest
        │
        ▼
IMapObstacleSource.ProjectToWorld()
        │  输出世界坐标的圆形或矩形几何
        ▼
EnvironmentMapBuilder + MapObstacleRasterizer
        │  写入构建期 EnvironmentGridMap
        ▼
PlanningMapAdapter + ObstacleDistanceField
        │  生成占据数组和保守距离数组
        ▼
PlanningGridMap
```

具体规则：

1. 调用者准备 `PlanningMapRequest` 和一个或多个 `IMapObstacleSource`。
2. 每个来源通过 `ProjectToWorld()` 输出世界坐标几何；Map 核心不主动读取 TwoLeg、定位或其他传感器。
3. `EnvironmentMapBuilder` 按来源 ID 排序，必需来源失败则整个建图失败；可选来源失败只保留诊断状态。
4. `MapObstacleRasterizer` 是唯一写入 `EnvironmentGridMap` 占据格的组件。
5. `PlanningMapAdapter` 生成不可变 `PlanningGridMap`，并附带保守障碍距离场。
6. `PlanningMapFactory` 返回最终快照及来源投影结果；粗路径规划只应消费这个快照。

## 构建状态与停止

`PlanningMapBuildResult.Status` 的类型为 `PlanningMapBuildStatus`：

- `Success`：成功发布不可变 `PlanningGridMap`；
- `Failed`：输入、来源或常规建图失败，读取 `FailureReason`；
- `Cancelled`：调用方取消了带预算的建图，`Map` 为 `null`，不会写入缓存；
- `TimedOut`：建图耗尽调用方的总超时预算，`Map` 为 `null`，不会写入缓存。

公开的 `PlanningMapFactory.Create(request)` 保持兼容且不设置时间限制；粗规划门面使用内部预算入口，使取消和超时能够覆盖地图创建、距离场和后续搜索。

## 坐标与单位

- 环境地图边界、障碍物几何、TwoLeg 投影输入均使用世界坐标，单位为 **mm**。
- `MapBoundsMm` 范围采用左闭右开：`[XMin, XMax) × [YMin, YMax)`；最大边界不属于地图。
- `EnvironmentGridMap` 查询使用 mm；`PlanningGridMap` 的世界查询使用 **m**。
- 行 `row` 对应 Y 方向，列 `col` 对应 X 方向，存储顺序为行主序 `row * Cols + col`。
- `PlanningGridMap` 的越界位置按占据处理，障碍净距返回 0 m。
- Map 不做车辆自身占据或安全外扩；车辆外形与安全裕度由后续碰撞检测负责。

## 最小调用示例

```csharp
var mapFactory = new PlanningMapFactory(); // 长期持有，不要每次规划重新创建

IMapObstacle[] obstacles =
{
    new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
    new CircleObstacle(3600f, 1200f, 180f),
};

var request = new PlanningMapRequest
{
    Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
    ResolutionMm = 50f,
    ObstacleSources = new IMapObstacleSource[]
    {
        new ManualObstacleSource("manual", 1L, true, obstacles),
    },
    AllowExplicitEmptyMap = false,
};

PlanningMapBuildResult result = mapFactory.Create(request);
if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
    throw new InvalidOperationException(result.FailureReason);

PlanningGridMap map = result.Map; // 交给粗路径规划器
```

真实 TwoLeg 数据应由上层检测模块在检测时刻构造成 `TwoLegProjectionInput`，再交给 `TwoLegObstacleSource`。不要让 Map 模块主动读取检测器或定位器。

## 缓存与版本

`PlanningMapFactory` 内部有容量为 4 的两级 LRU 缓存：

- **输入命中（Input）**：边界、分辨率、空图策略、来源 ID、`SourceVersion` 和必需性均相同，直接返回同一个 `PlanningGridMap` 对象。
- **占据命中（Occupancy）**：来源版本变化，但最终占据栅格相同，复用不可变的占据/距离数组，并颁发新的 `SnapshotId`。
- **未命中（None）**：栅格内容变化，重新生成规划快照。

因此，障碍来源的快照内容发生变化时，调用方必须增加其 `SourceVersion`。未递增版本会错误复用旧地图；仅修改日志、PNG 开关、起终点或车辆参数不应改变地图版本。

## 测试与调试

- `Test/MovementTest.MapTest.cs` 是手工 Clumsy 测试入口。文件顶部可设置地图范围、分辨率、TwoLeg 测试快照、`EnableTerminalDebugLog` 和 `SavePng`。
- `PlanningMapImageExporter` 仅在 `SavePng` 开启时输出 PNG；它只读取 `PlanningGridMap`，不参与建图、缓存键或规划结果。
- 自动检查脚本位于 `ClumsyPilot/tests`：工具、工厂、适配器、PNG 和 MapTest 配置分别有独立验证脚本。
- 旧版 `Occupancygird_Map/Map_test/TrapMapImageExporter.cs` 与 `MovementTest.Trapmaptest.cs` 仍保留作历史对照；它们不是新粗路径规划的运行时地图入口。

## 详细使用指南

本节说明调用方如何从“障碍物数据”逐步得到可交给粗路径规划器的 `PlanningGridMap`。新地图的唯一创建入口是：

```csharp
PlanningMapBuildResult result = mapFactory.Create(request);
```

其中 `mapFactory` 是长期持有的 `PlanningMapFactory`，`request` 是本次建图输入，`result.Map` 是成功时的只读规划地图。

### 第 1 步：长期创建地图工厂

地图工厂内部维护容量为 4 的缓存，因此不要在每次规划前重新创建它。应把它作为规划服务或 MovementTest 的字段长期保存：

```csharp
using MultiWheelC.TrajectoryPlanning.Mapping;

private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
```

### 第 2 步：准备手工障碍物

圆形和轴对齐矩形都实现 `IMapObstacle`。障碍物的坐标是**世界坐标**，单位都是 **mm**。

```csharp
IMapObstacle[] manualObstacles =
{
    // 参数依次为：X最小值、X最大值、Y最小值、Y最大值，单位均为 mm。
    new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),

    // 参数依次为：圆心X、圆心Y、半径，单位均为 mm。
    new CircleObstacle(3600f, 1200f, 180f),
};
```

目前 Map 只负责“环境障碍物”。不要在这里添加 AMR 自身外形，也不要在圆半径或矩形尺寸中叠加安全裕度；车辆外形与安全距离由粗路径的碰撞检查处理。

### 第 3 步：包装为统一障碍来源

所有障碍物必须通过 `IMapObstacleSource` 进入地图。手工障碍使用 `ManualObstacleSource`：

```csharp
var manualSource = new ManualObstacleSource(
    sourceId: "manual",
    sourceVersion: 1L,
    isRequired: true,
    obstacles: manualObstacles);
```

参数意义如下：

- `sourceId`：来源的唯一名称；同一次请求内不能重复。
- `sourceVersion`：来源快照版本。障碍物位置、数量、半径或尺寸变化后，必须递增。
- `isRequired`：`true` 表示来源无效时整次建图失败；`false` 表示仅记录该来源状态并继续建图。
- `obstacles`：当前时刻的不可变障碍物集合。

例如障碍物内容变化后，应创建带新版本号的来源：

```csharp
var changedManualSource = new ManualObstacleSource(
    "manual",
    2L,             // 1L 变为 2L，通知工厂地图输入已改变
    true,
    changedObstacles);
```

### 第 4 步：可选地加入 TwoLeg 障碍物

Map 不主动调用 TwoLeg 检测器。上层检测模块应在检测时刻取得 AMR 世界位姿和两腿局部坐标，构造成 `TwoLegProjectionInput`：

```csharp
var twoLegInput = new TwoLegProjectionInput(
    hasDetection: true,

    // 检测时 AMR 的世界位姿：位置单位 mm，航向单位 rad。
    detectionWorldX: 1000f,
    detectionWorldY: 2000f,
    detectionHeadingRadians: 0d,

    // 两条腿相对于检测时 AMR 位姿的局部坐标，单位 mm。
    firstLocalX: 300f,
    firstLocalY: 150f,
    secondLocalX: 300f,
    secondLocalY: -150f,

    // 每条腿最终投影为圆形障碍物的半径，单位 mm。
    radiusMm: 80f,
    diagnostic: "TwoLeg 检测快照");

var twoLegSource = new TwoLegObstacleSource(
    sourceId: "two-leg",
    sourceVersion: 5L,
    isRequired: false,
    input: twoLegInput);
```

没有检测结果时仍可传入空快照：

```csharp
var noTwoLegInput = new TwoLegProjectionInput(
    false, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f,
    "当前没有 TwoLeg 检测结果");
```

`hasDetection` 为 `false` 时，该来源会返回“空”结果，不会将零坐标当作障碍物。若 TwoLeg 只是可选信息，建议 `isRequired` 设置为 `false`。

### 第 5 步：创建建图请求

边界与分辨率仍使用 **mm**。范围采用左闭右开，例如 `XMax = 6000f` 时，`x = 6000f` 不属于地图。

```csharp
var request = new PlanningMapRequest
{
    // 世界范围：[0, 6000) × [0, 4000)，单位 mm。
    Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),

    // 每格 50 mm。
    ResolutionMm = 50f,

    ObstacleSources = new IMapObstacleSource[]
    {
        manualSource,
        twoLegSource,
    },

    // false：没有任何有效障碍物时，地图不能直接交给规划器。
    // true：调用方明确确认空地图安全时，才允许空图参与规划。
    AllowExplicitEmptyMap = false,
};
```

### 第 6 步：创建地图并处理失败

```csharp
PlanningMapBuildResult result = _mapFactory.Create(request);

if (!result.Succeeded)
{
    Console.WriteLine("建图失败：" + result.FailureReason);
    return;
}

if (result.Map == null || !result.Map.PlanningReady)
{
    Console.WriteLine("地图不能用于规划：" + result.Map?.PlanningBlockReason);
    return;
}

PlanningGridMap planningMap = result.Map;
```

此处必须同时检查：

- `Succeeded`：请求、来源和栅格构建过程是否成功；
- `Map != null`：是否产出了规划快照；
- `PlanningReady`：是否允许把该快照交给规划器。隐式空图会在这一项被拦截。

需要查看每个来源是否成功投影时，读取 `result.SourceResults`；需要观察缓存效果时，读取 `result.CacheHit`。

```csharp
Console.WriteLine(
    "快照编号=" + planningMap.SnapshotId
    + "，缓存=" + result.CacheHit
    + "，栅格=" + planningMap.Cols + "×" + planningMap.Rows);
```

### 第 7 步：交给粗路径规划器查询

`PlanningGridMap` 是不可变对象，可以安全地作为一次规划任务的输入。注意：它的世界查询坐标单位已经变成 **m**，而不是 mm。

```csharp
// 查询 (2.5 m, 1.0 m) 是否位于占据格；越界也会返回 true。
bool occupied = planningMap.IsOccupiedWorld(2.5d, 1.0d);

// 查询该位置到最近障碍物的保守净距，单位 m；越界返回 0 m。
double clearanceMeters =
    planningMap.GetConservativeObstacleDistanceMeters(2.5d, 1.0d);
```

粗路径规划器应只使用 `PlanningGridMap` 的占据与距离查询，不应直接修改或重建其中的栅格。

### 完整实例

```csharp
private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();

private PlanningGridMap CreateMapForCoarsePlanning()
{
    IMapObstacle[] obstacles =
    {
        new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
        new CircleObstacle(3600f, 1200f, 180f),
    };

    var manualSource = new ManualObstacleSource("manual", 1L, true, obstacles);

    var request = new PlanningMapRequest
    {
        Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
        ResolutionMm = 50f,
        ObstacleSources = new IMapObstacleSource[] { manualSource },
        AllowExplicitEmptyMap = false,
    };

    PlanningMapBuildResult result = _mapFactory.Create(request);
    if (!result.Succeeded)
        throw new InvalidOperationException("建图失败：" + result.FailureReason);
    if (result.Map == null || !result.Map.PlanningReady)
        throw new InvalidOperationException("地图不可规划：" + result.Map?.PlanningBlockReason);

    return result.Map;
}
```

### 常见错误

| 情况 | 原因 | 处理方式 |
| --- | --- | --- |
| 改了障碍物但地图仍复用旧快照 | 未递增 `SourceVersion` | 障碍内容每次变化后增加该来源版本号 |
| 规划查询位置总是越界 | 将 mm 坐标传给了 `PlanningGridMap` | 规划查询前将 mm 除以 1000 转为 m |
| 地图创建成功但不能规划 | 未明确允许空图且没有有效障碍物 | 补充有效来源，或确认安全后设置 `AllowExplicitEmptyMap = true` |
| TwoLeg 出现在错误位置 | 未使用检测时刻位姿，或混用了 mm 与 m | 用检测时刻的世界位姿和局部 mm 坐标创建快照 |
| 缓存总是未命中 | 每次都新建 `PlanningMapFactory`，或版本号无意义变化 | 长期复用工厂；仅在来源内容实际变化时递增版本 |