428 lines
23 KiB
Markdown
428 lines
23 KiB
Markdown
# Hybrid A* P0 规划核心 Implementation Plan
|
||
|
||
> **For agentic workers:** REQUIRED SUB-SKILL: Use `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，按任务顺序实施，并使用 `- [ ]` 更新执行状态。
|
||
|
||
**Goal:** 在既有 `PlanningGridMap` 之上提供可复用、可验证且不依赖 UI 的 Hybrid A* 粗路径规划服务。
|
||
|
||
**Architecture:** P0 先固定 m/rad/1/m 数据契约和连续车辆碰撞边界，再在该边界上实现恒曲率原语、二维启发式、确定性 Hybrid A*、路径重建和最终复核。`CoarsePathPlanningService` 是业务的唯一组合入口；`HybridAStarPlanner` 是只消费已建地图的下层门面。
|
||
|
||
**Tech Stack:** C# 10、.NET Standard 2.0、现有 `PlanningGridMap`、PowerShell 反射契约测试、`CancellationToken`。
|
||
|
||
## Global Constraints
|
||
|
||
- 所有运行时代码位于 `ClumsyPilot/ParkrobTrajplanner/CoarsePath/`，命名空间为 `MultiWheelC.TrajectoryPlanning.CoarsePath` 或其子命名空间。
|
||
- Map 只保存外部障碍物；安全余量只在连续车辆碰撞检查时扩张车辆矩形，绝不写入 Map。
|
||
- 地图输入和障碍几何使用 mm；CoarsePath 的位置使用 m、航向使用 rad、曲率使用 1/m。
|
||
- 规划器只能接受 `PlanningGridMap`，不得引用 TwoLeg、定位、Painter、UI 或系统时间。
|
||
- 所有公开类型、构造函数、属性和方法使用中文 XML 文档，明确参数单位、边界以及返回或失败语义；内部几何/搜索不变量使用简短中文注释。
|
||
- `netstandard2.0` 禁止直接使用 `PriorityQueue`、`Math.Clamp`、`double.IsFinite`、`record`、`init`。
|
||
- 固定约束：原语最大长度 0.50 m；积分最大步长 0.05 m；碰撞中心步长不超过 `min(0.025 m, Map.ResolutionMeters / 2)`；默认终点容差为 0.15 m、5°。
|
||
- 终点候选必须进入 Open List，只有作为最佳有效条目出队时才能成功；地图外始终按占据处理。
|
||
- 本计划不实现 P1 的 Clumsy `MovementTest`、Painter 绘制、Release 性能基准或旧 TrapMap 入口退役。
|
||
- 按用户现有约束，不执行 Git 自检、暂存、提交或推送。
|
||
|
||
---
|
||
|
||
## 文件结构
|
||
|
||
```text
|
||
ClumsyPilot/ParkrobTrajplanner/
|
||
├── CoarsePath/
|
||
│ ├── Contracts/ # 请求、结果、枚举和值对象
|
||
│ ├── Vehicle/ # 扩大车辆几何和连续碰撞
|
||
│ ├── Search/ # 原语、堆、启发式和 Hybrid A* 搜索
|
||
│ ├── Output/ # 回溯、稠密路径装配和最终验证
|
||
│ ├── Facade/ # 一次调用编排和调试旁路契约
|
||
│ ├── HybridAStarPlanner.cs
|
||
│ └── README.md # 粗规划调用方文档；链接至 ../Map/README.md
|
||
└── tests/
|
||
├── verify_coarse_path_collision.ps1
|
||
├── verify_coarse_path_search.ps1
|
||
└── verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
## Task 1: 固定公共契约、状态与默认配置
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/Pose2D.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/TravelDirection.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/GoalDirectionConstraint.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/VehicleParameters.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/HybridAStarConfiguration.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningRequest.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningStatus.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPoint.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/CoarsePathPointSource.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PathSegment.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningResult.cs`
|
||
- Modify: `ClumsyPilot/tests/verify_planning_utils.ps1`
|
||
|
||
**Produces:**
|
||
|
||
```csharp
|
||
public sealed class Pose2D
|
||
{
|
||
public Pose2D(double xMeters, double yMeters, double headingRadians);
|
||
public double X { get; }
|
||
public double Y { get; }
|
||
public double Heading { get; }
|
||
}
|
||
|
||
public sealed class PlanningRequest
|
||
{
|
||
public PlanningGridMap Map { get; set; }
|
||
public Pose2D Start { get; set; }
|
||
public Pose2D Goal { get; set; }
|
||
public VehicleParameters Vehicle { get; set; }
|
||
public HybridAStarConfiguration Configuration { get; set; }
|
||
public double StartVehicleCurvature { get; set; }
|
||
public TravelDirection? StartDirection { get; set; }
|
||
public GoalDirectionConstraint GoalDirection { get; set; }
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 1: 写失败的公共契约测试。** 在 `verify_planning_utils.ps1` 载入程序集后添加反射断言，检查 `Pose2D` 构造函数、三个枚举、`PlanningRequest` 属性和每个默认值。默认配置断言如下：
|
||
|
||
```powershell
|
||
$config = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.HybridAStarConfiguration
|
||
Assert-Equal 0.50 $config.PrimitiveLengthMeters '原语最大长度'
|
||
Assert-Equal 0.05 $config.IntegrationStepMeters '积分步长'
|
||
Assert-Equal 0.025 $config.MaximumCollisionCheckStepMeters '碰撞步长'
|
||
Assert-Equal 5 $config.CurvatureLevelCount '曲率等级数'
|
||
Assert-Equal 200000 $config.MaximumExpandedNodes '节点上限'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行测试并确认 RED。**
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
|
||
```
|
||
|
||
预期：脚本因 `CoarsePath` 类型尚不存在而以非零退出。
|
||
|
||
- [ ] **Step 3: 实现最小契约。** `TravelDirection` 仅含 `Forward`、`Reverse`；`GoalDirectionConstraint` 仅含 `Any`、`Forward`、`Reverse`；`CoarsePathPointSource` 仅含 `Start`、`MotionPrimitive`、`GoalTruncation`。`PlanningStatus` 必须包含 `Success`、`Cancelled`、`InvalidRequest`、`InvalidMap`、`MapNotReady`、`InvalidVehicleParameters`、`InvalidCurvatureConfiguration`、`StartOutsideMap`、`StartInCollision`、`GoalOutsideMap`、`GoalInCollision`、`SearchTimeout`、`SearchNodeLimitExceeded`、`NoFeasiblePath`、`BacktrackingFailed`、`FinalValidationFailed`、`InternalError`。
|
||
|
||
`HybridAStarConfiguration` 的构造默认值必须是：`Math.PI / 36d` 航向/终点航向容差、5 秒超时、`HeuristicWeight=1d`、`ReverseCostMultiplier=1.5d`、`GearSwitchPenaltyMeters=1d`、`CurvatureMagnitudeWeight=0.10d`、`CurvatureChangePenaltyMetersPerLevel=0.05d`、`ClearanceCostWeight=0.20d`、`ClearanceCostDistanceMeters=0.50d`。
|
||
|
||
`PlanningResult` 只允许成功结果携带非空路径与分段；所有失败工厂方法返回空只读集合并保留诊断。`PlanningDiagnostics` 固定记录扩展、生成、重开、陈旧堆条目、Open List 峰值、路径长度、最小保守净空、耗时和终止原因。
|
||
|
||
- [ ] **Step 4: 重新运行工具契约测试并确认 GREEN。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
|
||
```
|
||
|
||
预期：退出码为 0，既有 Utils/Map 契约仍可加载。
|
||
|
||
## Task 2: 实现扩大车辆足迹与连续碰撞检查
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleKinematics.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/VehicleFootprint.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/OrientedRectangleCellIntersection.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Vehicle/FootprintCollisionChecker.cs`
|
||
- Create: `ClumsyPilot/tests/verify_coarse_path_collision.ps1`
|
||
|
||
**Consumes:** `PlanningGridMap`、`Pose2D`、`VehicleParameters`。
|
||
|
||
**Produces:**
|
||
|
||
```csharp
|
||
public sealed class FootprintCollisionChecker
|
||
{
|
||
public bool IsPoseCollisionFree(
|
||
Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||
double additionalMarginMeters, out double bodyClearanceMeters);
|
||
|
||
public bool IsSweptMotionCollisionFree(
|
||
Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
|
||
double maximumCenterStepMeters, out double minimumBodyClearanceMeters);
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 1: 写失败的连续碰撞测试。** 脚本通过 `PlanningMapFactory` 创建 50 mm 地图和单个薄矩形障碍，验证下面三种行为：车辆与障碍格擦边返回碰撞、距离场净空严格大于外接圆半径时返回安全、两个端点安全但中间穿过障碍时扫掠检查返回碰撞。
|
||
|
||
```powershell
|
||
$checker = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.FootprintCollisionChecker
|
||
$clearance = 0.0
|
||
$safe = $checker.IsPoseCollisionFree($pose, $map, $vehicle, 0.0, [ref]$clearance)
|
||
Assert-False $safe '矩形擦边必须视为碰撞'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行碰撞脚本并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
|
||
```
|
||
|
||
预期：因车辆命名空间和碰撞检查器不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现最小连续几何。** `VehicleKinematics` 在最大曲率与最小转弯半径都存在时取 `Math.Min(maximumCurvature, 1d / minimumRadius)`。`VehicleFootprint` 用 `LengthMeters + 2 * SafetyMarginMeters` 和 `WidthMeters + 2 * SafetyMarginMeters` 构造以 `Pose2D` 为几何中心的旋转矩形、AABB 与外接圆。
|
||
|
||
`OrientedRectangleCellIntersection` 使用 SAT：矩形的两个单位轴和格子的世界 X/Y 轴都作为投影轴；任一轴存在严格分离才是不相交，投影接触算相交。`FootprintCollisionChecker` 依次验证四角均在地图内、用严格 `distance > radius + additionalMargin` 快速放行、遍历 AABB 内占据格并执行 SAT。扫掠检查将中心位移切分到 `min(maximumCenterStepMeters, map.ResolutionMeters / 2d)`，每一段的临时边距为 `0.5d * (centerDisplacement + circumscribedRadius * Math.Abs(headingDelta))`。
|
||
|
||
- [ ] **Step 4: 扩展碰撞测试并运行 GREEN。** 加入 0°、45°、任意航向、栅格中心/亚栅格中心、薄障碍、边界外和扫掠场景；随后运行：
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
|
||
```
|
||
|
||
预期：构建与脚本退出码均为 0。
|
||
|
||
## Task 3: 实现原语积分、目标容差与内部截断
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitive.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitiveGenerator.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GoalToleranceChecker.cs`
|
||
- Create: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||
|
||
**Consumes:** `Pose2D`、方向、车辆最大曲率、`HybridAStarConfiguration`、`FootprintCollisionChecker`。
|
||
|
||
**Produces:** 含方向、曲率、实际长度和内部积分点的不可变 `MotionPrimitive`；目标检查器只判定位置、航向和目标进入方向。
|
||
|
||
- [ ] **Step 1: 写失败的原语测试。** 验证直行、圆弧、倒车、0.50 m 上限、积分点间距上限，以及目标在 0.30 m 处时原语恰好截断到第一个满足条件的内部点。
|
||
|
||
```powershell
|
||
$primitive = $generator.Generate($start, $curvature, $direction, $config, $map)
|
||
Assert-True ($primitive.Points.Count -ge 1) '原语必须产生内部积分点'
|
||
Assert-Equal 0.30 $truncated.ActualLengthMeters '0.30m 目标必须在原语内部截断'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行搜索脚本并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||
```
|
||
|
||
预期：因原语类型尚不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现解析积分和检查顺序。** 单步积分使用：
|
||
|
||
```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;
|
||
}
|
||
```
|
||
|
||
原语点步长不得超过 `min(IntegrationStepMeters, MaximumCollisionCheckStepMeters, Map.ResolutionMeters / 2d)`。每个内部点严格按“有限数值 → 从前一点的扫掠碰撞 → 终点容差”执行；命中目标即截断，并标记 `GoalTruncation`。起点已满足目标时生成零长度终点候选，不生成运动原语。
|
||
|
||
- [ ] **Step 4: 运行原语测试并确认 GREEN。** 加入五个曲率等级、曲率相邻变化最多一级和 `±π` 航向容差的断言；再运行 `verify_coarse_path_search.ps1`，预期退出码为 0。
|
||
|
||
## Task 4: 实现确定性 Open List、代价与二维启发式
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/BinaryMinHeap.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/SearchCostCalculator.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/GridDijkstraHeuristic.cs`
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||
|
||
**Produces:** 内部二叉最小堆、等效米代价计算器和目标反向八邻域距离启发式。
|
||
|
||
- [ ] **Step 1: 写失败的堆、代价和启发式测试。** 验证堆的排序优先级为 `F`、`H`、较大 `G`、插入序号；验证八邻域斜向代价和禁止切过两个正交障碍的对角夹角；验证倒车、换向、曲率和净空代价项。
|
||
|
||
```powershell
|
||
Assert-Equal 'node-b' $heap.Pop().Id '相同 F 时应先选较小 H'
|
||
Assert-Throws { $calculator.Calculate($invalidInput) } '负权重必须拒绝'
|
||
Assert-True ([double]::IsPositiveInfinity($heuristic.GetCost($blockedRow, $blockedCol))) '二维不可达应为无穷'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行搜索测试并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||
```
|
||
|
||
预期：因堆、代价或启发式类型不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现最小支持结构。** `BinaryMinHeap` 使用 `List<T>`，比较器严格按 `F`、`H`、反向 `G`、插入序号。代价必须实现：
|
||
|
||
```text
|
||
length * directionMultiplier *
|
||
(1 + curvatureMagnitudeWeight * abs(curvature / maximumCurvature)
|
||
+ clearanceCostWeight * max(0, 1 - clearance / clearanceCostDistance))
|
||
+ gearSwitchPenalty
|
||
+ curvatureChangePenalty * abs(curvatureLevelDelta)
|
||
```
|
||
|
||
`GridDijkstraHeuristic` 从目标格反向传播四邻域 1 倍格长和对角 `sqrt(2)` 倍格长；对角移动前确认两个正交邻格均未占据。
|
||
|
||
- [ ] **Step 4: 运行搜索测试并确认 GREEN。** 重复构造同一输入两次，断言出队顺序相同；运行 `verify_coarse_path_search.ps1`，预期退出码为 0。
|
||
|
||
## Task 5: 实现 Hybrid A* 节点、重开与终点候选管理
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNode.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarNodeKey.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs`
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1`
|
||
|
||
**Consumes:** Task 2–4 的碰撞、原语、堆、代价与启发式。
|
||
|
||
**Produces:** 接收已验证请求并返回成功节点索引或明确搜索失败状态的内部搜索器。
|
||
|
||
- [ ] **Step 1: 写失败的搜索测试。** 覆盖空图前进、单矩形绕行、允许倒车的狭窄场景、起始曲率、目标方向、无解、取消、超时、节点上限、较小 `G` 重开和终点候选出队顺序。
|
||
|
||
```powershell
|
||
$result = $search.Search($request, [Threading.CancellationToken]::None)
|
||
Assert-Equal 'Success' $result.Status '空图应规划成功'
|
||
Assert-True $result.ReopenedNodeCount -gt 0 '更小 G 到达同键时必须允许重开'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行搜索脚本并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||
```
|
||
|
||
预期：因 `HybridAStarSearch` 不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现离散键与搜索循环。** `HybridAStarNodeKey` 固定包含位置行列、航向索引、方向和曲率等级。普通状态的最佳 `G` 保存在 `Dictionary<HybridAStarNodeKey, double>`；发现严格更小的 `G` 时压入新条目，旧条目在弹出时丢弃。循环在扩展前检查 `CancellationToken`、配置超时和最大扩展数。
|
||
|
||
终点候选压入同一 Open List，但不放入普通键的去重表；它只能在作为当前最佳有效条目弹出、重新验证终点条件与末段碰撞后成功。Open List 耗尽返回 `NoFeasiblePath`。
|
||
|
||
- [ ] **Step 4: 运行全量搜索场景并确认 GREEN。** 对每个固定场景重复运行两次并断言状态、路径代价和节点扩展顺序一致；运行 `verify_coarse_path_search.ps1`，预期退出码为 0。
|
||
|
||
## Task 6: 回溯、路径装配、最终验证与下层门面
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/PathBacktracker.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathValidator.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs`
|
||
- Create: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
||
|
||
**Produces:**
|
||
|
||
```csharp
|
||
public sealed class HybridAStarPlanner
|
||
{
|
||
public PlanningResult Plan(
|
||
PlanningRequest request,
|
||
CancellationToken cancellationToken = default(CancellationToken));
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 1: 写失败的路径输出测试。** 验证首点弧长为 0、弧长不递减、展开航向连续、终点截断来源、相邻重复点只允许作为换向对、分段的包含式索引覆盖全部路径。
|
||
|
||
```powershell
|
||
Assert-Equal 0.0 $result.Path[0].ArcLength '起点弧长必须为零'
|
||
Assert-True ($result.Segments[-1].EndIndex -eq ($result.Path.Count - 1)) '分段必须覆盖尾点'
|
||
Assert-Equal 'FinalValidationFailed' $invalid.Status '最终复核失败不能发布部分路径'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行集成脚本并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
预期：因 `HybridAStarPlanner` 与输出类型不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现回溯和最终复核。** 搜索节点只保留父索引和原语描述；`PathBacktracker` 在成功后使用同一解析积分公式重建内部点。`CoarsePathAssembler` 累计弧长，保持换向处两个相同位姿/弧长而方向不同的点，并让新方向点设置 `IsGearSwitchPoint=true`。
|
||
|
||
`CoarsePathValidator` 使用与搜索相同的 `FootprintCollisionChecker` 和扫掠规则，检查有限数、曲率上限、起终点容差/方向、弧长单调性、换向对与分段覆盖。验证失败返回 `FinalValidationFailed`，路径和分段均为空。
|
||
|
||
`HybridAStarPlanner` 在调用搜索前映射空请求、Map 未就绪、车辆无效、曲率配置无效、起终点越界及起终点碰撞；其余异常收敛为 `InternalError` 并记录诊断。
|
||
|
||
- [ ] **Step 4: 运行碰撞、搜索和集成脚本并确认 GREEN。**
|
||
|
||
```powershell
|
||
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
|
||
```
|
||
|
||
预期：三个脚本均退出 0。
|
||
|
||
## Task 7: 一次调用服务、调试旁路契约与 README
|
||
|
||
**Files:**
|
||
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJob.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningJobResult.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/PlanningDebugOptions.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/IPlanningDebugSink.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Facade/CoarsePathPlanningService.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
||
|
||
**Produces:**
|
||
|
||
```csharp
|
||
public sealed class CoarsePathPlanningService
|
||
{
|
||
public CoarsePathPlanningJobResult Plan(
|
||
CoarsePathPlanningJob job,
|
||
CancellationToken cancellationToken = default(CancellationToken));
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 1: 写失败的一次调用测试。** 断言服务先建图、地图失败时不搜索、成功时同时返回 `PlanningMapBuildResult` 和 `PlanningResult`；同一服务实例两次使用相同地图请求时第二次是 `Input` 缓存命中。
|
||
|
||
```powershell
|
||
$service = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Facade.CoarsePathPlanningService
|
||
$first = $service.Plan($job, [Threading.CancellationToken]::None)
|
||
$second = $service.Plan($job, [Threading.CancellationToken]::None)
|
||
Assert-Equal 'Input' $second.MapResult.CacheHit.ToString() '服务必须长期持有地图工厂'
|
||
```
|
||
|
||
- [ ] **Step 2: 运行集成脚本并确认 RED。**
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
预期：因门面类型与 README 不存在而失败。
|
||
|
||
- [ ] **Step 3: 实现服务和文档。** `CoarsePathPlanningService` 构造时创建一个长期 `PlanningMapFactory` 与一个 `HybridAStarPlanner`，计划调用顺序固定为：
|
||
|
||
```text
|
||
PlanningMapFactory.Create(job.MapRequest)
|
||
-> 地图失败：包装 MapResult，返回空 PlanningResult
|
||
-> 地图成功：HybridAStarPlanner.Plan(job 转换的 PlanningRequest)
|
||
-> 仅依 Debug 选项向 IPlanningDebugSink 发布旁路数据
|
||
```
|
||
|
||
默认 sink 为空实现。任何 sink 异常只追加调试诊断，绝不改变地图哈希、规划状态、路径或分段。
|
||
|
||
README 必须包含以下小节：模块范围；Map 与 CoarsePath 的职责表；`CoarsePathPlanningService.Plan` 的可编译调用示例；mm/m/rad/1/m 单位表；`SourceVersion` 与缓存规则；`PlanningStatus` 处理示例；路径点和方向分段含义；第一版不支持的平滑、速度规划、控制和横移能力；到 `../Map/README.md` 的链接。
|
||
|
||
- [ ] **Step 4: 运行完整 P0 验收。**
|
||
|
||
```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
|
||
```
|
||
|
||
预期：构建和全部七个脚本退出码均为 0；不执行 P1 UI 或性能工作。
|
||
|
||
## Plan Self-Review
|
||
|
||
- 覆盖性：Task 1 覆盖公共契约；Task 2 覆盖连续车辆碰撞；Task 3–5 覆盖原语、代价、启发式、确定性搜索与终点候选；Task 6 覆盖输出与最终复核；Task 7 覆盖一次调用门面、文档和全量验收。
|
||
- 类型一致性：所有搜索和门面输入均以 `PlanningGridMap`、`PlanningRequest`、`CoarsePathPlanningJob` 为唯一跨层契约；Map 构建只存在于 Task 7 的服务门面。
|
||
- 范围：没有包含 Clumsy UI、Painter、性能基准或旧 TrapMap 迁移，这些均为 P1。
|