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

建图数据流

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

具体规则：

调用者准备 PlanningMapRequest 和一个或多个 IMapObstacleSource。
每个来源通过 ProjectToWorld() 输出世界坐标几何；Map 核心不主动读取 TwoLeg、定位或其他传感器。
EnvironmentMapBuilder 按来源 ID 排序，必需来源失败则整个建图失败；可选来源失败只保留诊断状态。
MapObstacleRasterizer 是唯一写入 EnvironmentGridMap 占据格的组件。
PlanningMapAdapter 生成不可变 PlanningGridMap，并附带保守障碍距离场。
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 不做车辆自身占据或安全外扩；车辆外形与安全裕度由后续碰撞检测负责。

最小调用示例

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。新地图的唯一创建入口是：

PlanningMapBuildResult result = mapFactory.Create(request);

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

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

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

using MultiWheelC.TrajectoryPlanning.Mapping;

private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();

第 2 步：准备手工障碍物

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

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：

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

参数意义如下：

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

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

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

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

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

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);

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

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 不属于地图。

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 步：创建地图并处理失败

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。

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

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

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

// 查询 (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 的占据与距离查询，不应直接修改或重建其中的栅格。

完整实例

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`，或版本号无意义变化	长期复用工厂；仅在来源内容实际变化时递增版本

README.md Unescape Escape

Map 模块说明

文件结构