# Hybrid A* 粗路径规划实施计划

> **For agentic workers:** REQUIRED SUB-SKILL: Use `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，按任务顺序实施，并使用 `- [ ]` 更新执行状态。

**目标：** 在 ClumsyPilot `netstandard2.0` 项目中交付可复用的静态规划地图和 Hybrid A* 粗路径规划模块，通过一次 `CoarsePathPlanningService.Plan(job)` 完成建图、快照复用、粗路径搜索和可选调试发布。

**架构：** `Map` 模块统一接收人工障碍、TwoLeg 及未来障碍来源，生成只含外部障碍物的不可变 `PlanningGridMap`；`HybridAStarPlanner` 只消费该快照并完成保守碰撞检查与搜索；`CoarsePathPlanningService` 负责编排。核心逻辑不读取传感器、UI 或系统时间，Clumsy `MovementTest` 和 PNG 导出只作为旁路适配器。

**技术栈：** C# 10、.NET Standard 2.0、PowerShell 反射契约测试、Clumsy `MovementTest`、StbImageWriteSharp 1.16.7。

**设计依据：** `docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md`

## 全局约束

- 所有命令均从仓库根目录执行；项目文件为 `ClumsyPilot/ClumsyPilot.csproj`。
- 目标框架保持 `netstandard2.0`，不得直接使用 `PriorityQueue`、`Math.Clamp`、`double.IsFinite` 或依赖 `record/init` 的实现。
- 地图边界、人工障碍和 TwoLeg 快照使用世界坐标 mm；规划内部统一使用 m、rad、1/m。
- 地图只保存外部障碍物，不写入 AMR 自身足迹，不在地图侧添加车辆安全膨胀。
- 安全余量只在碰撞检查时扩大车体矩形，防止地图膨胀与车体膨胀重复计算。
- `ResolutionMm` 必须在 20～200 mm；地图最多 4,000,000 格，分配前使用 `checked` 检查。
- 地图坐标采用 `[XMin, XMax) × [YMin, YMax)`；地图外始终按占据处理。
- `PrimitiveLengthMeters=0.50` 表示原语最大长度；`IntegrationStepMeters=0.05` 表示积分最大步长。
- 碰撞采样中心位移不得超过 `min(0.025 m, Map.ResolutionMeters / 2)`。
- 默认终点容差为 0.15 m 和 5°。每个原语在内部积分点逐点检查目标，第一次满足条件即截断为终点候选。
- 生成终点候选时不能立即成功；候选必须进入 Open List，作为当前最佳有效条目出队时才成功。
- 第一版只支持恒曲率前进、倒车及原语边界换向；不支持蟹行、横移、原地旋转、Reeds-Shepp 精确连接、平滑、速度规划或底盘控制。
- PowerShell 脚本首行设置 `$ErrorActionPreference = 'Stop'`，统一加载 `bin/Debug/netstandard2.0/ClumsyPilot.dll`。
- 用户已明确不需要 Git 自检。本计划不包含 `git diff`、`git add`、`git commit` 等步骤；版本管理由用户另行处理。

## P0 与 P1 完成定义

| 阶段 | 必须完成的结果 | 阶段出口 |
| --- | --- | --- |
| P0-MAP：当前首要工作 | Map 文件结构、统一障碍来源、TwoLeg 投影、栅格化、距离场、快照缓存、PNG 迁移、自动测试和 `MovementTest.MapTest` 实际调用 | Map 独立构建成功；3 个 Map 脚本通过；Clumsy MapTest 只通过 `PlanningMapFactory.Create` 完成建图与显示 |
| P0-PLAN：Map Gate 之后 | CoarsePath 契约、保守碰撞、原语截断、Open List、Hybrid A*、输出校验和一次调用门面 | 6 个非 UI P0 脚本全部通过；固定场景可由 `CoarsePathPlanningService` 返回经最终复核的粗路径 |
| P1：集成与优化 | CoarsePath MovementTest、性能基准、旧地图退役和 README | 7 个功能脚本及 Release 基准通过；UI 入口只做调试展示；旧地图类型不再成为规划运行时入口 |

执行顺序固定为“P0-MAP 独立闭环 → P0-PLAN → P1”。除了 Map 自己的 `MovementTest.MapTest` 和 PNG 调试辅助外，在 Map Gate 通过前不编写粗路径搜索或 CoarsePath UI，避免旧 `MovementTest.Trapmaptest.cs` 的传感器和渲染依赖进入核心模块。

## 当前首要里程碑：P0-MAP 独立闭环

当前阶段只处理 `Utils` 中被 Map 使用的无状态工具、完整 `Map` 目录、Map 自动化测试和 Map 实际调用。不得提前创建 `CoarsePath/Search`、`CoarsePath/Vehicle` 或 `CoarsePath/Output` 运行时代码。

### Map 实施顺序

1. 建立 `Map/Core`、`Map/Obstacles`、`Map/Sources`、`Map/Planning`、`Map/Test/Visualization` 目录和命名空间。
2. 完成 `MapBoundsMm`、连续行优先 `EnvironmentGridMap`、圆/矩形 DTO 和唯一栅格化器。
3. 完成统一 `IMapObstacleSource`、人工来源、TwoLeg 快照来源和事务式 `EnvironmentMapBuilder`。
4. 完成 `PlanningGridMap`、精确 EDT、保守距离场和 mm→m 适配。
5. 完成 `PlanningMapFactory`、容量 4 的两级快照缓存和地图变化判断。
6. 将旧 `TrapMapImageExporter.cs` 能力拆到 `Map/Test/Visualization`，只消费只读快照。
7. 完成三个 Map PowerShell 脚本和 `MovementTest.MapTest`，用真实入口验证建图、复用和显示。

对应详细任务的执行次序为：`Task 1 → Task 3 → Task 4 → Task 5 → Task 6 → Task 13 → Task 14 的 MapTest 部分 → Map Gate`。`Task 2` 和 `Task 7～12` 在 Map Gate 之后执行。

### Map 实际调用契约

`PlanningMapFactory` 应由长期存在的服务或测试对象持有，不能每次调用都重新 `new`，否则容量 4 的快照缓存无法跨调用复用：

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

public PlanningMapBuildResult CreateCurrentMap(
    MapBoundsMm bounds,
    long manualVersion,
    IReadOnlyList<IMapObstacle> manualObstacles,
    long twoLegVersion,
    TwoLegProjectionInput twoLegSnapshot)
{
    return _mapFactory.Create(new PlanningMapRequest
    {
        Bounds = bounds,
        ResolutionMm = 50f,
        ObstacleSources = new IMapObstacleSource[]
        {
            new ManualObstacleSource(
                "manual", manualVersion, true, manualObstacles),
            new TwoLegObstacleSource(
                "two-leg", twoLegVersion, false, twoLegSnapshot),
        },
        AllowExplicitEmptyMap = false,
    });
}
```

调用方只判断统一结果，不接触 builder、rasterizer、投影器或 adapter：

```csharp
PlanningMapBuildResult result = CreateCurrentMap(
    bounds, manualVersion, manualObstacles, twoLegVersion, twoLegSnapshot);

if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
    return;

PlanningGridMap map = result.Map;
bool worldOriginIsOccupied = map.IsOccupiedWorld(0d, 0d);
double worldOriginClearance =
    map.GetConservativeObstacleDistanceMeters(0d, 0d);
```

### Map Gate

以下条件必须全部满足，才开始 CoarsePath 契约和搜索：

- [ ] 新 `Map` 运行时代码不读取 `TwoLegDetect`、定位、Painter、Toast、UI 或系统时间。
- [ ] 人工、TwoLeg 以及未来来源全部经 `IMapObstacleSource.ProjectToWorld()` 输出世界 mm 几何，再由唯一 rasterizer 写图。
- [ ] 地图不写 AMR 自身，不使用旧默认 300 mm 膨胀。
- [ ] `PlanningMapFactory.Create` 对完整输入命中、占据命中和占据变化给出可区分结果。
- [ ] `MovementTest.MapTest` 只调用 `PlanningMapFactory` 和可选 PNG exporter，不复制地图构造逻辑。
- [ ] 下列命令全部通过：

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
```

### P0-MAP 执行状态（2026-07-27）

- [x] 已完成 Task 1、Task 3～6、Task 13，以及 Task 14 的 `MovementTest.MapTest` 部分。
- [x] 已建立新的 `Map` 目录结构、统一障碍物来源与 TwoLeg 检测时位姿投影；Map 核心不读取传感器、UI 或系统时钟。
- [x] 已完成只读规划快照、保守 EDT 距离场、容量 4 的 LRU 缓存、PNG 可视化拆分和实际 MapTest 调用。
- [x] 已通过上述 Map Gate 命令；另外 `verify_planning_utils.ps1` 也已通过。
- [x] 未执行 Git 自检、暂存或提交。

## 最终目录与文件职责

```text
ClumsyPilot/ParkrobTrajplanner/
├── Initial_plan/                         ------ 方案与实施文档，不放运行时代码
├── Utils/                                ------ 通用、无状态、确定性数值工具
│   ├── AngleMath.cs                      ------ 角度归一化、最短角差和航向离散索引
│   ├── UnitConverter.cs                  ------ mm/m、deg/rad 和半径/曲率转换
│   ├── CoordinateTransform.cs            ------ 车体坐标与世界坐标二维刚体变换
│   ├── NumericGuard.cs                   ------ 有限值、正值和参数范围校验
│   └── GridIndex.cs                      ------ 不可变行列索引值对象
├── Map/                                  ------ MultiWheelC.TrajectoryPlanning.Mapping
│   ├── Core/
│   │   ├── EnvironmentGridMap.cs         ------ 只保存外部障碍物的 mm 占据栅格
│   │   ├── MapBoundsMm.cs                ------ 有限、非退化的 mm 地图边界
│   │   ├── MapBuildRequest.cs            ------ 内部环境图构建请求
│   │   ├── EnvironmentMapBuildResult.cs  ------ 环境图、来源摘要和失败原因
│   │   └── EnvironmentMapBuilder.cs      ------ 校验来源并事务式合并障碍图层
│   ├── Obstacles/
│   │   ├── IMapObstacle.cs               ------ 世界坐标障碍物公共几何契约
│   │   ├── AxisAlignedRectangleObstacle.cs ------ 轴对齐矩形障碍 DTO
│   │   ├── CircleObstacle.cs             ------ 圆形障碍 DTO
│   │   └── MapObstacleRasterizer.cs      ------ 唯一占据栅格写入器
│   ├── Sources/
│   │   ├── IMapObstacleSource.cs         ------ 纯快照障碍来源统一接口
│   │   ├── ObstacleSourceStatus.cs       ------ Applied/Empty/Unavailable/Invalid
│   │   ├── ObstacleProjectionResult.cs   ------ 来源版本、状态、诊断和障碍集合
│   │   ├── ManualObstacleSource.cs       ------ 输出人工圆和矩形
│   │   ├── TwoLegProjectionInput.cs      ------ 两腿端点、检测状态和检测位姿快照
│   │   ├── TwoLegObstacleSource.cs       ------ TwoLeg 统一来源适配器
│   │   └── TwoLegObstacleProjector.cs    ------ 车体系两腿端点投影到世界系
│   ├── Planning/
│   │   ├── PlanningGridMap.cs            ------ 只读 m 占据图、距离场和快照元数据
│   │   ├── EuclideanDistanceTransform.cs ------ 线性时间精确二维欧氏距离变换
│   │   ├── ObstacleDistanceField.cs      ------ 不高估真实净空的距离下界
│   │   ├── PlanningMapCache.cs           ------ 容量 4 的线程安全两级快照缓存
│   │   └── PlanningMapAdapter.cs         ------ 占据深拷贝、mm→m 和距离场生成
│   ├── PlanningMapRequest.cs             ------ Map 模块统一输入
│   ├── PlanningMapBuildResult.cs         ------ Map 模块统一输出和缓存命中类型
│   ├── PlanningMapFactory.cs             ------ Map 模块唯一公共创建入口
│   └── Test/
│       ├── MovementTest.MapTest.cs       ------ Clumsy UI 地图构建/显示入口
│       └── Visualization/
│           ├── PlanningMapImageExportRequest.cs ------ 快照、叠加层和输出选项
│           ├── PlanningMapImageExportResult.cs ------ PNG 状态、路径、尺寸和诊断
│           ├── PlanningMapImageExporter.cs ------ 校验、渲染编排和原子发布
│           ├── PlanningMapImageRenderer.cs ------ 地图/车辆/路径绘制到 RGBA
│           └── ValidatedPngWriter.cs     ------ Stb 编码、PNG 结构和 CRC 校验
├── CoarsePath/                           ------ MultiWheelC.TrajectoryPlanning.CoarsePath
│   ├── Contracts/
│   │   ├── Pose2D.cs                     ------ m/rad 不可变二维位姿
│   │   ├── TravelDirection.cs            ------ Forward/Reverse
│   │   ├── GoalDirectionConstraint.cs    ------ Any/Forward/Reverse
│   │   ├── VehicleParameters.cs          ------ 尺寸、安全余量和最大曲率
│   │   ├── PlanningRequest.cs            ------ 纯规划器输入
│   │   ├── HybridAStarConfiguration.cs   ------ 原语、离散、代价、限额和容差
│   │   ├── PlanningResult.cs             ------ 状态、诊断、稠密路径和分段
│   │   ├── PlanningStatus.cs             ------ 输入、碰撞、搜索和验证状态
│   │   ├── PlanningDiagnostics.cs        ------ 节点、堆、耗时、路径和终止统计
│   │   ├── CoarsePathPoint.cs            ------ 位姿、弧长、方向、曲率和净空
│   │   ├── CoarsePathPointSource.cs      ------ Start/MotionPrimitive/GoalTruncation
│   │   └── PathSegment.cs                ------ 包含式方向分段索引
│   ├── Vehicle/
│   │   ├── VehicleKinematics.cs          ------ 解析保守最大曲率
│   │   ├── VehicleFootprint.cs           ------ 扩大矩形、AABB 和外接圆
│   │   ├── OrientedRectangleCellIntersection.cs ------ 旋转矩形与格矩形精确相交
│   │   └── FootprintCollisionChecker.cs  ------ 边界、快速放行、精确和扫掠检查
│   ├── Search/
│   │   ├── MotionPrimitive.cs            ------ 恒曲率原语和实际截断长度
│   │   ├── MotionPrimitiveGenerator.cs   ------ 解析积分并保留内部采样点
│   │   ├── BinaryMinHeap.cs              ------ netstandard2.0 确定性 Open List
│   │   ├── SearchCostCalculator.cs       ------ 统一计算等效米搜索代价
│   │   ├── HybridAStarNode.cs            ------ 连续状态、代价、父索引和原语
│   │   ├── HybridAStarNodeKey.cs         ------ 位置/航向/方向/曲率离散键
│   │   ├── GoalToleranceChecker.cs       ------ 位置、航向和进入方向判断
│   │   ├── GridDijkstraHeuristic.cs      ------ 八邻域、禁止切角的二维启发
│   │   └── HybridAStarSearch.cs          ------ 扩展、重开、限额和候选管理
│   ├── Output/
│   │   ├── PathBacktracker.cs            ------ 根据父索引重建原语内部点
│   │   ├── CoarsePathAssembler.cs        ------ 弧长、换向点和方向分段
│   │   └── CoarsePathValidator.cs        ------ 数值、碰撞、曲率、终点和分段复核
│   ├── HybridAStarPlanner.cs             ------ 只消费 PlanningGridMap 的下层门面
│   ├── Facade/
│   │   ├── CoarsePathPlanningJob.cs      ------ 一次调用所需全部输入
│   │   ├── CoarsePathPlanningJobResult.cs ------ 地图结果与粗路径结果
│   │   ├── PlanningDebugOptions.cs       ------ 地图、路径、碰撞调试开关
│   │   ├── IPlanningDebugSink.cs         ------ 不改变规划状态的调试消费接口
│   │   └── CoarsePathPlanningService.cs  ------ 建图、搜索和调试的一次调用入口
│   └── Test/
│       ├── CoarsePathScenarioFactory.cs  ------ 固定、可复现的地图与规划场景
│       └── MovementTest.CoarsePathTest.cs ------ 后台规划并在 Clumsy UI 显示
│   └── README.md                         ------ 粗规划模块边界、调用示例和文档链接

ClumsyPilot/tests/
├── verify_planning_utils.ps1             ------ 工具与公共契约
├── verify_planning_map_factory.ps1       ------ 来源、事务、缓存与快照
├── verify_planning_map_adapter.ps1       ------ 栅格、适配和距离场
├── verify_planning_map_image.ps1         ------ PNG 渲染与原子发布
├── verify_coarse_path_collision.ps1      ------ 亚栅格、擦边和扫掠碰撞
├── verify_coarse_path_search.ps1         ------ 原语截断、堆、代价和搜索
├── verify_coarse_path_integration.ps1    ------ 一次调用端到端验证
└── benchmark_coarse_path.ps1             ------ Release 性能与内存门槛
```

## 固定公共调用方式

常规业务代码只保留一个长期存在的服务实例：

```csharp
var result = planningService.Plan(new CoarsePathPlanningJob
{
    MapRequest = new PlanningMapRequest
    {
        Bounds = bounds,
        ResolutionMm = 50f,
        ObstacleSources = new IMapObstacleSource[]
        {
            new ManualObstacleSource(
                "manual", manualVersion, true, manualObstacles),
            new TwoLegObstacleSource("two-leg", twoLegVersion, false, twoLegSnapshot),
        },
        AllowExplicitEmptyMap = true,
    },
    Start = startPose,
    Goal = goalPose,
    Vehicle = vehicle,
    Configuration = configuration,
    StartVehicleCurvature = 0d,
    StartDirection = null,
    GoalDirection = GoalDirectionConstraint.Any,
    Debug = new PlanningDebugOptions
    {
        VisualizeMap = true,
        VisualizePath = true,
        VisualizeCollisionChecks = false,
    },
}, cancellationToken);
```

已有地图快照复用或纯搜索测试才直接调用 `HybridAStarPlanner.Plan(PlanningRequest, CancellationToken)`。业务调用方不得直接拼装 builder、rasterizer、碰撞器、运动原语或搜索节点。

## P0：正确可用

### Task 1：建立 Utils 与测试基座

**文件：**

- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/AngleMath.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/UnitConverter.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/CoordinateTransform.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/NumericGuard.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/GridIndex.cs`
- Create: `ClumsyPilot/tests/verify_planning_utils.ps1`

**产出接口：**

```csharp
double AngleMath.NormalizeRadians(double radians);
double AngleMath.ShortestSignedDifference(double from, double to);
int AngleMath.ToHeadingIndex(double heading, double resolution, int binCount);
double UnitConverter.MillimetersToMeters(double value);
double UnitConverter.DegreesToRadians(double value);
bool NumericGuard.IsFinite(double value);
readonly struct GridIndex { int Row; int Col; }
```

- [ ] 写失败测试：断言 `2π→0`、`179°→-179°` 最短角差为 `2°`、`1250 mm→1.25 m`、车体系 `(1000,0)` 在世界位姿 `(2000,3000,90°)` 后得到 `(2,4) m`，并拒绝 NaN/Infinity。
- [ ] 执行以下命令，预期构建成功、脚本因新类型不存在返回非零。

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
```

- [ ] 实现：角度统一到 `[-π,π)`；航向索引先归一化再 `floor`；刚体变换使用 `world=origin+R(heading)×local`；`GridIndex` 实现值相等和稳定哈希。
- [ ] 重跑脚本，预期输出 `Planning utility checks passed.`。

### Task 2：锁定 CoarsePath 公共契约与默认参数

**文件：**

- Create: `CoarsePath/Contracts/` 下目录树列出的 12 个契约文件
- Modify: `ClumsyPilot/tests/verify_planning_utils.ps1`

**固定默认值：**

```csharp
PrimitiveLengthMeters = 0.50;
IntegrationStepMeters = 0.05;
MaximumCollisionCheckStepMeters = 0.025;
HeadingResolutionRadians = Math.PI / 36d;
CurvatureLevelCount = 5;
GoalPositionToleranceMeters = 0.15;
GoalHeadingToleranceRadians = Math.PI / 36d;
MaximumExpandedNodes = 200000;
SearchTimeout = TimeSpan.FromSeconds(5);
HeuristicWeight = 1.0;
ReverseCostMultiplier = 1.5;
GearSwitchPenaltyMeters = 1.0;
CurvatureMagnitudeWeight = 0.10;
CurvatureChangePenaltyMetersPerLevel = 0.05;
ClearanceCostWeight = 0.20;
ClearanceCostDistanceMeters = 0.50;
```

- [ ] 扩展失败测试：断言上述默认值，三个方向/来源枚举，以及 `PlanningRequest` 的起始曲率、起始方向、目标进入方向。
- [ ] 断言 `PlanningStatus` 包含：`Success`、`Cancelled`、`InvalidRequest`、`InvalidMap`、`MapNotReady`、`InvalidVehicleParameters`、`InvalidCurvatureConfiguration`、`StartOutsideMap`、`StartInCollision`、`GoalOutsideMap`、`GoalInCollision`、`SearchTimeout`、`SearchNodeLimitExceeded`、`NoFeasiblePath`、`BacktrackingFailed`、`FinalValidationFailed`、`InternalError`。
- [ ] 实现不可变 `Pose2D`；结果工厂只允许成功结果携带路径，失败结果路径为空。
- [ ] `PlanningDiagnostics` 固定记录扩展节点数、生成节点数、重新打开节点数、陈旧堆条目数、Open List 峰值、总路径长度、最小保守净空、耗时和终止原因。
- [ ] `CoarsePathPoint` 固定包含 `X/Y`、`Heading/UnwrappedHeading`、`ArcLength`、`Direction`、`VehicleCurvature`、`BodyClearance`、`IsGearSwitchPoint` 和 `Source`；`PathSegment` 固定包含方向和包含式 `StartIndex/EndIndex`。
- [ ] 重跑工具脚本，预期契约和默认值全部通过。

### Task 3：实现环境栅格、边界与统一栅格化

**文件：**

- Create: `Map/Core/MapBoundsMm.cs`
- Create: `Map/Core/EnvironmentGridMap.cs`
- Create: `Map/Obstacles/IMapObstacle.cs`
- Create: `Map/Obstacles/AxisAlignedRectangleObstacle.cs`
- Create: `Map/Obstacles/CircleObstacle.cs`
- Create: `Map/Obstacles/MapObstacleRasterizer.cs`
- Create: `ClumsyPilot/tests/verify_planning_map_adapter.ps1`

上述 Map/CoarsePath 相对路径均位于 `ClumsyPilot/ParkrobTrajplanner/`。

- [ ] 写边界和栅格失败测试：20/200 mm 合法，范围外失败；4,000,001 格分配前失败；`XMax/YMax` 排他；非完整末格裁剪；地图外占据；圆/矩形与格边或格角接触时保守占据；完全在地图外的合法障碍不写格。
- [ ] 运行脚本，预期新 Map 类型不存在。
- [ ] 实现私有行优先 `byte[]`，索引固定为 `row*Cols+col`；世界转格使用 `floor((value-min)/resolution)`；不得返回内部缓冲区。
- [ ] 实现唯一栅格化器：圆使用“圆心到格矩形最近点距离”，轴对齐矩形使用闭区间相交，只遍历裁剪后的候选包围盒。
- [ ] 确认不存在 `MarkVehicleFootprint` 或安全距离参数，重跑脚本通过。

### Task 4：统一人工与 TwoLeg 障碍来源

**文件：**

- Create: `Map/Sources/` 下目录树列出的 7 个文件
- Create: `Map/Core/MapBuildRequest.cs`
- Create: `Map/Core/EnvironmentMapBuildResult.cs`
- Create: `Map/Core/EnvironmentMapBuilder.cs`
- Create: `ClumsyPilot/tests/verify_planning_map_factory.ps1`

**固定接口：**

```csharp
public interface IMapObstacleSource
{
    string SourceId { get; }
    long SourceVersion { get; }
    bool IsRequired { get; }
    ObstacleProjectionResult ProjectToWorld();
}
```

- [ ] 构造函数统一为 `ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles)` 和 `TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)`；计划内所有实际调用均使用这两个签名。
- [ ] 写来源事务失败测试：人工 `Applied/Empty`、TwoLeg 两个圆、可选来源 `Unavailable/Invalid` 保留人工图层、必需来源失败导致整图失败、重复 `SourceId` 失败、输入顺序不同但结果相同。
- [ ] 校验 `SourceId` 非空且一次请求内唯一，`SourceVersion` 非负；来源快照内容发生变化时，上层必须递增版本。
- [ ] 写 TwoLeg 坐标测试：使用检测时车辆位姿完成车体系 mm 到世界系 mm 变换，不得使用规划开始时位姿。
- [ ] 实现纯快照投影：`ProjectToWorld()` 不能调用 `TwoLegDetect`、定位、UI 或系统时间；过期判断由上层采集适配器在构造 DTO 前完成。
- [ ] builder 按 `SourceId` 排序；可选失败只记诊断；必需失败不发布地图；全部成功几何交给唯一 rasterizer。
- [ ] 重跑工厂脚本，预期来源状态、投影和事务断言通过。

### Task 5：生成不可变 PlanningGridMap 和保守距离场

**文件：**

- Create: `Map/Planning/EuclideanDistanceTransform.cs`
- Create: `Map/Planning/ObstacleDistanceField.cs`
- Create: `Map/Planning/PlanningGridMap.cs`
- Create: `Map/Planning/PlanningMapAdapter.cs`
- Modify: `ClumsyPilot/tests/verify_planning_map_adapter.ps1`

- [ ] 扩展失败测试：mm→m、占据深拷贝、地图外距离为零、障碍格距离为零、空图距离正无穷、末格裁剪，以及所有样本距离不高于暴力几何距离。
- [ ] 实现两次一维平方距离变换，复杂度 `O(Rows×Cols)`；不得逐自由格遍历全部障碍格。
- [ ] 对精确栅格中心距离应用：

```csharp
Math.Max(0d, centerDistanceMeters - Math.Sqrt(2d) * resolutionMeters)
```

- [ ] 查询使用点所在格的保守值，不做可能抬高结果的插值；空图跳过 EDT；边界检查独立执行。
- [ ] `PlanningGridMap` 私有保存连续占据/距离数组，不提供写入口和缓冲区引用。
- [ ] 重跑地图适配脚本，预期栅格、深拷贝、空图和保守距离全部通过。

### Task 6：实现 PlanningMapFactory 与两级快照缓存

**文件：**

- Create: `Map/Planning/PlanningMapCache.cs`
- Create: `Map/PlanningMapRequest.cs`
- Create: `Map/PlanningMapBuildResult.cs`
- Create: `Map/PlanningMapFactory.cs`
- Modify: `Map/Planning/PlanningGridMap.cs`
- Modify: `ClumsyPilot/tests/verify_planning_map_factory.ps1`

```csharp
public sealed class PlanningMapFactory
{
    public PlanningMapBuildResult Create(PlanningMapRequest request);
}
```

`PlanningMapRequest` 固定包含 `MapBoundsMm Bounds`、`float ResolutionMm`、`IReadOnlyList<IMapObstacleSource> ObstacleSources`、`bool AllowExplicitEmptyMap`。

- [ ] 写缓存测试：完全相同请求返回同一 `PlanningGridMap`；来源版本变化但最终占据未变化时产生新 `SnapshotId` 并复用占据/距离缓冲；占据变化时重建距离场。
- [ ] 写规划可用性测试：至少一个成功来源提供有效障碍语义时 `PlanningReady=true`；所有来源均为空时仅 `AllowExplicitEmptyMap=true` 可用；必需来源失败或未明确空图语义时 `PlanningReady=false` 并填写 `PlanningBlockReason`，规划器随后返回 `MapNotReady`。
- [ ] 验证失效边界：起终点、车辆、规划参数和可视化开关不进入地图指纹；边界、分辨率、空图策略、来源状态/版本或规范化几何变化必须进入。
- [ ] 实现确定性 `InputFingerprint`：来源先按 ID 排序，字段固定顺序，浮点按 IEEE 位模式；命中后仍比较规范化结构。
- [ ] 栅格化后计算 `OccupancyHash`；命中后仍比较地图几何、数组长度和逐字节内容，不能只信任哈希。
- [ ] 实现容量 4 的线程安全 LRU；完整命中返回同一快照，占据命中共享不可变缓冲但生成新元数据，只有占据变化才运行 EDT。
- [ ] `PlanningMapBuildResult` 固定返回 `Succeeded`、失败原因、来源摘要、缓存命中类型和成功时的 `PlanningGridMap`；快照固定保存 `PlanningReady`、`PlanningBlockReason`、来源版本摘要、`InputFingerprint`、`OccupancyHash`、单调 `SnapshotId`。
- [ ] 增加 16 个并发相同请求测试和 LRU 淘汰测试，重跑脚本通过。

### Task 7：实现连续车体足迹和保守碰撞检查

**文件：**

- Create: `CoarsePath/Vehicle/` 下目录树列出的 4 个文件
- Create: `ClumsyPilot/tests/verify_coarse_path_collision.ps1`

**固定接口：**

```csharp
bool IsPoseCollisionFree(
    Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
    double additionalMarginMeters, out double bodyClearanceMeters);

bool IsSweptMotionCollisionFree(
    Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
    double maximumCenterStepMeters, out double minimumBodyClearanceMeters);
```

- [ ] 写失败测试：正交、45°、任意航向、栅格中心/亚栅格中心、边角接触、薄障碍、地图边界、距离场快速放行，以及两个无碰撞端点之间有障碍的扫掠场景。
- [ ] 实现以几何中心为参考的扩大车体；安全余量加到长度和宽度两侧；最大曲率与最小转弯半径并存时取更保守限制。
- [ ] 使用分离轴定理精确判断连续旋转矩形与占据格矩形相交，接触视为碰撞；不得使用离散航向模板。
- [ ] 检查顺序：扩大车体边界 → 保守距离严格大于外接圆时快速放行 → AABB 内占据格 SAT。
- [ ] 扫掠采样中心步长不超过 `min(configuredStep,map.Resolution/2)`；每段临时附加余量为：

```text
0.5 × (centerDisplacement
       + circumscribedRadius × abs(headingDelta))
```

- [ ] 重跑碰撞脚本，预期所有亚栅格、擦边和扫掠案例通过。

### Task 8：实现解析恒曲率原语和内部终点截断

**文件：**

- Create: `CoarsePath/Search/MotionPrimitive.cs`
- Create: `CoarsePath/Search/MotionPrimitiveGenerator.cs`
- Create: `CoarsePath/Search/GoalToleranceChecker.cs`
- Create/Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`

**解析积分：**

```csharp
double signedDistance = direction == TravelDirection.Forward ? step : -step;
double nextHeading = AngleMath.NormalizeRadians(
    heading + curvature * signedDistance);

if (Math.Abs(curvature) < 1e-12)
{
    nextX = x + signedDistance * Math.Cos(heading);
    nextY = y + signedDistance * Math.Sin(heading);
}
else
{
    nextX = x + (Math.Sin(nextHeading) - Math.Sin(heading)) / curvature;
    nextY = y - (Math.Cos(nextHeading) - Math.Cos(heading)) / curvature;
}
```

- [ ] 写原语测试：直行、圆弧、倒车、五级曲率、相邻曲率最多变化一级、最大长度 0.50 m、实际采样步长不超过 `min(IntegrationStep,CollisionStep,MapResolution/2)`。
- [ ] 写用户提出的案例：目标距起点 0.30 m、原语最大 0.50 m、收紧容差；断言第一个满足目标的内部点截断，后续点不生成，来源为 `GoalTruncation`。
- [ ] 每个内部点严格按“有限值 → 扫掠碰撞 → 目标条件”检查；碰撞必须先于目标。
- [ ] 起点已满足目标时创建零长度候选，不生成原语。
- [ ] 重跑搜索脚本，预期几何、碰撞采样和 0.30 m 截断案例通过。

### Task 9：实现 Open List、统一代价和二维启发

**文件：**

- Create: `CoarsePath/Search/BinaryMinHeap.cs`
- Create: `CoarsePath/Search/SearchCostCalculator.cs`
- Create: `CoarsePath/Search/GridDijkstraHeuristic.cs`
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`

**固定代价：**

```text
primitiveCost =
    lengthMeters
  × directionMultiplier
  × (1
     + CurvatureMagnitudeWeight × abs(curvature / maximumCurvature)
     + ClearanceCostWeight × max(0, 1 - clearance / ClearanceCostDistanceMeters))
  + gearSwitchPenalty
  + CurvatureChangePenaltyMetersPerLevel × abs(curvatureLevelDelta)
```

- [ ] 写堆顺序测试：较小 `F`、较小 `H`、较大 `G`、较小插入序号；相同输入重复运行顺序一致。
- [ ] 写代价测试：前进、倒车、换向、曲率幅值、曲率变化和净空项；拒绝负数及非有限权重。
- [ ] 写 Dijkstra 测试：八邻域直/斜代价；两个正交邻格任一占据时禁止对角切角；二维不可达返回明确状态。
- [ ] 实现专用二叉最小堆，不依赖 `PriorityQueue`；允许旧条目由搜索层惰性丢弃。
- [ ] `HeuristicWeight=1` 使用 `F=G+H`；大于 1 时只承诺可行性。
- [ ] 重跑搜索脚本，预期顺序、公式和切角限制通过。

### Task 10：实现 Hybrid A* 节点、重开和终点候选管理

**文件：**

- Create: `CoarsePath/Search/HybridAStarNode.cs`
- Create: `CoarsePath/Search/HybridAStarNodeKey.cs`
- Create: `CoarsePath/Search/HybridAStarSearch.cs`
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`

- [ ] 写搜索测试：空图前进、单矩形绕行、允许倒车的狭窄场景、起始曲率、目标进入方向、`±π` 容差、无解、取消、超时、节点上限、重开和确定性。
- [ ] 写候选排序测试：先生成较大 `F` 的终点候选时不得结束；较小 `F` 普通节点先出队；候选成为最佳有效条目后才成功。
- [ ] 离散键固定为位置格、航向格、方向和曲率等级；连续位姿保留在节点中。
- [ ] `Dictionary<HybridAStarNodeKey,double>` 保存普通状态最佳 `G`；更小 `G` 允许重开；旧普通堆条目惰性丢弃。
- [ ] 终点候选放入同一堆，但不能仅因另一个连续位姿落入相同离散键且 `G` 更低而被删除；候选出队时重新验证目标和末段碰撞。
- [ ] 搜索循环在每次扩展前按顺序检查取消、5 s 超时、200,000 节点上限，再弹出有效条目；Open List 为空返回 `NoFeasiblePath`。
- [ ] 重跑搜索脚本，预期候选顺序、重开、限额和所有场景通过。

### Task 11：回溯、装配、最终复核和 HybridAStarPlanner

**文件：**

- Create: `CoarsePath/Output/PathBacktracker.cs`
- Create: `CoarsePath/Output/CoarsePathAssembler.cs`
- Create: `CoarsePath/Output/CoarsePathValidator.cs`
- Create: `CoarsePath/HybridAStarPlanner.cs`
- Create: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`

```csharp
public sealed class HybridAStarPlanner
{
    public PlanningResult Plan(
        PlanningRequest request,
        CancellationToken cancellationToken = default(CancellationToken));
}
```

- [ ] 写失败状态测试：请求、地图就绪、车辆参数、曲率配置、起终点越界/碰撞，以及搜索失败到结果的无异常映射。
- [ ] 写输出测试：首点弧长零；弧长不递减；`UnwrappedHeading` 连续；终点截断来源正确；除换向对外无相邻重复点。
- [ ] 写换向分段测试：换向处保留两个坐标/航向/弧长相同而方向不同的点；新方向点标记 `IsGearSwitchPoint`；包含式分段完整覆盖路径。
- [ ] 搜索节点只存父索引、方向、曲率和实际原语长度；成功后使用相同解析积分和有效步长重建内部点。
- [ ] 最终复核有限数值、曲率、扫掠碰撞、目标容差/方向、弧长、换向对和分段；失败返回 `FinalValidationFailed` 且不发布部分路径。
- [ ] 重跑碰撞、搜索和集成脚本，预期全部通过。

### Task 12：实现一次调用 CoarsePathPlanningService

**文件：**

- Create: `CoarsePath/Facade/` 下目录树列出的 5 个文件
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`

```csharp
public sealed class CoarsePathPlanningService
{
    public CoarsePathPlanningJobResult Plan(
        CoarsePathPlanningJob job,
        CancellationToken cancellationToken = default(CancellationToken));
}
```

- [ ] 写一次调用测试：固定执行 `PlanningMapFactory.Create → HybridAStarPlanner.Plan → debug sink`；地图失败不启动搜索；结果同时保留地图和规划结果。
- [ ] 写旁路隔离测试：可视化开关不改变地图指纹、占据哈希、规划状态或路径；debug sink 异常只写调试诊断。
- [ ] 服务持有同一 `PlanningMapFactory`，多次调用共享容量 4 缓存；默认 debug sink 为空行为。
- [ ] 重跑以下 P0 验收，预期构建成功且 6 个脚本退出码为 0。

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```

## P0-MAP 收尾与 P1：集成、迁移和性能

### Task 13（P0-MAP）：拆分迁移 TrapMapImageExporter

**现有来源：**

- Read/Migrate: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs`

**新文件：**

- Create: `Map/Test/Visualization/PlanningMapImageExportRequest.cs`
- Create: `Map/Test/Visualization/PlanningMapImageExportResult.cs`
- Create: `Map/Test/Visualization/PlanningMapImageExporter.cs`
- Create: `Map/Test/Visualization/PlanningMapImageRenderer.cs`
- Create: `Map/Test/Visualization/ValidatedPngWriter.cs`
- Create: `ClumsyPilot/tests/verify_planning_map_image.ps1`

- [ ] 写新图片测试：输入只允许 `PlanningGridMap`；覆盖关闭导出、非法尺寸、唯一命名、临时文件清理、PNG 签名/IHDR/IEND 和每个 chunk CRC。
- [ ] 保留现有有效限制：`PixelsPerCell=4`、`MaximumImageEdgePixels=4000`、`MaximumFileSizeBytes=50 MiB`、`OutputDpi=300`、最多 1024 次重名重试、StbImageWriteSharp 1.16.7。
- [ ] 按职责迁移：Request/Result 只放 DTO；Renderer 生成 RGBA；Writer 负责编码/CRC；Exporter 校验、独占临时文件和原子发布。
- [ ] 新导出器不得依赖 `GridMapData`、`TrapMapVehiclePose`、TwoLeg 状态、传感器或地图构造器；车辆、起终点和路径只作可选叠加层。
- [ ] 运行图片脚本通过后先保留旧文件，Task 16 确认等价覆盖后再退役。

### Task 14：增加地图与粗路径 MovementTest

**文件：**

- Create: `Map/Test/MovementTest.MapTest.cs`
- Create: `CoarsePath/Test/CoarsePathScenarioFactory.cs`
- Create: `CoarsePath/Test/MovementTest.CoarsePathTest.cs`
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`

- [ ] **P0-MAP 部分：** 先创建 `MovementTest.MapTest.cs`。它只调用长期持有的 `PlanningMapFactory`，显示来源状态、栅格、快照 ID 和缓存命中，并可选调用新 PNG 导出器；通过 Map Gate 后即可结束当前首要里程碑。
- [ ] **P1 部分：** Map Gate 和 P0-PLAN 均通过后，再创建 `CoarsePathScenarioFactory.cs` 与 `MovementTest.CoarsePathTest.cs`。
- [ ] 场景工厂提供显式空图、单矩形绕行、人工圆+矩形+TwoLeg、相同地图缓存命中、倒车换向和无解案例。
- [ ] CoarsePath 案例使用同一 `CoarsePathPlanningService`；测试类不得直接实例化 rasterizer、碰撞器、原语生成器或搜索节点。
- [ ] `MovementTest.CoarsePathTest` 在后台任务调用同步 `Plan`，绘制起点、目标、路径、换向点和扩大车体检查点。
- [ ] `TestStop` 先取消专用 `CancellationTokenSource`，再清理任务和 Painter；两个入口不得发送底盘运动命令。
- [ ] Clumsy UI 手动运行时不阻塞界面，停止后无后台规划残留，调试开关不改变结果。

### Task 15（P1）：性能、资源和确定性验收

**文件：**

- Create: `ClumsyPilot/tests/benchmark_coarse_path.ps1`
- Modify: `Map/Planning/PlanningMapCache.cs`
- Modify: `Map/Planning/EuclideanDistanceTransform.cs`
- Modify: `CoarsePath/Search/BinaryMinHeap.cs`
- Modify: `CoarsePath/Search/HybridAStarSearch.cs`
- Modify: `CoarsePath/Contracts/PlanningDiagnostics.cs`

- [ ] 基准脚本输出地图规模、缓存命中、状态、耗时、扩展/生成/重开节点、陈旧堆条目、Open List 峰值和托管内存增量，超限返回非零。
- [ ] 地图参考：20 m×20 m、0.05 m、160,000 格、100 障碍；20 次后完整构建 P95≤200 ms，完整缓存命中 P95≤5 ms。
- [ ] 地图极限：4,000,000 格、100 障碍；3 s 内成功或明确失败；成功时内存增量≤160 MB，无溢出和部分快照。
- [ ] 规划参考：12 m×8 m、0.05 m、矩形阻断直线、距离≥8 m；20 次 P95≤2 s，内存增量≤256 MB。
- [ ] 规划压力：20 m×20 m、0.05 m；成功或无解均在 5 s、200,000 节点和 512 MB 增量内返回。
- [ ] 优化只针对查询分配、重复 EDT、堆扩容和稠密点保存；不得降低碰撞保守性、跳过最终复核或放宽失败状态。
- [ ] 运行：

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj -c Release --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\benchmark_coarse_path.ps1 -Configuration Release
```

预期：全部指标达标，脚本退出码为 0。

### Task 16（P1）：退役旧地图入口并完成文档

**文件：**

- Modify/Delete after equivalent coverage: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/MovementTest.Trapmaptest.cs`
- Delete after equivalent coverage: `ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs`
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_grid.ps1`
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_inputs.ps1`
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_lifecycle.ps1`
- Update/Delete after equivalent coverage: `ClumsyPilot/tests/verify_trapmap_image.ps1`
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`

- [ ] 建立覆盖表：`TrapMapBounds→MapBoundsMm`、`GridMapData→EnvironmentGridMap/PlanningGridMap`、`TrapMapLayerComposer→EnvironmentMapBuilder`、`TrapMapBuilder.Get→上层采集+CoarsePathPlanningService`、旧 exporter→五个 Visualization 文件。
- [ ] 等价脚本全部通过后再删除旧实现；不保留车辆写图、默认 300 mm 膨胀或地图构建器直接调用 `TwoLegDetect` 的兼容开关。
- [ ] README 写明一次调用、下层门面、mm/m-rad 边界、快照变化判断、调试开关不参与指纹、P0/P1 命令和非目标。
- [ ] 执行旧引用搜索：

```powershell
rg -n "GridMapData|TrapMapBuilder|TrapMapLayerComposer|TrapMapImageExporter|MarkVehicleFootprint" .\ClumsyPilot
```

预期：仅迁移说明或历史文档可命中；运行时代码和新测试不得命中旧类型。

- [ ] 运行最终回归：

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```

预期：Debug 构建成功，7 个功能脚本退出码均为 0；随后重跑 Task 15 Release 基准并通过。

## 最终验收清单

- [ ] `CoarsePathPlanningService.Plan(job)` 是推荐的一次调用入口。
- [ ] `PlanningMapFactory` 和 `HybridAStarPlanner` 仅作为可独立测试的下层门面。
- [ ] 人工、TwoLeg 和未来障碍通过同一个 `IMapObstacleSource` 进入唯一栅格化器。
- [ ] 地图未变化时复用完整快照或占据/距离缓冲；可视化、起终点和车辆参数不参与地图变化判断。
- [ ] 地图不包含车辆自身和安全膨胀，碰撞检查使用扩大车辆矩形。
- [ ] 距离场是净空下界，不能因高估而跳过精确碰撞。
- [ ] 0.50 m 原语可在任意内部采样点截断；终点候选按 Open List 顺序出队后才终止。
- [ ] best-G、重开、陈旧条目、候选保护和堆排序均有自动化测试。
- [ ] 输出包含稠密点、保守净空、实际曲率、换向点和完整方向分段。
- [ ] PNG 和 MovementTest 只消费只读快照，不成为核心规划依赖。
- [ ] 失败、取消、超时和限额均返回空路径及明确状态，不发布部分结果。
- [ ] Debug 与 Release 验收全部通过，且未执行 Git 自检或提交。