16 KiB
16 KiB
Map 模块说明
Map 为粗路径规划提供只读、可复用的规划地图快照。它只负责把外部障碍物投影并栅格化，生成占据图和保守障碍距离场；不读取传感器、定位、UI 或系统时钟，也不写入 AMR 自身占据和安全外扩。
规划器应长期持有一个 PlanningMapFactory，通过一次 Create 调用取得 PlanningGridMap。
文件结构
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，或版本号无意义变化 |
长期复用工厂；仅在来源内容实际变化时递增版本 |