Files

T

梁薄云 2f4fd15e52 chore: save current workspace progress

2026-08-09 22:13:18 +08:00

20 KiB

Raw Blame History

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>，并检查请求尚未存在时的类型/方法失败。

$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:

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。

/// <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 调用它的零障碍物分支，以保留当前空图契约：

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:

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 源码边界断言

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

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 -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1

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

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

在 CoarsePathPlanningTest 中新增：

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。随后替换现有工厂调用：

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:

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 的数组中加入：

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

Step 2: 运行 UI 脚本确认 README 断言失败

Run:

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 位姿与手动终点”小节中，替换“空图入口”的单一说明，加入以下事实：

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

Step 4: 运行完整验证

Run:

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 在所有任务中使用相同名称和单位定义。

20 KiB Raw Blame History Unescape Escape

P1 手动障碍物输入 Implementation Plan

Global Constraints

文件结构

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

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

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

自检

20 KiB

Raw Blame History