535 lines
29 KiB
Markdown
535 lines
29 KiB
Markdown
|
# P1 粗路径 Clumsy UI 集成 Implementation Plan
|
|||
|
|
|
|||
|
|
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
|||
|
|
|
|||
|
|
**Goal:** 交付可在 Clumsy 中后台运行的七个粗路径测试入口，显示真实规划栅格快照和完整路径信息，并支持传入 AMR 世界位姿与手动目标位姿。
|
|||
|
|
|
|||
|
|
**Architecture:** `CoarsePathScenarioFactory` 保持无 UI 的纯输入构造职责，提供六个可重复的回归场景及一个显式标注为空图演示的“AMR 位姿 + 手动终点”请求创建入口。`MovementTest.CoarsePathTest.cs` 只作为 UI 适配层：把 AMR/目标 mm+deg 转为核心所需的 m+rad，使用一个共享门面在 `Task.Run` 后台运行，并从不可变 `CoarsePathPlanningJobResult` 绘制地图快照和路径。
|
|||
|
|
|
|||
|
|
**Tech Stack:** C# / `netstandard2.0`、现有 Clumsy `MovementTest`/`Painter`、`CoarsePathPlanningService`、PowerShell 反射验证脚本。
|
|||
|
|
|
|||
|
|
## Global Constraints
|
|||
|
|
|
|||
|
|
- `PlanningMapRequest` 的地图、障碍物、AMR 输入和手动目标 X/Y 均为世界 mm；`Pose2D` 和路径 X/Y 为世界 m；核心航向为 rad。
|
|||
|
|
- 项目上游的 AMR `th` 输入按 deg 适配为 `th * Math.PI / 180d`；不得沿用直接将该值传给 `Math.Cos/Sin` 的旧写法。
|
|||
|
|
- AMR 起点必须是车辆几何中心；传感器安装点必须由上游先按外参转换。
|
|||
|
|
- UI 和测试只能调用 `CoarsePathPlanningService.Plan(job, token)`；不得直接实例化 `PlanningMapFactory`、`HybridAStarPlanner`、栅格化器、碰撞器、原语生成器或搜索节点。
|
|||
|
|
- 所有七个 MovementTest 都不得引用 `BasicPilotBase.Chassis`、`SendMotion`、`DriveTask` 或任何底盘控制 API。
|
|||
|
|
- `Test` 不得等待后台任务或读取 `Task.Result`；`TestStop` 先取消令牌，再使运行编号失效、解绑任务并清空 Painter。
|
|||
|
|
- 只在 `PlanningStatus.Success` 绘制路径、方向箭头、换向点和扩大车体检查框；失败、取消和超时只显示地图、起点、终点和状态。
|
|||
|
|
- 代码兼容 `netstandard2.0`，不引入新 NuGet 包；公开类型/成员写中文 XML 文档，复杂单位与并发逻辑写简短中文行注释。
|
|||
|
|
- 不改动 TrapMap 文件或旧 TrapMap 验证脚本；不执行 Git 状态、差异、提交或重置操作。
|
|||
|
|
|
|||
|
|
---
|
|||
|
|
|
|||
|
|
## 文件结构
|
|||
|
|
|
|||
|
|
| 文件 | 变更职责 |
|
|||
|
|
| --- | --- |
|
|||
|
|
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs` | 新建纯场景工厂、六场景枚举、AMR/目标位姿 mm+deg 到核心 `Pose2D` 的转换，以及空图演示手动目标请求。 |
|
|||
|
|
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs` | 新建七个 UI 入口、共享会话执行器、任务取消、Painter 地图/路径/图例绘制与手动输入解析。 |
|
|||
|
|
| `ClumsyPilot/tests/verify_coarse_path_integration.ps1` | 为工厂行为、单位转换、缓存/换向/无解、UI 源码边界与 README 内容新增真实程序集和文本断言。 |
|
|||
|
|
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md` | 记录 P1 测试入口、输入单位、空图演示限制、图例、停止语义及无底盘命令边界。 |
|
|||
|
|
|
|||
|
|
### 固定接口
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
|||
|
|
|
|||
|
|
public enum CoarsePathTestScenario
|
|||
|
|
{
|
|||
|
|
ExplicitEmpty,
|
|||
|
|
RectangleDetour,
|
|||
|
|
ManualAndTwoLeg,
|
|||
|
|
CacheHit,
|
|||
|
|
ReverseGearSwitch,
|
|||
|
|
NoFeasiblePath,
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static class CoarsePathScenarioFactory
|
|||
|
|
{
|
|||
|
|
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario);
|
|||
|
|
|
|||
|
|
public static CoarsePathPlanningJob CreateManualGoalDemo(
|
|||
|
|
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
|||
|
|
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees);
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
`CreateManualGoalDemo` 只构造带 2,000 mm 边缘留白的显式空图演示请求，并在 README/测试名称中明确其不代表真实环境安全。未来现场入口必须提供真实 `IMapObstacleSource` 快照，而不是修改此方法的语义。
|
|||
|
|
|
|||
|
|
### Task 1: 工厂契约与失败测试
|
|||
|
|
|
|||
|
|
**Files:**
|
|||
|
|
|
|||
|
|
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
|||
|
|
- Create later in Task 2: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs`
|
|||
|
|
|
|||
|
|
**Consumes:** 已有 `CoarsePathPlanningService.Plan(CoarsePathPlanningJob, CancellationToken)`、`Find-Method`、`Assert-True`、`Assert-Equal` 与程序集加载逻辑。
|
|||
|
|
|
|||
|
|
**Produces:** 对 `CoarsePathTestScenario` 和 `CoarsePathScenarioFactory` 的反射行为约束；Task 2 的最小实现必须使这些断言通过。
|
|||
|
|
|
|||
|
|
- [ ] **Step 1: 在集成脚本加入工厂反射测试**
|
|||
|
|
|
|||
|
|
在现有 facade 检查后、最终输出前插入以下 PowerShell。它要求类型和两个公开方法都存在，因此在工厂未创建时失败。
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
$testNamespace = $coarsePath + 'Test.'
|
|||
|
|
$scenarioEnumType = $assembly.GetType($testNamespace + 'CoarsePathTestScenario', $false)
|
|||
|
|
$scenarioFactoryType = $assembly.GetType($testNamespace + 'CoarsePathScenarioFactory', $false)
|
|||
|
|
Assert-True ($scenarioEnumType -ne $null) 'P1 scenario enum must exist.'
|
|||
|
|
Assert-True ($scenarioFactoryType -ne $null) 'P1 scenario factory must exist.'
|
|||
|
|
|
|||
|
|
$factoryCreate = Find-Method $scenarioFactoryType 'Create' @($scenarioEnumType)
|
|||
|
|
$factoryManual = Find-Method $scenarioFactoryType 'CreateManualGoalDemo' @(
|
|||
|
|
[double], [double], [double], [double], [double], [double])
|
|||
|
|
Assert-True ($factoryCreate -ne $null) 'P1 scenario factory must expose Create(scenario).'
|
|||
|
|
Assert-True ($factoryManual -ne $null) 'P1 scenario factory must expose CreateManualGoalDemo with six doubles.'
|
|||
|
|
|
|||
|
|
$scenarioNames = @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'CacheHit', 'ReverseGearSwitch', 'NoFeasiblePath')
|
|||
|
|
foreach ($scenarioName in $scenarioNames) {
|
|||
|
|
$scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
|
|||
|
|
$jobA = $factoryCreate.Invoke($null, @($scenario))
|
|||
|
|
$jobB = $factoryCreate.Invoke($null, @($scenario))
|
|||
|
|
Assert-True ($jobA -ne $null) "Scenario $scenarioName must return a job."
|
|||
|
|
Assert-False ([object]::ReferenceEquals($jobA, $jobB)) "Scenario $scenarioName must return a new job per call."
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$manualJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 90.0, 4000.0, 2000.0, 0.0))
|
|||
|
|
Assert-Near 1.0 $manualJob.Start.X 'Manual AMR X must convert mm to m.'
|
|||
|
|
Assert-Near 2.0 $manualJob.Start.Y 'Manual AMR Y must convert mm to m.'
|
|||
|
|
Assert-Near ([Math]::PI / 2.0) $manualJob.Start.Heading 'Manual AMR heading must convert degrees to radians.'
|
|||
|
|
Assert-Near 4.0 $manualJob.Goal.X 'Manual goal X must convert mm to m.'
|
|||
|
|
Assert-Near 0.0 $manualJob.Goal.Heading 'Manual goal heading must convert degrees to radians.'
|
|||
|
|
Assert-True $manualJob.MapRequest.AllowExplicitEmptyMap 'Manual goal demo must declare its empty map explicitly.'
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- [ ] **Step 2: 运行脚本确认失败**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 构建成功；脚本因 `P1 scenario enum must exist.` 失败。
|
|||
|
|
|
|||
|
|
### Task 2: 实现纯场景工厂并通过行为测试
|
|||
|
|
|
|||
|
|
**Files:**
|
|||
|
|
|
|||
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs`
|
|||
|
|
- Test: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
|||
|
|
|
|||
|
|
**Consumes:** Task 1 的枚举和两个公开工厂方法；`MapBoundsMm`、`ManualObstacleSource`、`TwoLegObstacleSource`、`Pose2D`、`VehicleParameters`、`HybridAStarConfiguration`。
|
|||
|
|
|
|||
|
|
**Produces:** 六个可重复 job 与一个空图演示手动 job，供 UI 入口和后续脚本行为断言共同使用。
|
|||
|
|
|
|||
|
|
- [ ] **Step 1: 先建立最小的公共类型和转换辅助函数**
|
|||
|
|
|
|||
|
|
创建工厂文件并定义以下枚举、转换函数和公共入口。所有输入先做有限值检查；非有限输入抛出 `ArgumentOutOfRangeException`，避免伪造核心请求。
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
public enum CoarsePathTestScenario
|
|||
|
|
{
|
|||
|
|
ExplicitEmpty,
|
|||
|
|
RectangleDetour,
|
|||
|
|
ManualAndTwoLeg,
|
|||
|
|
CacheHit,
|
|||
|
|
ReverseGearSwitch,
|
|||
|
|
NoFeasiblePath,
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static class CoarsePathScenarioFactory
|
|||
|
|
{
|
|||
|
|
private const double MillimetersPerMeter = 1000d;
|
|||
|
|
private const double DegreesToRadians = Math.PI / 180d;
|
|||
|
|
private const float ResolutionMillimeters = 50f;
|
|||
|
|
private const double ManualMapPaddingMillimeters = 2000d;
|
|||
|
|
|
|||
|
|
public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
|
|||
|
|
{
|
|||
|
|
switch (scenario)
|
|||
|
|
{
|
|||
|
|
case CoarsePathTestScenario.ExplicitEmpty: return CreateExplicitEmpty();
|
|||
|
|
case CoarsePathTestScenario.RectangleDetour: return CreateRectangleDetour();
|
|||
|
|
case CoarsePathTestScenario.ManualAndTwoLeg: return CreateManualAndTwoLeg();
|
|||
|
|
case CoarsePathTestScenario.CacheHit: return CreateRectangleDetour();
|
|||
|
|
case CoarsePathTestScenario.ReverseGearSwitch: return CreateReverseGearSwitch();
|
|||
|
|
case CoarsePathTestScenario.NoFeasiblePath: return CreateNoFeasiblePath();
|
|||
|
|
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
public static CoarsePathPlanningJob CreateManualGoalDemo(
|
|||
|
|
double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
|
|||
|
|
double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
|
|||
|
|
{
|
|||
|
|
EnsureFinite(startXMillimeters, nameof(startXMillimeters));
|
|||
|
|
EnsureFinite(startYMillimeters, nameof(startYMillimeters));
|
|||
|
|
EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
|
|||
|
|
EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
|
|||
|
|
EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
|
|||
|
|
EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
|
|||
|
|
return CreateJob(CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters),
|
|||
|
|
ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
|
|||
|
|
ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees), null, GoalDirectionConstraint.Any);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
|
|||
|
|
=> new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
|
|||
|
|
headingDegrees * DegreesToRadians);
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- [ ] **Step 2: 实现统一请求模板和六个固定场景**
|
|||
|
|
|
|||
|
|
使用统一的车辆和配置，避免场景间无意改变安全或搜索语义。模板必须是新对象：
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
|
|||
|
|
TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
|
|||
|
|
{
|
|||
|
|
return new CoarsePathPlanningJob
|
|||
|
|
{
|
|||
|
|
MapRequest = mapRequest,
|
|||
|
|
Start = start,
|
|||
|
|
Goal = goal,
|
|||
|
|
Vehicle = new VehicleParameters
|
|||
|
|
{
|
|||
|
|
LengthMeters = 0.80d,
|
|||
|
|
WidthMeters = 0.60d,
|
|||
|
|
SafetyMarginMeters = 0.05d,
|
|||
|
|
MaximumCurvaturePerMeter = 1d / 1.20d,
|
|||
|
|
},
|
|||
|
|
Configuration = new HybridAStarConfiguration(),
|
|||
|
|
StartDirection = startDirection,
|
|||
|
|
GoalDirection = goalDirection,
|
|||
|
|
};
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
固定地图均使用 `new MapBoundsMm(0f, 6000f, 0f, 4000f)`、50 mm 分辨率。以下代码固定各场景的障碍来源和世界位姿，所有 `ManualObstacleSource` 版本为 `1L`、所有必需来源为 `true`：
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
private static CoarsePathPlanningJob CreateExplicitEmpty()
|
|||
|
|
=> CreateJob(CreateMap(true, Array.Empty<IMapObstacleSource>()),
|
|||
|
|
new Pose2D(1d, 2d, 0d), new Pose2D(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
|||
|
|
|
|||
|
|
private static CoarsePathPlanningJob CreateRectangleDetour()
|
|||
|
|
=> CreateJob(CreateMap(false, new IMapObstacleSource[]
|
|||
|
|
{
|
|||
|
|
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
|||
|
|
{ new AxisAlignedRectangleObstacle(2700f, 3300f, 1200f, 2800f) }),
|
|||
|
|
}), new Pose2D(1d, 2d, 0d), new Pose2D(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
|||
|
|
|
|||
|
|
private static CoarsePathPlanningJob CreateManualAndTwoLeg()
|
|||
|
|
=> CreateJob(CreateMap(false, new IMapObstacleSource[]
|
|||
|
|
{
|
|||
|
|
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
|||
|
|
{
|
|||
|
|
new CircleObstacle(2400f, 1300f, 220f),
|
|||
|
|
new AxisAlignedRectangleObstacle(3000f, 3600f, 2000f, 2600f),
|
|||
|
|
}),
|
|||
|
|
new TwoLegObstacleSource("two-leg", 1L, true,
|
|||
|
|
new TwoLegProjectionInput(true, 3900f, 2500f, 0d,
|
|||
|
|
-180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
|
|||
|
|
}), new Pose2D(1d, 1d, 0d), new Pose2D(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
|
|||
|
|
|
|||
|
|
private static CoarsePathPlanningJob CreateNoFeasiblePath()
|
|||
|
|
=> CreateJob(CreateMap(false, new IMapObstacleSource[]
|
|||
|
|
{
|
|||
|
|
new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
|
|||
|
|
{ new AxisAlignedRectangleObstacle(2900f, 3100f, 0f, 4000f) }),
|
|||
|
|
}), new Pose2D(1d, 2d, 0d), new Pose2D(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
`CreateMap` 返回新的 `PlanningMapRequest`，固定写入地图边界、分辨率、给定来源和 `AllowExplicitEmptyMap`。倒车换向场景使用空图、起点 `(1,2,0)`、终点 `(4,2,0)`、`StartDirection=Forward`、`GoalDirection=Reverse`，使路径必须以至少一次换向结束。
|
|||
|
|
|
|||
|
|
倒车换向场景使用空图、起点 `(1,2,0)`、终点 `(4,2,0)`、`StartDirection=Forward`、`GoalDirection=Reverse`，使路径必须以至少一次换向结束。若 P0 的离散搜索在此几何下无法稳定得到成功，只允许调整此场景的目标距离或障碍布局，且测试必须继续要求 `IsGearSwitchPoint=true`。
|
|||
|
|
|
|||
|
|
`CreateManualDemoMap` 用起终点 X/Y 的最小/最大值各扩展 `ManualMapPaddingMillimeters`，按 50 mm 向外取整，并明确设置 `AllowExplicitEmptyMap=true` 和空的 `ObstacleSources`。
|
|||
|
|
|
|||
|
|
- [ ] **Step 3: 扩展行为断言以覆盖所有场景的实际状态**
|
|||
|
|
|
|||
|
|
在 Task 1 的反射代码之后增加服务执行测试。它不引用任何 Painter 或 MovementTest：
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
$scenarioService = [Activator]::CreateInstance($serviceType)
|
|||
|
|
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
|
|||
|
|
$job = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, $scenarioName)))
|
|||
|
|
$result = $servicePlan.Invoke($scenarioService, @($job, [Threading.CancellationToken]::None))
|
|||
|
|
Assert-Equal 'Success' $result.PlanningResult.Status.ToString() "Scenario $scenarioName must succeed."
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
$reverseJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'ReverseGearSwitch')))
|
|||
|
|
$reverseResult = $servicePlan.Invoke($scenarioService, @($reverseJob, [Threading.CancellationToken]::None))
|
|||
|
|
Assert-True (($reverseResult.PlanningResult.Path | Where-Object { $_.IsGearSwitchPoint }).Count -ge 1) 'Reverse scenario must expose a gear-switch point.'
|
|||
|
|
|
|||
|
|
$noPathJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')))
|
|||
|
|
$noPathResult = $servicePlan.Invoke($scenarioService, @($noPathJob, [Threading.CancellationToken]::None))
|
|||
|
|
Assert-Equal 'NoFeasiblePath' $noPathResult.PlanningResult.Status.ToString() 'Barrier scenario must be infeasible.'
|
|||
|
|
Assert-Equal 0 $noPathResult.PlanningResult.Path.Count 'Infeasible scenario must not publish a path.'
|
|||
|
|
|
|||
|
|
$cacheJobA = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
|||
|
|
$cacheJobB = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
|||
|
|
$cacheFirst = $servicePlan.Invoke($scenarioService, @($cacheJobA, [Threading.CancellationToken]::None))
|
|||
|
|
$cacheSecond = $servicePlan.Invoke($scenarioService, @($cacheJobB, [Threading.CancellationToken]::None))
|
|||
|
|
Assert-Equal 'Input' $cacheSecond.MapResult.CacheHit.ToString() 'Cache-hit scenario must reuse the complete map input.'
|
|||
|
|
Assert-Equal $cacheFirst.PlanningResult.Status $cacheSecond.PlanningResult.Status 'Map cache reuse must not change planning status.'
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- [ ] **Step 4: 运行测试并固定数值场景**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 工厂、单位转换、六个固定场景、缓存与无解断言通过；此时尚未加入 MovementTest 源码检查，因此脚本整体通过。
|
|||
|
|
|
|||
|
|
### Task 3: MovementTest 后台会话与完整 Painter 绘制
|
|||
|
|
|
|||
|
|
**Files:**
|
|||
|
|
|
|||
|
|
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
|||
|
|
- Create: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs`
|
|||
|
|
|
|||
|
|
**Consumes:** Task 2 的 `CoarsePathScenarioFactory.Create` 和 `CreateManualGoalDemo`；`CoarsePathPlanningService`、`CoarsePathPlanningJobResult`、`PlanningGridMap`、`Painter`。
|
|||
|
|
|
|||
|
|
**Produces:** 六个固定场景入口与一个“AMR 位姿 + 手动终点（空图演示）”入口；所有入口通过同一个后台执行器运行和绘制。
|
|||
|
|
|
|||
|
|
- [ ] **Step 1: 写入失败的 UI 源码结构断言**
|
|||
|
|
|
|||
|
|
在 PowerShell 脚本中加入下列纯文本检查，避免在自动化测试中实例化外部 UI：
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
$movementTestPath = Join-Path $plannerRoot 'CoarsePath\Test\MovementTest.CoarsePathTest.cs'
|
|||
|
|
if (-not (Test-Path -LiteralPath $movementTestPath -PathType Leaf)) {
|
|||
|
|
throw 'P1 coarse-path MovementTest source file must exist.'
|
|||
|
|
}
|
|||
|
|
$movementTestContent = Get-Content -LiteralPath $movementTestPath -Raw
|
|||
|
|
foreach ($required in @(
|
|||
|
|
'[MovementTest(name = "粗路径-显式空图")]',
|
|||
|
|
'[MovementTest(name = "粗路径-矩形绕行")]',
|
|||
|
|
'[MovementTest(name = "粗路径-多来源障碍")]',
|
|||
|
|
'[MovementTest(name = "粗路径-缓存命中")]',
|
|||
|
|
'[MovementTest(name = "粗路径-倒车换向")]',
|
|||
|
|
'[MovementTest(name = "粗路径-无解")]',
|
|||
|
|
'[MovementTest(name = "粗路径-AMR起点手动终点(空图演示)")]',
|
|||
|
|
'Task.Run', 'CancellationTokenSource', 'CoarsePathPlanningService',
|
|||
|
|
'PlanningGridMap', 'IsOccupied', 'ResolutionMm', 'SnapshotId', '图例', 'IsGearSwitchPoint')) {
|
|||
|
|
Assert-True $movementTestContent.Contains($required) "MovementTest must contain: $required"
|
|||
|
|
}
|
|||
|
|
foreach ($forbidden in @('PlanningMapFactory', 'HybridAStarPlanner', 'MapObstacleRasterizer',
|
|||
|
|
'FootprintCollisionChecker', 'MotionPrimitiveGenerator', 'HybridAStarSearch',
|
|||
|
|
'BasicPilotBase.Chassis', 'SendMotion', 'DriveTask', '.Wait()', '.Result')) {
|
|||
|
|
Assert-False $movementTestContent.Contains($forbidden) "MovementTest must not depend on: $forbidden"
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- [ ] **Step 2: 运行脚本确认 UI 结构检查失败**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 在工厂断言通过后，因 `P1 coarse-path MovementTest source file must exist.` 失败。
|
|||
|
|
|
|||
|
|
- [ ] **Step 3: 实现共享会话执行器与七个薄入口**
|
|||
|
|
|
|||
|
|
在新文件中使用 `namespace MultiWheelC;`，引用 `System.Threading`、`System.Threading.Tasks`、`System.Drawing`、`System.Numerics`、`ClumsyCore`、`MDCSToolBox.Clumsy.Movements` 和 Map/CoarsePath 命名空间。定义一个内部静态执行器，核心形状如下：
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
internal static class CoarsePathMovementTestRunner
|
|||
|
|
{
|
|||
|
|
private static readonly object SyncRoot = new object();
|
|||
|
|
private static readonly CoarsePathPlanningService Service = new CoarsePathPlanningService();
|
|||
|
|
private static readonly Painter Painter = UI.GetPainter("CoarsePathPlanningV1", true);
|
|||
|
|
private static long _nextRunId;
|
|||
|
|
private static long _activeRunId;
|
|||
|
|
private static CancellationTokenSource _activeCancellation;
|
|||
|
|
private static Task _activeTask;
|
|||
|
|
|
|||
|
|
internal static void Start(string displayName, CoarsePathPlanningJob job)
|
|||
|
|
{
|
|||
|
|
CancellationTokenSource previous;
|
|||
|
|
long runId;
|
|||
|
|
var cancellation = new CancellationTokenSource();
|
|||
|
|
lock (SyncRoot)
|
|||
|
|
{
|
|||
|
|
previous = _activeCancellation;
|
|||
|
|
runId = ++_nextRunId;
|
|||
|
|
_activeRunId = runId;
|
|||
|
|
_activeCancellation = cancellation;
|
|||
|
|
Painter.Clear();
|
|||
|
|
_activeTask = Task.Run(() => Service.Plan(job, cancellation.Token));
|
|||
|
|
_activeTask.ContinueWith(task => Complete(runId, displayName, job, cancellation, task),
|
|||
|
|
CancellationToken.None, TaskContinuationOptions.None, TaskScheduler.Default);
|
|||
|
|
}
|
|||
|
|
previous?.Cancel();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
internal static void Stop()
|
|||
|
|
{
|
|||
|
|
CancellationTokenSource cancellation;
|
|||
|
|
lock (SyncRoot)
|
|||
|
|
{
|
|||
|
|
cancellation = _activeCancellation;
|
|||
|
|
}
|
|||
|
|
cancellation?.Cancel();
|
|||
|
|
lock (SyncRoot)
|
|||
|
|
{
|
|||
|
|
if (!ReferenceEquals(_activeCancellation, cancellation)) return;
|
|||
|
|
_activeCancellation = null;
|
|||
|
|
_activeTask = null;
|
|||
|
|
_activeRunId = ++_nextRunId;
|
|||
|
|
}
|
|||
|
|
Painter.Clear();
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
`Complete` 必须捕获 `task.Exception`，但正常情况下只接受 `CoarsePathPlanningJobResult`。在锁内确认 `runId == _activeRunId`、任务未取消并且 `task.Status == TaskStatus.RanToCompletion` 后才绘制；无论绘制与否都在 finally 中释放该任务专用 `CancellationTokenSource`。不可在锁内等待任务。
|
|||
|
|
|
|||
|
|
添加一个抽象 `CoarsePathScenarioMovementTest`，其 `Test` 调用 `CoarsePathMovementTestRunner.Start(DisplayName, CoarsePathScenarioFactory.Create(Scenario))`，其 `TestStop` 调用 `Stop()`。实现六个带固定属性名称的密封子类。第七个类在 `Test` 中只读取一次六个 UI 输入：AMR 起点 X/Y/航向和目标 X/Y/航向（分别为 mm/mm/deg），调用 `CreateManualGoalDemo` 后启动；输入解析失败时仅记录错误并不启动任务。
|
|||
|
|
|
|||
|
|
- [ ] **Step 4: 实现确定的地图和结果绘制辅助方法**
|
|||
|
|
|
|||
|
|
在同一执行器内只消费 `job` 与 `CoarsePathPlanningJobResult`，按固定顺序调用以下辅助方法：
|
|||
|
|
|
|||
|
|
```csharp
|
|||
|
|
private static void DrawMap(PlanningGridMap map);
|
|||
|
|
private static void DrawPose(Color color, string label, Pose2D pose);
|
|||
|
|
private static void DrawGoalTolerance(Pose2D goal, HybridAStarConfiguration configuration);
|
|||
|
|
private static void DrawSuccessfulPath(PlanningResult result, VehicleParameters vehicle);
|
|||
|
|
private static void DrawLegendAndStatus(string displayName, CoarsePathPlanningJobResult result, int gridStride);
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
`DrawMap` 画 `[Bounds.XMin, Bounds.XMax) × [Bounds.YMin, Bounds.YMax)` 的粗边界和 X/Y 参考。`gridStride = Max(1, Ceiling(Max(Rows, Cols) / 100d))`；每 `gridStride` 个真实栅格画一条线，状态文字写入 `分辨率=...mm，显示每...格`。遍历 `row/col`，仅对 `map.IsOccupied(row,col)` 为 true 的单元以四条边线画深色格框，确保显示的是最终快照而非原始几何。
|
|||
|
|
|
|||
|
|
`DrawPose` 将 m 转 mm，以圆、朝向短线和标签分别绘制绿色起点、橙色终点。`DrawGoalTolerance` 将位置容差 m 转 mm，绘制橙色容差圆。`DrawSuccessfulPath` 仅在 `result.Status == PlanningStatus.Success` 时运行：相邻路径点按当前点 `Direction` 使用青色（前进）或蓝色（倒车）连线；每隔 10 点画短箭头；`IsGearSwitchPoint` 画紫色圆与“换向”；首、末、换向和每 20 点调用旋转矩形绘制，半长/半宽严格按车辆长宽加安全余量。`DrawLegendAndStatus` 在边界左上方显示边界、占据格、起点、终点、前进、倒车、换向和扩大车体颜色说明，另显示 `SnapshotId`、`MapResult.Status`、`CacheHit`、`PlanningResult.Status`、`Elapsed` 和终止原因。
|
|||
|
|
|
|||
|
|
- [ ] **Step 5: 运行构建与集成脚本**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 构建成功；工厂行为、七入口结构、后台取消约束和 Painter 数据来源检查全部通过。
|
|||
|
|
|
|||
|
|
### Task 4: README 与文档断言
|
|||
|
|
|
|||
|
|
**Files:**
|
|||
|
|
|
|||
|
|
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
|
|||
|
|
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
|||
|
|
|
|||
|
|
**Consumes:** Task 2 的 `CreateManualGoalDemo` 单位契约和 Task 3 的七个入口名称、图例颜色和停止行为。
|
|||
|
|
|
|||
|
|
**Produces:** 可独立使用的 P1 UI 说明，以及对其关键安全声明的自动化保护。
|
|||
|
|
|
|||
|
|
- [ ] **Step 1: 为 README 写失败断言**
|
|||
|
|
|
|||
|
|
在现有 README 检查后加入：
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
foreach ($requiredReadmeText in @(
|
|||
|
|
'## P1：Clumsy 手动测试与可视化',
|
|||
|
|
'粗路径-AMR起点手动终点(空图演示)',
|
|||
|
|
'AMR 位姿输入：X/Y 使用世界 mm，th 使用 deg',
|
|||
|
|
'Pose2D：X/Y 使用 m，航向使用 rad',
|
|||
|
|
'显式空图只能用于演示',
|
|||
|
|
'不会发送底盘运动命令',
|
|||
|
|
'TestStop',
|
|||
|
|
'栅格边界',
|
|||
|
|
'占据格',
|
|||
|
|
'换向')) {
|
|||
|
|
Assert-True $coarsePathReadmeContent.Contains($requiredReadmeText) "CoarsePath README must document: $requiredReadmeText"
|
|||
|
|
}
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
- [ ] **Step 2: 运行脚本确认 README 检查失败**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 所有代码检查通过；脚本因 `CoarsePath README must document: ## P1：Clumsy 手动测试与可视化` 失败。
|
|||
|
|
|
|||
|
|
- [ ] **Step 3: 在 README 增加 P1 专节**
|
|||
|
|
|
|||
|
|
在“第一版限制”之前增加 `## P1：Clumsy 手动测试与可视化`，逐项写明：
|
|||
|
|
|
|||
|
|
1. 七个 MovementTest 名称及对应场景；缓存测试连续运行两次，第二次展示 `Input` 命中。
|
|||
|
|
2. AMR/手动目标输入契约：世界 `X/Y(mm)`、`th(deg)`，转换成 `Pose2D` 的 m/rad；起点是车辆几何中心。
|
|||
|
|
3. 空图手动目标入口只能演示坐标、路径和取消流程；现场必须提供真实障碍物快照。
|
|||
|
|
4. 可视化图例：边界、抽稀格线、占据格、起点、终点及容差、前进、倒车、换向和扩大车体检查框；失败不显示部分路径。
|
|||
|
|
5. `Test` 在后台规划，`TestStop` 取消令牌并清空图层；测试只显示结果，绝不发送底盘运动命令或执行路径跟踪。
|
|||
|
|
|
|||
|
|
- [ ] **Step 4: 运行 README 与集成检查**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```powershell
|
|||
|
|
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
|||
|
|
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
|||
|
|
```
|
|||
|
|
|
|||
|
|
Expected: 构建成功；README 和全部 P1 集成检查通过。
|
|||
|
|
|
|||
|
|
### Task 5: 全量回归与手动核验说明
|
|||
|
|
|
|||
|
|
**Files:**
|
|||
|
|
|
|||
|
|
- Verify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs`
|
|||
|
|
- Verify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs`
|
|||
|
|
- Verify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
|
|||
|
|
- Verify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
|
|||
|
|
|
|||
|
|
**Consumes:** Tasks 1–4 的代码、文档和脚本。
|
|||
|
|
|
|||
|
|
**Produces:** 通过 Debug 回归的 P1 UI 集成首个交付；不进入 Release 性能基准。
|
|||
|
|
|
|||
|
|
- [ ] **Step 1: 执行 Debug 构建和所有现存 P0/P1 功能脚本**
|
|||
|
|
|
|||
|
|
Run:
|
|||
|
|
|
|||
|
|
```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
|
|||
|
|
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 errors；每个存在的脚本返回 0 并输出其 `passed` 消息。
|
|||
|
|
|
|||
|
|
- [ ] **Step 2: 手动 Clumsy 验收**
|
|||
|
|
|
|||
|
|
在 Clumsy 的 MovementTest 列表依次运行“粗路径-矩形绕行”和“粗路径-AMR起点手动终点(空图演示)”。检查：测试启动后界面仍可操作；图层拥有边界、格线、占据格、图例、起终点与状态；成功案例有方向区分路径和扩大车体框；点击停止后图层清空且没有任何底盘运动命令。
|
|||
|
|
|
|||
|
|
- [ ] **Step 3: 记录交付边界**
|
|||
|
|
|
|||
|
|
在完成报告中明确：P1 UI 集成已完成；下一 P1 子项目是 Release 性能、资源和确定性基准；TrapMap 迁移/清理继续排除；没有执行 Git 操作。
|
|||
|
|
|
|||
|
|
## 自检
|
|||
|
|
|
|||
|
|
- 覆盖性：Task 2 交付纯场景与单位转换；Task 3 交付后台七入口和完整视觉要素；Task 4 交付 README；Task 5 交付自动化与手动验收。规格中的空图限制、实际占据快照、停止语义、无底盘命令和不显示部分路径均有对应任务。
|
|||
|
|
- 占位符：已检查任务不含未决占位、延后实现或泛化错误处理类措辞；每个实现任务均给出文件、接口、测试、命令和具体代码形状。
|
|||
|
|
- 类型一致性：所有任务统一使用 `CoarsePathTestScenario`、`CoarsePathScenarioFactory.Create`、`CreateManualGoalDemo`、`CoarsePathPlanningJob`、`CoarsePathPlanningJobResult` 和 `PlanningGridMap`；AMR 输入始终是 mm+deg，核心位姿始终是 m+rad。
|