# P1 手动障碍物输入 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:** 让“粗路径规划”MovementTest 支持一次输入最多 20 个圆形或轴对齐矩形障碍物，并通过既有门面规划、快照绘制和取消流程验证结果。

**Architecture:** 纯几何输入和 Map 请求构造保留在 `CoarsePathScenarioFactory`，UI 只读取、验证和冻结操作者输入。每次含障碍物的手动运行由共享执行器颁发单调递增快照版本，保证 Map 缓存不会错误复用旧障碍物；Painter 继续只读取最终 `PlanningGridMap`。

**Tech Stack:** C# / `netstandard2.0`、现有 `ManualObstacleSource`、Clumsy `MovementTest`/`UI.GetInput`、PowerShell 反射与源码验证脚本。

## Global Constraints

- 不改变 `CoarsePathPlanningService.Plan(job, token)` 作为唯一业务规划入口的边界；UI 不得直接创建地图工厂、搜索器、碰撞器或原语。
- 手动输入的 X/Y、圆半径和矩形长宽全部使用世界 mm；AMR/目标航向输入使用 deg；核心 `Pose2D` 使用 m/rad。
- 障碍物数量范围固定为 0–20；圆半径、矩形 X 长度和 Y 宽度必须是有限正数；矩形始终与世界坐标轴平行。
- 有障碍物时使用必需的 `ManualObstacleSource("manual-user-input", version, true, ...)` 且关闭显式空图；零障碍物时才允许显式空图。
- 手动地图边界必须覆盖起点、终点及每个障碍物完整外轮廓，再保留 2000 mm 留白并按 50 mm 向外取整。
- 含障碍物手动提交必须使用单调递增快照版本；固定场景的缓存命中行为不得改变。
- 保留后台 `Task.Run`、`CancellationTokenSource`、`TestStop`、结果快照绘制和无底盘命令边界。
- 不支持旋转矩形、多边形、文件导入、拖拽编辑或运行中修改障碍物；不执行 Git 操作。

---

## 文件结构

| 文件 | 修改职责 |
| --- | --- |
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs` | 新增手动障碍物纯数据类型、工厂方法、几何校验、动态边界和来源快照构造。 |
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs` | 为“粗路径规划”读取数量、类型、中心和尺寸，生成单调来源版本并提交工厂请求。 |
| `ClumsyPilot/tests/verify_coarse_path_integration.ps1` | 通过程序集反射验证工厂、障碍来源、空图分支、几何边界和无效尺寸。 |
| `ClumsyPilot/tests/verify_coarse_path_ui.ps1` | 验证 UI 入口包含手动障碍物输入与工厂调用，同时保持无直接地图/搜索依赖。 |
| `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md` | 补充手动障碍物的输入顺序、单位、上限、矩形方向和空图限制。 |

### Task 1: 手动障碍物工厂契约与行为验证

**Files:**

- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`
- Modify later: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/CoarsePathScenarioFactory.cs`

**Consumes:** 现有 `$assembly`、`$testNamespace`、`$scenarioFactoryType`、`Find-Method`、`Assert-True`、`Assert-Equal`、`Assert-Near`。

**Produces:** `ManualCoarsePathObstacleKind`、`ManualCoarsePathObstacle` 和 `CreateManualObstacleDemo` 的反射/行为契约。

- [ ] **Step 1: 在 P1 工厂断言后加入失败的手动障碍物检查**

在 `$manualJob` 的现有断言之后插入下面代码。它使用数组传入 `IReadOnlyList<ManualCoarsePathObstacle>`，并检查请求尚未存在时的类型/方法失败。

```powershell
$manualObstacleKindType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacleKind', $false)
$manualObstacleType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacle', $false)
Assert-True ($manualObstacleKindType -ne $null) 'Manual obstacle kind enum must exist.'
Assert-True ($manualObstacleType -ne $null) 'Manual obstacle value type must exist.'

$manualCircle = Find-Method $manualObstacleType 'Circle' @([double], [double], [double])
$manualRectangle = Find-Method $manualObstacleType 'AxisAlignedRectangle' @([double], [double], [double], [double])
$manualObstacleFactory = $scenarioFactoryType.GetMethods() | Where-Object {
    $_.Name -eq 'CreateManualObstacleDemo' -and $_.GetParameters().Length -eq 8
} | Select-Object -First 1
Assert-True ($manualCircle -ne $null) 'Manual obstacle type must create circles from center and radius.'
Assert-True ($manualRectangle -ne $null) 'Manual obstacle type must create rectangles from center and X/Y dimensions.'
Assert-True ($manualObstacleFactory -ne $null) 'Scenario factory must expose CreateManualObstacleDemo with six poses, obstacles and version.'

$manualObstacles = [Array]::CreateInstance($manualObstacleType, 2)
$manualObstacles.SetValue($manualCircle.Invoke($null, @([double]6500, [double]2000, [double]200)), 0)
$manualObstacles.SetValue($manualRectangle.Invoke($null, @([double]-2000, [double]500, [double]600, [double]400)), 1)
$manualObstacleJob = $manualObstacleFactory.Invoke($null, @(
    1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $manualObstacles, [long]77))
Assert-False $manualObstacleJob.MapRequest.AllowExplicitEmptyMap 'Manual obstacles must disable the explicit-empty-map mode.'
Assert-Equal 1 $manualObstacleJob.MapRequest.ObstacleSources.Count 'Manual obstacles must create one unified source.'
Assert-Equal 'manual-user-input' $manualObstacleJob.MapRequest.ObstacleSources[0].SourceId 'Manual source ID must be stable.'
Assert-Equal 77 $manualObstacleJob.MapRequest.ObstacleSources[0].SourceVersion 'Manual source version must be preserved.'
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMin -le -4300.0) 'Manual map must include the rectangle outline and padding.'
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMax -ge 8700.0) 'Manual map must include the circle outline and padding.'

$emptyManualObstacles = [Array]::CreateInstance($manualObstacleType, 0)
$emptyManualJob = $manualObstacleFactory.Invoke($null, @(
    1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $emptyManualObstacles, [long]0))
Assert-True $emptyManualJob.MapRequest.AllowExplicitEmptyMap 'Zero manual obstacles must retain explicit empty-map mode.'
Assert-Equal 0 $emptyManualJob.MapRequest.ObstacleSources.Count 'Zero manual obstacles must not create a fake source.'

try {
    $null = $manualCircle.Invoke($null, @([double]1000, [double]1000, [double]0))
    throw 'Zero-radius manual circle must be rejected.'
}
catch [Reflection.TargetInvocationException] {
    Assert-True ($_.Exception.InnerException -is [ArgumentOutOfRangeException]) 'Invalid manual geometry must report argument range.'
}
```

- [ ] **Step 2: 构建并运行脚本确认新契约失败**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```

Expected: 构建成功；脚本报出 `Manual obstacle kind enum must exist.`。

- [ ] **Step 3: 在场景工厂实现不可变手动障碍物类型**

在 `CoarsePathScenarioFactory.cs` 的固定场景枚举之后加入如下公共类型。构造函数保持私有，强制圆形与矩形分别通过语义明确的静态工厂创建；所有几何输入均为 mm。

```csharp
/// <summary>手动障碍物的支持几何类型。</summary>
public enum ManualCoarsePathObstacleKind
{
    /// <summary>由圆心和半径定义的圆形障碍物。</summary>
    Circle,
    /// <summary>由几何中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
    AxisAlignedRectangle,
}

/// <summary>手动粗路径测试的不可变障碍物输入；全部几何数据使用世界 mm。</summary>
public sealed class ManualCoarsePathObstacle
{
    private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
        double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
    {
        Kind = kind; CenterXMillimeters = centerXMillimeters; CenterYMillimeters = centerYMillimeters;
        SizeXMillimeters = sizeXMillimeters; SizeYMillimeters = sizeYMillimeters;
    }

    public ManualCoarsePathObstacleKind Kind { get; }
    public double CenterXMillimeters { get; }
    public double CenterYMillimeters { get; }
    public double SizeXMillimeters { get; }
    public double SizeYMillimeters { get; }

    public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
        double radiusMillimeters)
    {
        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
            centerYMillimeters, radiusMillimeters, radiusMillimeters);
    }

    public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
        double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
    {
        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
        EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
        EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
            centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
    }
}
```

将 `EnsureFinite` 与新增 `EnsurePositiveFinite` 定义为可被同一命名空间类型调用的内部静态校验辅助方法，或在 `ManualCoarsePathObstacle` 中实现等价私有辅助方法；无效值必须抛出 `ArgumentOutOfRangeException`。

- [ ] **Step 4: 实现手动障碍物请求和动态边界**

在 `CoarsePathScenarioFactory` 加入下面公共方法，并让现有 `CreateManualGoalDemo` 调用它的零障碍物分支，以保留当前空图契约：

```csharp
public static CoarsePathPlanningJob CreateManualObstacleDemo(
    double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
    double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
    IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
{
    ValidateManualPoseInputs(startXMillimeters, startYMillimeters, startHeadingDegrees,
        goalXMillimeters, goalYMillimeters, goalHeadingDegrees);
    IReadOnlyList<ManualCoarsePathObstacle> items = obstacles ??
        throw new ArgumentNullException(nameof(obstacles));
    if (items.Count > MaximumManualObstacleCount)
        throw new ArgumentOutOfRangeException(nameof(obstacles));
    if (items.Count == 0)
        return CreateJob(CreateManualDemoMap(startXMillimeters, startYMillimeters,
                goalXMillimeters, goalYMillimeters, Array.Empty<ManualCoarsePathObstacle>()),
            ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
            ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees), null, GoalDirectionConstraint.Any);
    if (obstacleSnapshotVersion <= 0)
        throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion));

    IMapObstacle[] mapObstacles = ConvertManualObstacles(items);
    IMapObstacleSource[] sources =
    {
        new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true, mapObstacles),
    };
    return CreateJob(CreateManualDemoMap(startXMillimeters, startYMillimeters,
            goalXMillimeters, goalYMillimeters, items),
        ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
        ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees), null, GoalDirectionConstraint.Any);
}
```

Use `CreateManualMapRequest(items, sources)` rather than leaving the above source array unused: it must create a `PlanningMapRequest` with the dynamic bounds, `ResolutionMm = 50f`, those sources and `AllowExplicitEmptyMap = false`. `ConvertManualObstacles` must map a circle to `new CircleObstacle(centerX, centerY, radius)` and a rectangle to `new AxisAlignedRectangleObstacle(centerX - lengthX / 2, centerX + lengthX / 2, centerY - widthY / 2, centerY + widthY / 2)` after range-safe float conversion.

Refactor `CreateManualDemoMap` to accept an obstacle collection and include its circle/rectangle extents before adding 2000 mm padding and applying `ToGridLowerBound`/`ToGridUpperBound`. Zero obstacles must retain `Array.Empty<IMapObstacleSource>()` and `AllowExplicitEmptyMap = true`.

- [ ] **Step 5: 运行工厂行为检查确认通过**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```

Expected: 构建成功；输出既有三行集成通过信息，且手动圆/矩形、空障碍物和无效半径断言均通过。

### Task 2: MovementTest 逐项输入与来源版本

**Files:**

- Modify: `ClumsyPilot/tests/verify_coarse_path_ui.ps1`
- Modify later: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs`

**Consumes:** Task 1 的 `ManualCoarsePathObstacle.Circle`、`ManualCoarsePathObstacle.AxisAlignedRectangle` 与 `CoarsePathScenarioFactory.CreateManualObstacleDemo`。

**Produces:** `CoarsePathPlanningTest` 在启动规划前读取并冻结最多 20 个手动障碍物，随后使用递增版本提交给工厂。

- [ ] **Step 1: 加入失败的 UI 源码边界断言**

在现有手动工厂断言后加入：

```powershell
Assert-Match $source 'ManualCoarsePathObstacle' 'The manual UI must construct typed manual obstacles.'
Assert-Match $source 'CreateManualObstacleDemo\s*\(' 'The manual UI must submit obstacles through the factory.'
Assert-Match $source 'MaximumManualObstacleCount\s*=\s*20' 'The manual UI must bound obstacle input to 20.'
Assert-Match $source 'ReadManualObstacles\s*\(' 'The manual UI must read the requested obstacle sequence.'
Assert-Match $source 'Interlocked\.Increment\s*\(' 'The manual UI must issue a fresh obstacle snapshot version.'
Assert-Match $source 'Circle\s*\(' 'The manual UI must support circle input.'
Assert-Match $source 'AxisAlignedRectangle\s*\(' 'The manual UI must support axis-aligned rectangle input.'
```

- [ ] **Step 2: 运行 UI 脚本确认新断言失败**

Run:

```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
```

Expected: `The manual UI must construct typed manual obstacles.`。

- [ ] **Step 3: 实现输入辅助方法与提交逻辑**

在 `CoarsePathPlanningTest` 中新增：

```csharp
private const int MaximumManualObstacleCount = 20;
private static long _nextManualObstacleSnapshotVersion;

private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
{
    int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
    var obstacles = new List<ManualCoarsePathObstacle>(count);
    for (int index = 0; index < count; index++)
    {
        int kind = ReadBoundedIntegerInput("障碍物 " + (index + 1) + " 类型（1圆形，2矩形）", 1, 2);
        double centerX = ReadFiniteInput("障碍物 " + (index + 1) + " 中心 X（世界 mm）");
        double centerY = ReadFiniteInput("障碍物 " + (index + 1) + " 中心 Y（世界 mm）");
        if (kind == 1)
        {
            double radius = ReadPositiveFiniteInput("障碍物 " + (index + 1) + " 半径 r（mm）");
            obstacles.Add(ManualCoarsePathObstacle.Circle(centerX, centerY, radius));
        }
        else
        {
            double lengthX = ReadPositiveFiniteInput("障碍物 " + (index + 1) + " X方向长度（mm）");
            double widthY = ReadPositiveFiniteInput("障碍物 " + (index + 1) + " Y方向宽度（mm）");
            obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerX, centerY, lengthX, widthY));
        }
    }
    return obstacles;
}
```

`ReadBoundedIntegerInput` 复用 `UI.GetInput` 和当前文化/InvariantCulture 解析，拒绝非整数或超出 `[minimum, maximum]` 的输入；`ReadPositiveFiniteInput` 在 `ReadFiniteInput` 返回后拒绝 `<= 0d`。所有失败继续由现有 `ShowInputFailure` 显示。

在 `Test()` 中的终点读取后调用 `ReadManualObstacles()`。当集合非空时，用 `Interlocked.Increment(ref _nextManualObstacleSnapshotVersion)` 取得版本；集合为空时使用 `0L`。随后替换现有工厂调用：

```csharp
IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
long snapshotVersion = obstacles.Count == 0 ? 0L :
    Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
    amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
    obstacles, snapshotVersion);
CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
```

保留 `TestStop`、`Run`、Painter 和底盘禁止边界，不在 UI 内构造 `ManualObstacleSource`、`PlanningMapRequest` 或搜索对象。

- [ ] **Step 4: 运行 UI 结构检查确认通过**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
```

Expected: `Coarse path P1 UI source checks passed.`

### Task 3: README 输入说明与最终回归

**Files:**

- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md`
- Modify: `ClumsyPilot/tests/verify_coarse_path_ui.ps1`
- Verify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1`

**Consumes:** Tasks 1–2 的工厂与 UI 输入契约。

**Produces:** README 中与实际输入顺序一致的手动障碍物说明，以及最终的构建、UI 和集成证据。

- [ ] **Step 1: 为 README 增加失败的 ASCII 文档断言**

在 `$readmeStructure` 的数组中加入：

```powershell
'CreateManualObstacleDemo',
'ManualCoarsePathObstacle',
'manual-user-input',
'0-20',
'AxisAlignedRectangle',
```

- [ ] **Step 2: 运行 UI 脚本确认 README 断言失败**

Run:

```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
```

Expected: `Restructured CoarsePath README must document CreateManualObstacleDemo.`

- [ ] **Step 3: 更新 README 的 P1 手动测试段落**

在 `## P1 手动测试与可视化（P1 Manual Tests and Visualization）` 的“AMR 位姿与手动终点”小节中，替换“空图入口”的单一说明，加入以下事实：

1. 目标输入之后先输入 `0-20` 的障碍物数量；
2. 每项输入 `1` 圆形或 `2` 矩形、中心 X/Y(mm)，圆形半径或矩形 X 长度/Y 宽度(mm)；
3. 矩形是 `AxisAlignedRectangle`，不支持旋转；尺寸必须为正；
4. `CreateManualObstacleDemo` 将它们包装为 `manual-user-input` 快照，有障碍物时关闭显式空图；
5. 零障碍物才是坐标/取消演示的显式空图；真实作业仍必须提供真实障碍物来源；
6. 地图边界自动覆盖起终点和障碍物完整外轮廓，保留 2000 mm 留白并按 50 mm 对齐；
7. 每次含障碍物提交使用新版本，Painter 仍显示最终 `PlanningGridMap` 占据格而不是原始几何。

- [ ] **Step 4: 运行完整验证**

Run:

```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
```

Expected: 构建为 `0 个错误`；UI 脚本输出 `Coarse path P1 UI source checks passed.`；集成脚本依次输出既有三行 `passed` 消息。

## 自检

- **规格覆盖：** Task 1 覆盖几何类型、非空/空地图、动态边界、版本和无效尺寸；Task 2 覆盖 0–20 输入、形状输入、版本和后台门面边界；Task 3 覆盖 README 与回归。
- **完整性检查：** 每个实现步骤指定了文件、调用签名、验证规则和命令；不引入未命名接口或外部依赖。
- **一致性：** `ManualCoarsePathObstacle`、`CreateManualObstacleDemo`、`manual-user-input` 与 `obstacleSnapshotVersion` 在所有任务中使用相同名称和单位定义。
