docs: plan hybrid astar coarse path modules

ClumsyPilot/ParkrobTrajplanner/Initial_plan/AMR_HybridAStar_粗路径实施计划.md

@@ -0,0 +1,347 @@
+# Hybrid A* 粗路径规划实施计划
+
+> For agentic workers: implement task-by-task with checkbox tracking.
+
+**目标：** 完成可独立调用的规划地图入口与 Hybrid A* 粗路径输出。所有核心功能和自动化测试通过后，最后编写 MovementTest。
+
+**架构：** Map 的唯一公共调用类是 PlanningMapFactory；CoarsePath 的唯一公共调用类是 HybridAStarPlanner。Map 输出只读 PlanningGridMap，Planner 只接收该输入。
+
+**技术栈：** C# 10、.NET Standard 2.0、现有 Clumsy 引用、PowerShell 反射测试、Clumsy MovementTest。
+
+## 全局约束
+
+- 外部地图与障碍物使用现有世界坐标 mm；规划内部使用 m、rad、1/m。
+- 环境占据图只保存外部障碍物；车体尺寸与安全余量只属于碰撞检查。
+- 支持中心放置的轴对齐矩形、圆形和可选 TwoLegDetect 投影；障碍物不膨胀。
+- 第一版终点使用可配置位置/航向容差；不含 Reeds-Shepp、平滑、SQP、时间轨迹与控制。
+- 每个公共接口必须有成功、边界、失败测试；MovementTest 是最后一项任务。
+
+---
+
+## 最终目录与职责
+
+~~~text
+ClumsyPilot/ParkrobTrajplanner/
+├── Initial_plan/
+│   ├── AMR_轨迹规划技术路线_Agent版.md
+│   └── AMR_HybridAStar_粗路径实施计划.md
+├── Utils/                                      # MultiWheelC.TrajectoryPlanning.Utils
+│   ├── AngleMath.cs                            # 角度归一化、角差、航向索引
+│   ├── UnitConverter.cs                        # mm/m、deg/rad、半径/曲率
+│   ├── CoordinateTransform.cs                  # 世界系与车体系二维变换
+│   ├── NumericGuard.cs                         # 数值合法性校验
+│   └── GridIndex.cs                            # 行列索引值对象
+├── Map/                                        # MultiWheelC.TrajectoryPlanning.Mapping
+│   ├── Core/
+│   │   ├── EnvironmentGridMap.cs               # 仅外部障碍物的 mm 栅格
+│   │   ├── MapBuildRequest.cs                  # 环境图构造参数
+│   │   ├── EnvironmentMapBuildResult.cs        # 环境图构造结果
+│   │   └── EnvironmentMapBuilder.cs            # 内部构造器
+│   ├── Obstacles/
+│   │   ├── IMapObstacle.cs                     # 障碍物几何契约
+│   │   ├── AxisAlignedRectangleObstacle.cs     # 中心、宽、高矩形
+│   │   ├── CircleObstacle.cs                   # 中心、半径圆形
+│   │   └── MapObstacleRasterizer.cs            # 唯一占据写入器
+│   ├── Sources/
+│   │   ├── ManualObstacleSource.cs             # 人工障碍物集合
+│   │   └── TwoLegObstacleProjector.cs          # TwoLegDetect 投影
+│   ├── Planning/
+│   │   ├── PlanningGridMap.cs                  # 只读 m 单位规划地图
+│   │   ├── ObstacleDistanceField.cs            # 最近障碍距离
+│   │   └── PlanningMapAdapter.cs               # mm 地图适配
+│   ├── PlanningMapRequest.cs                   # Map 统一输入
+│   ├── PlanningMapBuildResult.cs                # Map 统一输出
+│   ├── PlanningMapFactory.cs                   # Map 唯一公共调用类
+│   └── Test/MovementTest.MapTest.cs             # 最后阶段的地图测试入口
+├── CoarsePath/                                 # MultiWheelC.TrajectoryPlanning.CoarsePath
+│   ├── Contracts/
+│   │   ├── Pose2D.cs                           # m/rad 位姿
+│   │   ├── VehicleParameters.cs                # 尺寸、曲率、安全余量
+│   │   ├── PlanningRequest.cs                  # Planner 输入
+│   │   ├── HybridAStarConfiguration.cs         # 可调参数
+│   │   ├── PlanningResult.cs                   # Planner 输出
+│   │   ├── PlanningStatus.cs                   # 成功/失败状态
+│   │   ├── PlanningDiagnostics.cs              # 诊断统计
+│   │   ├── CoarsePathPoint.cs                  # 稠密空间路径点
+│   │   └── PathSegment.cs                      # 前进/倒车分段
+│   ├── Vehicle/
+│   │   ├── VehicleKinematics.cs                # 最大曲率
+│   │   ├── HeadingFootprintTemplate.cs         # 单航向车体模板
+│   │   ├── HeadingFootprintTemplateCache.cs    # 模板预计算
+│   │   └── FootprintCollisionChecker.cs        # 碰撞检查
+│   ├── Search/
+│   │   ├── MotionPrimitive.cs                  # 恒曲率原语
+│   │   ├── MotionPrimitiveGenerator.cs         # 原语积分
+│   │   ├── HybridAStarNode.cs                  # 搜索节点
+│   │   ├── HybridAStarNodeKey.cs               # Closed Set 键
+│   │   ├── GoalToleranceChecker.cs             # 容差终点
+│   │   ├── GridDijkstraHeuristic.cs            # 2D 启发
+│   │   └── HybridAStarSearch.cs                # 搜索管理
+│   ├── Output/
+│   │   ├── PathBacktracker.cs                  # 恢复内部积分点
+│   │   ├── CoarsePathAssembler.cs              # 弧长、换向、分段
+│   │   └── CoarsePathValidator.cs              # 最终复核
+│   ├── HybridAStarPlanner.cs                   # CoarsePath 唯一公共调用类
+│   └── Test/
+│       ├── CoarsePathScenarioFactory.cs        # 可复现场景
+│       └── MovementTest.CoarsePathTest.cs      # 最后阶段的规划测试入口
+└── README.md                                   # 两个门面类的调用说明
+
+ClumsyPilot/tests/
+├── verify_planning_utils.ps1
+├── verify_planning_map_factory.ps1
+├── verify_planning_map_adapter.ps1
+├── verify_coarse_path_search.ps1
+└── verify_coarse_path_integration.ps1
+~~~
+
+## 唯一调用方式
+
+~~~csharp
+var mapResult = new PlanningMapFactory().Create(new PlanningMapRequest
+{
+    ResolutionMm = 50f,
+    ManualObstacles = manualObstacles,
+    TwoLegInput = optionalTwoLegInput,
+    AllowExplicitEmptyMap = true,
+});
+
+if (!mapResult.Succeeded)
+    return PlanningResult.FromMapFailure(mapResult);
+
+return new HybridAStarPlanner().Plan(new PlanningRequest
+{
+    Map = mapResult.Map,
+    Start = startPose,
+    Goal = goalPose,
+    Vehicle = vehicle,
+    Configuration = configuration,
+});
+~~~
+
+调用方不得直接创建 EnvironmentMapBuilder、MapObstacleRasterizer、PlanningMapAdapter、碰撞检查器、原语生成器或搜索节点。
+
+---
+
+### Task 1：建立 Utils 与工具测试
+
+**文件：**
+- Create：Utils 下的 AngleMath.cs、UnitConverter.cs、CoordinateTransform.cs、NumericGuard.cs、GridIndex.cs
+- Test：ClumsyPilot/tests/verify_planning_utils.ps1
+
+**输出：** NormalizeRadians、ShortestSignedAngleDifference、MillimetersToMeters、DegreesToRadians、TransformLocalToWorld 和 GridIndex。
+
+- [ ] 写失败测试：
+
+~~~powershell
+Assert-Equal 1.25 ([UnitConverter]::MillimetersToMeters([single]1250))
+Assert-Near 0.0 ([AngleMath]::NormalizeRadians(6.283185307179586)) 1e-12
+~~~
+
+- [ ] 运行 verify_planning_utils.ps1；预期 FAIL，类型不存在。
+- [ ] 实现 UnitConverter：
+
+~~~csharp
+public static double MillimetersToMeters(float millimeters) => millimeters / 1000d;
+public static double DegreesToRadians(float degrees) => degrees * Math.PI / 180d;
+~~~
+
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add Utils 与 verify_planning_utils.ps1；git commit -m "feat: add planning utilities"。
+
+### Task 2：定义 CoarsePath 契约与可调容差
+
+**文件：**
+- Create：CoarsePath/Contracts 下的 Pose2D、VehicleParameters、PlanningRequest、HybridAStarConfiguration、PlanningResult、PlanningStatus、PlanningDiagnostics、CoarsePathPoint、PathSegment。
+- Test：verify_planning_utils.ps1
+
+**输出：** 默认原语 0.50 m、积分 0.05 m、航向 5°、位置容差 0.15 m、航向容差 5°。
+
+- [ ] 写失败测试，断言上述配置默认值和 Success、InvalidMap、StartInCollision、GoalInCollision、NoFeasiblePath 状态。
+- [ ] 运行脚本；预期 FAIL，配置类型不存在。
+- [ ] 实现不可变请求/结果契约；结果不得包含速度、加速度或时间。
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add CoarsePath/Contracts 与 verify_planning_utils.ps1；git commit -m "feat: add coarse path contracts"。
+
+### Task 3：实现外部障碍环境地图
+
+**文件：**
+- Create：Map/Core/EnvironmentGridMap.cs
+- Create：Map/Obstacles 下的 IMapObstacle、AxisAlignedRectangleObstacle、CircleObstacle、MapObstacleRasterizer。
+- Create：Map/Sources/ManualObstacleSource.cs
+- Test：verify_planning_map_factory.ps1
+
+**输出：** 矩形按中心/宽/高，圆按中心/半径栅格化；环境地图没有车辆足迹接口。
+
+- [ ] 写失败测试，分别验证矩形中心、圆心、边界外占据和不存在 MarkVehicleFootprint。
+- [ ] 运行脚本；预期 FAIL，地图和障碍物类型不存在。
+- [ ] 实现轴对齐矩形与圆-格矩形相交栅格化。
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add Map/Core Map/Obstacles Map/Sources/ManualObstacleSource.cs 和测试；git commit -m "feat: add external obstacle map"。
+
+### Task 4：实现 TwoLeg 图层和 PlanningMapFactory
+
+**文件：**
+- Create：Map/Sources/TwoLegObstacleProjector.cs
+- Create：Map/Core 下的 MapBuildRequest、EnvironmentMapBuildResult、EnvironmentMapBuilder。
+- Create：Map 下的 PlanningMapRequest、PlanningMapBuildResult、PlanningMapFactory。
+- Test：verify_planning_map_factory.ps1
+
+**输出：** new PlanningMapFactory().Create(request) 是 Map 唯一公共入口；TwoLeg 失败不能删除人工地图。
+
+- [ ] 写失败测试，验证人工地图成功时 TwoLegApplied 为 false 仍然成功。
+- [ ] 运行脚本；预期 FAIL，PlanningMapFactory 不存在。
+- [ ] 实现工厂：合并人工障碍与可选投影；TwoLegObstacleProjector 只产出 CircleObstacle。
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add Map 与 verify_planning_map_factory.ps1；git commit -m "feat: add planning map factory"。
+
+### Task 5：适配 PlanningGridMap 并生成距离场
+
+**文件：**
+- Create：Map/Planning 下的 PlanningGridMap、ObstacleDistanceField、PlanningMapAdapter。
+- Modify：Map/PlanningMapFactory.cs
+- Test：verify_planning_map_adapter.ps1
+
+**输出：** 深拷贝占据格，mm 转 m，越界占据，生成最近障碍距离场。
+
+- [ ] 写失败测试，断言 ResolutionMeters=0.05、障碍坐标转换、越界和障碍格距离为 0。
+- [ ] 运行脚本；预期 FAIL，适配类型不存在。
+- [ ] 实现 PlanningMapAdapter.Adapt(EnvironmentGridMap) 和多源距离场；显式空旷图使用正无穷距离。
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add Map/Planning Map/PlanningMapFactory.cs 和测试；git commit -m "feat: adapt planning map"。
+
+### Task 6：实现车辆足迹与碰撞检查
+
+**文件：**
+- Create：CoarsePath/Vehicle 下的 VehicleKinematics、HeadingFootprintTemplate、HeadingFootprintTemplateCache、FootprintCollisionChecker。
+- Test：verify_coarse_path_search.ps1
+
+**输出：** IsCollisionFree(Pose2D, PlanningGridMap, VehicleParameters, out clearanceMeters)。
+
+- [ ] 写失败测试，覆盖占据碰撞、45° 安全、越界碰撞和不同航向。
+- [ ] 运行脚本；预期 FAIL，碰撞类型不存在。
+- [ ] 实现扩大车体模板；距离场仅在大于外接圆时放行，否则查询航向模板。
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add CoarsePath/Vehicle 和测试；git commit -m "feat: add footprint collision checking"。
+
+### Task 7：生成恒曲率运动原语
+
+**文件：**
+- Create：CoarsePath/Search/MotionPrimitive.cs、MotionPrimitiveGenerator.cs
+- Test：verify_coarse_path_search.ps1
+
+**输出：** 五级曲率、前进/倒车、最多 0.50 m、每步不超过 0.05 m、保留内部点。
+
+- [ ] 写失败测试，覆盖直行 10 点、曲线航向变化、倒车和曲率跳变拒绝。
+- [ ] 运行脚本；预期 FAIL，原语生成器不存在。
+- [ ] 实现积分：
+
+~~~csharp
+x += directionSign * Math.Cos(theta) * stepMeters;
+y += directionSign * Math.Sin(theta) * stepMeters;
+theta = AngleMath.NormalizeRadians(theta + directionSign * curvature * stepMeters);
+~~~
+
+- [ ] 构建并重跑脚本；预期 PASS，所有内部点经过碰撞检查。
+- [ ] 提交：git add 两个原语文件和测试；git commit -m "feat: add motion primitives"。
+
+### Task 8：实现目标容差、启发和 Hybrid A* 搜索
+
+**文件：**
+- Create：CoarsePath/Search 下的 HybridAStarNode、HybridAStarNodeKey、GoalToleranceChecker、GridDijkstraHeuristic、HybridAStarSearch。
+- Test：verify_coarse_path_search.ps1
+
+**输出：** Closed Set 键为位置格、航向格、方向、曲率索引；终点读取可调容差。
+
+- [ ] 写失败测试，无障碍前进、单障碍绕行、一次倒车、容差成功、无解和节点上限。
+- [ ] 运行脚本；预期 FAIL，搜索类型不存在。
+- [ ] 实现搜索循环：
+
+~~~csharp
+while (openList.Count > 0 && diagnostics.ExpandedNodeCount < configuration.MaxExpandedNodes)
+{
+    var current = PopBestValidNode();
+    if (_goalChecker.IsReached(current.Pose, request.Goal, configuration, current.Direction))
+        return SearchResult.Succeeded(current.NodeIndex);
+    ExpandValidPrimitives(current);
+}
+~~~
+
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add CoarsePath/Search 和测试；git commit -m "feat: add hybrid astar search"。
+
+### Task 9：回溯、输出和 HybridAStarPlanner 门面
+
+**文件：**
+- Create：CoarsePath/Output 下的 PathBacktracker、CoarsePathAssembler、CoarsePathValidator。
+- Create：CoarsePath/HybridAStarPlanner.cs
+- Test：verify_coarse_path_integration.ps1
+
+**输出：** new HybridAStarPlanner().Plan(request) 返回稠密路径、换向标记、分段、诊断和状态。
+
+- [ ] 写失败测试，验证首点弧长为 0、内部点连续、换向标记、终点容差和最终复核。
+- [ ] 运行脚本；预期 FAIL，门面不存在。
+- [ ] 实现编排：
+
+~~~csharp
+var search = _search.Run(request);
+if (!search.Succeeded) return PlanningResult.Failed(search.Status, search.Diagnostics);
+var points = _assembler.Assemble(_backtracker.Backtrack(search));
+return _validator.Validate(points, request)
+    ? PlanningResult.Succeeded(points, search.Diagnostics)
+    : PlanningResult.Failed(PlanningStatus.FinalValidationFailed, search.Diagnostics);
+~~~
+
+- [ ] 构建并重跑脚本；预期 PASS。
+- [ ] 提交：git add CoarsePath/Output CoarsePath/HybridAStarPlanner.cs 和测试；git commit -m "feat: expose coarse path planner"。
+
+### Task 10：完成非 UI 回归与调用说明
+
+**文件：**
+- Create：ParkrobTrajplanner/README.md
+- Modify：全部 verify_planning 脚本。
+
+- [ ] 写失败测试，断言 PlanningMapFactory 和 HybridAStarPlanner 是唯一由外部测试实例化的模块门面。
+- [ ] 运行全部脚本；预期在 README、状态或门面断言未齐全时 FAIL。
+- [ ] 在 README 写入 Map 到 Planner 的调用代码、mm/m-rad 边界和非目标。
+- [ ] 构建、运行全部规划脚本并运行 git diff --check；预期全部 PASS。
+- [ ] 提交：git add README 和 tests；git commit -m "test: complete coarse path regression coverage"。
+
+### Task 11：最后编写 MovementTest 集成模块
+
+**文件：**
+- Create：Map/Test/MovementTest.MapTest.cs
+- Create：CoarsePath/Test/CoarsePathScenarioFactory.cs
+- Create：CoarsePath/Test/MovementTest.CoarsePathTest.cs
+- Test：Clumsy UI 手动运行两个 MovementTest，然后重跑 Task 10 自动化回归。
+
+**输出：** MapTest 只调用 PlanningMapFactory；CoarsePathTest 只调用两个门面；两者绝不发送底盘命令。
+
+- [ ] 写失败测试，场景工厂生成的地图请求可由 PlanningMapFactory 构建，规划请求可由 HybridAStarPlanner 成功规划。
+- [ ] 运行 verify_coarse_path_integration.ps1；预期 FAIL，场景工厂不存在。
+- [ ] 实现入口：
+
+~~~csharp
+[MovementTest(name = "Hybrid A* 粗路径测试")]
+public sealed class CoarsePathTest : MovementTest
+{
+    public override void Test()
+    {
+        var map = new PlanningMapFactory().Create(CoarsePathScenarioFactory.CreateSingleObstacleMapRequest());
+        var result = map.Succeeded
+            ? new HybridAStarPlanner().Plan(CoarsePathScenarioFactory.CreatePlanningRequest(map.Map))
+            : PlanningResult.FromMapFailure(map);
+        DrawMapAndPath(map, result);
+    }
+}
+~~~
+
+- [ ] MapTest 绘制人工矩形、人工圆、TwoLeg 投影和边界；CoarsePathTest 绘制起点、目标、路径、换向点和失败状态。TestStop 只清理 Painter/任务。
+- [ ] 构建、运行全部 PowerShell 脚本，并在 Clumsy UI 手动运行“规划地图测试”和“Hybrid A* 粗路径测试”；预期只显示调试结果，不发送运动命令。
+- [ ] 提交：git add Map/Test CoarsePath/Test 和 tests；git commit -m "feat: add coarse path movement tests"。
+
+## 实施前检查
+
+- PlanningMapFactory 与 HybridAStarPlanner 是未来调用方和测试入口唯一允许直接实例化的模块类。
+- MovementTest 位于最后一个任务；它之前所有 Map 与搜索逻辑必须已完成非 UI 自动化验证。
+

docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md

@@ -47,6 +47,9 @@ ClumsyPilot/ParkrobTrajplanner/
 │   │   ├── PlanningGridMap.cs
 │   │   ├── ObstacleDistanceField.cs
 │   │   └── PlanningMapAdapter.cs
+│   ├── PlanningMapRequest.cs
+│   ├── PlanningMapBuildResult.cs
+│   └── PlanningMapFactory.cs
 │   └── Test/
 │       └── MovementTest.MapTest.cs
 ├── CoarsePath/                           # MultiWheelC.TrajectoryPlanning.CoarsePath
@@ -105,6 +108,36 @@ ClumsyPilot/tests/
 
 ## Map 模块
 
+### 对外调用门面
+
+粗路径模块不直接实例化 `EnvironmentMapBuilder`、`MapObstacleRasterizer`、`TwoLegObstacleProjector` 或 `PlanningMapAdapter`。`PlanningMapFactory` 是 `Map` 模块唯一的公共创建入口：
+
+```csharp
+public sealed class PlanningMapFactory
+{
+    public PlanningMapBuildResult Create(PlanningMapRequest request);
+}
+```
+
+粗路径调用只依赖其稳定输出：
+
+```csharp
+var mapResult = new PlanningMapFactory().Create(mapRequest);
+if (!mapResult.Succeeded)
+    return PlanningResult.FromMapFailure(mapResult);
+
+var result = new HybridAStarPlanner().Plan(new PlanningRequest
+{
+    Map = mapResult.Map,
+    Start = startPose,
+    Goal = goalPose,
+    Vehicle = vehicle,
+    Configuration = configuration,
+});
+```
+
+`PlanningMapRequest` 集中地图边界/分辨率、人工障碍物、可选 `TwoLegDetect` 投影输入和空旷地图声明；`PlanningMapBuildResult` 返回 `Succeeded`、失败原因、每个障碍物来源的摘要，以及成功时不可变的 `PlanningGridMap`。因此 A* 的 `PlanningRequest.Map` 始终是已完成校验、mm→m 适配、距离场生成的输入，规划器不需要了解地图构造细节。
+
 ### 环境地图与图层
 
 `EnvironmentGridMap` 保存 mm 单位的地图边界、分辨率和仅含外部障碍物的占据单元。它不接受 `MarkVehicleFootprint` 一类接口。