diff --git a/ClumsyPilot/ParkrobTrajplanner/Initial_plan/AMR_HybridAStar_粗路径实施计划.md b/ClumsyPilot/ParkrobTrajplanner/Initial_plan/AMR_HybridAStar_粗路径实施计划.md new file mode 100644 index 0000000..12cb362 --- /dev/null +++ b/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 自动化验证。 + diff --git a/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md b/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md index cc2b4ca..a6ccca9 100644 --- a/docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md +++ b/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` 一类接口。