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