chore: save current workspace progress

This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,369 @@
# 规划操作预算与诊断收尾实施计划
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
**Goal:** 让一次粗规划调用在建图、距离场、Dijkstra 和 Hybrid A* 阶段共用可取消的总超时预算,并发布真实的 Open List 诊断计数。
**Architecture:**`Utils` 新增内部 `PlanningOperationBudget`,它不依赖 Map 或 CoarsePath,只报告继续、取消、超时。Map 与搜索分别将该中立结果映射为自己的结果;`CoarsePathPlanningService` 创建唯一预算并传递给下层。原有公开的无预算入口保持兼容,门面使用内部带预算入口。
**Tech Stack:** C# 10、.NET Standard 2.0、PowerShell 反射回归脚本、`Stopwatch``CancellationToken`
## Global Constraints
- 位置使用 m、地图输入使用 mm、航向使用 rad、曲率使用 1/m;不得改变现有单位边界。
- 取消或超时必须返回空路径和空方向分段,绝不发布部分路径或部分 `PlanningGridMap`
- 地图不得依赖 CoarsePath;共享预算只能放在 `ParkrobTrajplanner/Utils`
- 每 256 个或更少循环工作单元检查一次预算;外部 `IMapObstacleSource.ProjectToWorld()` 是调用方提供的同步快照接口,只能在调用前后检查,不能强制抢占其内部执行。
- 不改变碰撞保守性、运动原语、代价公式、目标候选保护或 Open List 排序。
- 保留 `PlanningMapFactory.Create(request)``HybridAStarPlanner.Plan(request, token)``HybridAStarSearch.Search(request, token)``GridDijkstraHeuristic(map,row,col)` 的兼容入口。
- 不执行 Git 添加、提交、重置或工作区清理。
---
## 文件结构
| 文件 | 职责 |
| --- | --- |
| `Utils/PlanningOperationBudget.cs` | 内部单调计时、取消检查和统一停止原因。 |
| `Map/PlanningMapBuildResult.cs` | 地图构建状态:成功、失败、取消、超时。 |
| `Map/Core/EnvironmentMapBuilder.cs``EnvironmentMapBuildResult.cs` | 将预算传入来源处理与栅格化,并保留停止原因。 |
| `Map/Obstacles/MapObstacleRasterizer.cs` | 在圆形/矩形逐格写入期间定期停止。 |
| `Map/Planning/{PlanningMapAdapter,ObstacleDistanceField,EuclideanDistanceTransform}.cs` | 在占据复制、EDT 和距离换算期间定期停止且不产出快照。 |
| `Map/PlanningMapFactory.cs` | 可取消地等待创建锁、检查缓存、建图、哈希和写缓存。 |
| `CoarsePath/Search/{GridDijkstraHeuristic,HybridAStarSearch}.cs` | 为 Dijkstra 和 Open List 使用共享预算,记录陈旧条目和峰值。 |
| `CoarsePath/{HybridAStarPlanner,Contracts/PlanningDiagnostics}.cs` | 将搜索统计传给最终结果,并让公开 Planner 包装兼容预算。 |
| `CoarsePath/Facade/CoarsePathPlanningService.cs` | 创建一次调用唯一预算,映射地图阶段停止状态。 |
| `Map/README.md``CoarsePath/README.md` | 分别说明地图构建状态,以及粗规划总超时和取消状态。 |
| `tests/verify_planning_map_factory.ps1``verify_planning_map_adapter.ps1``verify_coarse_path_search.ps1``verify_coarse_path_integration.ps1` | 预算与诊断的反射回归覆盖。 |
### Task 1: 建立独立的操作预算与地图终止契约
**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/Utils/PlanningOperationBudget.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/PlanningMapBuildResult.cs`
- Modify: `ClumsyPilot/tests/verify_planning_map_factory.ps1`
**Consumes:** `System.Diagnostics.Stopwatch``System.Threading.CancellationToken`
**Produces:** `PlanningOperationStopReason``PlanningOperationBudget``PlanningMapBuildStatus`;下游 Map/CoarsePath 均只通过这些类型传递停止信息。
- [ ] **Step 1: 写失败测试,锁定新的地图状态与预算公开反射形状。**
在现存的 `verify_planning_map_factory.ps1` 断言存在内部 `MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget` 与三值 `PlanningOperationStopReason`,并断言 `PlanningMapBuildResult.Status` 存在;已取消、已超时结果的 `Succeeded` 必须为 `false``Map``$null``CacheHit``None`
```powershell
$statusType = $assembly.GetType($ns + 'PlanningMapBuildStatus', $true)
Assert-True ($statusType.GetEnumNames() -contains 'Cancelled') 'Map build status must expose cancellation.'
Assert-True ($statusType.GetEnumNames() -contains 'TimedOut') 'Map build status must expose timeout.'
Assert-True ($resultType.GetProperty('Status') -ne $null) 'Map result must expose an explicit status.'
```
- [ ] **Step 2: 运行两个脚本,确认因类型或属性不存在而失败。**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
```
Expected: 断言报告 `PlanningOperationBudget` 或 `PlanningMapBuildStatus` 缺失。
- [ ] **Step 3: 实现最小共享预算和显式地图状态。**
`PlanningOperationBudget` 的核心接口固定如下;超时使用构造时启动的单调 `Stopwatch`,取消优先于超时:
```csharp
internal enum PlanningOperationStopReason { None, Cancelled, TimedOut }
internal sealed class PlanningOperationBudget
{
internal PlanningOperationBudget(CancellationToken cancellationToken, TimeSpan timeout);
internal static PlanningOperationBudget Unlimited(CancellationToken cancellationToken);
internal TimeSpan Elapsed { get; }
internal PlanningOperationStopReason GetStopReason();
internal PlanningOperationStopReason CheckEvery(ref int workItemCount);
}
```
`CheckEvery` 在第一次工作单元以及每 256 个工作单元检查;`Unlimited` 不启用时间限制但仍响应取消。将地图结果从布尔构造改为状态构造:
```csharp
public enum PlanningMapBuildStatus { Success, Failed, Cancelled, TimedOut }
public PlanningMapBuildStatus Status { get; }
public bool Succeeded { get { return Status == PlanningMapBuildStatus.Success; } }
internal static PlanningMapBuildResult Stopped(PlanningOperationStopReason reason,
IReadOnlyList<ObstacleProjectionResult> sourceResults)
{
return new PlanningMapBuildResult(
reason == PlanningOperationStopReason.Cancelled
? PlanningMapBuildStatus.Cancelled
: PlanningMapBuildStatus.TimedOut,
reason == PlanningOperationStopReason.Cancelled ? "地图创建已取消。" : "地图创建已超时。",
sourceResults, PlanningMapCacheHit.None, null);
}
```
- [ ] **Step 4: 重跑两个脚本,确认新契约通过且旧地图缓存断言未回归。**
Run: 与 Step 2 相同。
Expected: 地图工厂脚本输出 `Planning map factory checks passed.`。
### Task 2: 让地图创建、栅格化和距离场遵守预算
**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Core/EnvironmentMapBuildResult.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Core/EnvironmentMapBuilder.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Obstacles/MapObstacleRasterizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Planning/PlanningMapAdapter.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Planning/ObstacleDistanceField.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/Planning/EuclideanDistanceTransform.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/PlanningMapFactory.cs`
- Modify: `ClumsyPilot/tests/verify_planning_map_factory.ps1`
- Modify: `ClumsyPilot/tests/verify_planning_map_adapter.ps1`
**Consumes:** Task 1 的 `PlanningOperationBudget` 与 `PlanningOperationStopReason`。
**Produces:** `PlanningMapFactory` 的内部 `Create(PlanningMapRequest, PlanningOperationBudget)`;它在取消/超时时返回 `PlanningMapBuildResult.Stopped`,不会写入 LRU 缓存。
- [ ] **Step 1: 写失败测试,覆盖预先取消、EDT 中超时与缓存不污染。**
在工厂脚本创建一个已取消的 `CancellationTokenSource`,通过反射调用新的内部带预算 `Create`,断言:
```powershell
Assert-Equal 'Cancelled' $cancelledMapResult.Status.ToString() 'Cancelled map construction must report cancellation.'
Assert-False $cancelledMapResult.Succeeded 'Cancelled map construction must not succeed.'
Assert-Null $cancelledMapResult.Map 'Cancelled map construction must not publish a map.'
Assert-Equal 'None' $cancelledMapResult.CacheHit.ToString() 'Stopped construction must not publish a cache hit.'
```
在适配器脚本为含障碍的大栅格创建 `PlanningOperationBudget`,使用零超时调用内部 `TryCreate`,断言返回 `TimedOut` 且输出 `PlanningGridMap` 为 `$null`。随后用无预算入口再次创建相同地图,断言距离场仍可用,以证明停止时没有污染输入或缓存。
- [ ] **Step 2: 运行地图工厂和适配器脚本,确认新增反射入口缺失而失败。**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
```
Expected: 新带预算 `Create` 或 `TryCreate` 反射查找失败。
- [ ] **Step 3: 在地图管线的全部长循环中传递并检查预算。**
实现以下内部接口,所有 `Try*` 在停止时返回 `false` 并把 `stopReason` 设为非 `None`;普通参数错误仍按现有失败原因或异常处理。
```csharp
internal EnvironmentMapBuildResult Build(MapBuildRequest request, PlanningOperationBudget budget);
internal static bool TryRasterize(EnvironmentGridMap map, IMapObstacle obstacle,
PlanningOperationBudget budget, out PlanningOperationStopReason stopReason);
internal static bool TryCreate(EnvironmentGridMap environmentMap, PlanningOperationBudget budget,
out PlanningGridMap map, out PlanningOperationStopReason stopReason);
internal static bool TryCreate(byte[] occupied, int rows, int cols, double resolutionMeters,
PlanningOperationBudget budget, out ObstacleDistanceField field,
out PlanningOperationStopReason stopReason);
internal static bool TryComputeSquaredDistances(byte[] occupied, int rows, int cols,
PlanningOperationBudget budget, out double[] squared,
out PlanningOperationStopReason stopReason);
internal PlanningMapBuildResult Create(PlanningMapRequest request, PlanningOperationBudget budget);
```
保留现有公开 `Build`、`Rasterize`、`PlanningMapAdapter.Create`、`ObstacleDistanceField.Create` 与 `ComputeSquaredDistances`,让它们以 `PlanningOperationBudget.Unlimited(CancellationToken.None)` 包装新入口。对圆形/矩形逐格循环、EDT 两遍扫描、`Transform1D` 两个 `q` 循环、距离场扫描均调用 `budget.CheckEvery(ref workItemCount)`。
工厂以 `Monitor.TryEnter(_createGate, 16)` 循环等待创建锁;每次失败后检查预算。拿到锁后立刻再检查预算,随后在缓存读、来源处理、适配、占据哈希和每次缓存写入前检查。将 SHA-256 改为每 4096 字节调用 `TransformBlock` 的增量哈希,并在块间检查预算。任一非 `None` 停止原因直接返回 `PlanningMapBuildResult.Stopped`,且不会执行 `_cache.AddOccupancy` 或 `_cache.AddInput`。
- [ ] **Step 4: 重跑地图脚本,确认普通建图、两级缓存和新停止结果同时通过。**
Run: 与 Step 2 相同。
Expected: `Planning map factory checks passed.` 与 `Planning map adapter checks passed.`。
### Task 3: 为 Dijkstra 与 Hybrid A* 使用同一预算并记录 Open List 统计
**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GridDijkstraHeuristic.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs`
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
**Consumes:** Task 1 的预算和 Task 2 不可变地图;现有 `BinaryMinHeap<int>`。
**Produces:** `GridDijkstraHeuristic.TryCreate`、带共享预算的内部搜索/规划入口,以及 `HybridAStarSearchResult.StaleOpenListEntryCount` 和 `PeakOpenListCount`。
- [ ] **Step 1: 写失败测试,覆盖 Dijkstra 中取消、总超时和统计透传。**
在搜索脚本上创建至少 500×500 格的已就绪空地图,在地图创建完成后启动 `CancellationTokenSource.CancelAfter(1)` 并调用搜索。断言返回 `Cancelled`、`SuccessNodeIndex` 为 `$null`、运行时间小于 2 秒。再用同一地图和 `SearchTimeout = TimeSpan.Zero` 断言 `SearchTimeout`,以证明启发式创建前即尊重总预算。
反射断言搜索结果的新属性存在,并让直接路径搜索至少满足:
```powershell
Assert-True ($searchResultType.GetProperty('StaleOpenListEntryCount') -ne $null) 'Search result must expose stale Open List entries.'
Assert-True ($searchResultType.GetProperty('PeakOpenListCount') -ne $null) 'Search result must expose Open List peak size.'
Assert-True ($searchResult.PeakOpenListCount -ge 1) 'A successful search must retain at least one Open List entry.'
Assert-True ($searchResult.StaleOpenListEntryCount -ge 0) 'Stale Open List entry count must never be negative.'
```
在集成脚本断言 `PlanningResult.Diagnostics` 中的两项值与搜索结果一致;对产生重开/失效条目的固定障碍场景断言陈旧条目数大于零。
- [ ] **Step 2: 运行搜索和集成脚本,确认新增属性、带预算入口或中途取消断言失败。**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```
Expected: 新属性或 Dijkstra 中止行为缺失导致失败;现有“搜索前取消”检查不应被视为通过中途取消测试。
- [ ] **Step 3: 实现预算感知的 Dijkstra、搜索与真实诊断。**
`GridDijkstraHeuristic` 保留当前公开构造函数,并增加内部可失败构建:
```csharp
internal static bool TryCreate(PlanningGridMap map, int goalRow, int goalCol,
PlanningOperationBudget budget, out GridDijkstraHeuristic heuristic,
out PlanningOperationStopReason stopReason);
```
`Build` 的每次出堆与每 256 个邻居检查预算;停止时不返回部分启发式。`HybridAStarSearch.Search(request, token)` 从 `request.Configuration.SearchTimeout` 创建预算作为兼容包装,新增内部:
```csharp
internal HybridAStarSearchResult Search(PlanningRequest request, PlanningOperationBudget budget);
```
删除搜索器内部新建的 `Stopwatch` 与 `IsTimedOut`,所有原有取消/超时位置改为读取 `budget.GetStopReason()` 并精确映射为 `PlanningStatus.Cancelled` 或 `PlanningStatus.SearchTimeout`。Dijkstra 返回停止原因时立即返回对应搜索状态;节点上限仍只在预算检查之后、真正扩展之前检查。
扩展搜索结果构造函数与只读属性:
```csharp
public int StaleOpenListEntryCount { get; }
public int PeakOpenListCount { get; }
```
每次 `openList.Push` 后执行 `peakOpenListCount = Math.Max(peakOpenListCount, openList.Count)`。普通节点出堆后因 best-G 已更新、节点索引不匹配或已关闭而跳过时递增 `staleOpenListEntryCount`;目标候选的出队复核失败不算陈旧条目。所有 `CreateResult` 调用传递两项计数。
`HybridAStarPlanner` 的公开 `Plan` 保持签名并建立自己的预算;新增内部 `Plan(request, budget)` 供门面调用。诊断使用 `budget.Elapsed`,并把两个搜索统计填入原本为零的构造参数:
```csharp
searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
searchResult == null ? 0 : searchResult.PeakOpenListCount,
```
- [ ] **Step 4: 重跑搜索和集成脚本,确认取消、超时、目标候选与统计均通过。**
Run: 与 Step 2 相同。
Expected: `Coarse path search primitive checks passed.`、`Coarse path Hybrid A star search checks passed.`、`Coarse path integration checks passed.` 和 `Coarse path facade checks passed.`。
### Task 4: 让业务门面映射地图阶段终止状态并更新调用文档
**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningService.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/Map/README.md`
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
**Consumes:** Task 1 的地图状态、Task 2 的带预算工厂入口、Task 3 的内部 Planner 入口。
**Produces:** 一次 `CoarsePathPlanningService.Plan` 的统一预算和对调用方稳定的 `PlanningStatus` 映射。
- [ ] **Step 1: 写失败测试,锁定门面状态映射和空结果。**
在集成脚本使用已取消 Token 调用门面,断言:
```powershell
Assert-Equal 'Cancelled' $facadeResult.MapResult.Status.ToString() 'Facade must retain a cancelled map result.'
Assert-Equal 'Cancelled' $facadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage cancellation to planning cancellation.'
Assert-Equal 0 $facadeResult.PlanningResult.Path.Count 'Cancelled facade planning must publish no path.'
Assert-Equal 0 $facadeResult.PlanningResult.Segments.Count 'Cancelled facade planning must publish no segments.'
```
对 `SearchTimeout = TimeSpan.Zero` 的有效 job,断言地图结果和规划结果分别为 `TimedOut`、`SearchTimeout`,且调试 sink 不会把已停止操作改写为成功。
- [ ] **Step 2: 运行集成脚本,确认当前门面把地图阶段停止误报为 `InvalidMap` 或继续建图。**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```
Expected: 地图状态属性或正确的 `Cancelled`/`SearchTimeout` 映射不存在。
- [ ] **Step 3: 让门面创建并传递唯一预算,随后更新 README。**
门面从有效 `job.Configuration.SearchTimeout` 创建 `PlanningOperationBudget`;配置为空或时间值非法时使用无超时预算,让既有 Planner 预检继续返回原有无效配置状态。依次调用:
```csharp
PlanningMapBuildResult mapResult = _mapFactory.Create(job == null ? null : job.MapRequest, budget);
if (!mapResult.Succeeded)
{
PlanningStatus status = mapResult.Status == PlanningMapBuildStatus.Cancelled
? PlanningStatus.Cancelled
: mapResult.Status == PlanningMapBuildStatus.TimedOut
? PlanningStatus.SearchTimeout
: PlanningStatus.InvalidMap;
return PublishDebug(job, mapResult, PlanningResult.Failure(status, diagnostics));
}
PlanningResult planningResult = _planner.Plan(request, budget);
```
`Map/README.md` 在 `PlanningMapBuildResult` 的说明处增加 `Status``Success`、`Failed`、`Cancelled`、`TimedOut`;后两种不提供地图也不会进入缓存。`CoarsePath/README.md` 增加“总预算与取消”小节:`SearchTimeout` 是从门面开始的总预算,覆盖建图、距离场、Dijkstra 与 Hybrid A*`Cancelled`/`SearchTimeout` 一律无路径;不应以 `InvalidMap` 重试用户主动取消。
- [ ] **Step 4: 重跑集成脚本,确认门面状态映射、缓存复用和 debug 旁路隔离均通过。**
Run: 与 Step 2 相同。
Expected: `Coarse path integration checks passed.` 和 `Coarse path facade checks passed.`。
### Task 5: 全量回归与验收记录
**Files:**
- Modify only if a command reveals a concrete regression: the exact responsible source or test file from Tasks 14.
- [ ] **Step 1: 执行 Debug 构建。**
Run:
```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```
Expected: `0 个警告`、`0 个错误`。
- [ ] **Step 2: 执行全部现行 P0 地图与粗规划回归。**
Run:
```powershell
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_planning_map_documentation.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_test_config.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
```
Expected: 每个脚本退出码为 0 并输出 `passed`。
- [ ] **Step 3: 对照设计完成验收。**
逐项检查:预先取消和中途 Dijkstra 取消均返回 `Cancelled`;零总超时返回 `SearchTimeout`;地图停止不创建快照或缓存条目;正常输入的路径与缓存行为不变;诊断两项不再硬编码为零;README 已说明总预算语义。
## 自检
- 规格覆盖:Task 1 定义共享预算和显式地图状态;Task 2 覆盖地图、EDT、锁和缓存;Task 3 覆盖 Dijkstra、Hybrid A*、统计和 PlannerTask 4 覆盖门面映射与文档;Task 5 覆盖完整回归。
- 类型一致性:所有耗时组件仅接收 `PlanningOperationBudget` 并输出 `PlanningOperationStopReason`Map 使用 `PlanningMapBuildStatus`,粗规划使用既有 `PlanningStatus`。
- 范围:不触及 UI、Painter、场景工厂、Release 基准、运动模型或旧 TrapMap 脚本。