First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
2f4fd15e5290351c32d3f2754e29dea225e1d121
ParkingRobot/.task8-sweep/ParkrobTrajplanner/Initial_plan/AMR_HybridAStar_粗路径实施计划.md
T

43 KiB
Raw Blame History

Hybrid A* 粗路径规划实施计划

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development（推荐）或 superpowers:executing-plans，按任务顺序实施，并使用 - [ ] 更新执行状态。

目标： 在 ClumsyPilot netstandard2.0 项目中交付可复用的静态规划地图和 Hybrid A* 粗路径规划模块，通过一次 CoarsePathPlanningService.Plan(job) 完成建图、快照复用、粗路径搜索和可选调试发布。

架构： Map 模块统一接收人工障碍、TwoLeg 及未来障碍来源，生成只含外部障碍物的不可变 PlanningGridMap；HybridAStarPlanner 只消费该快照并完成保守碰撞检查与搜索；CoarsePathPlanningService 负责编排。核心逻辑不读取传感器、UI 或系统时间，Clumsy MovementTest 和 PNG 导出只作为旁路适配器。

技术栈： C# 10、.NET Standard 2.0、PowerShell 反射契约测试、Clumsy MovementTest、StbImageWriteSharp 1.16.7。

设计依据： docs/superpowers/specs/2026-07-26-hybrid-astar-coarse-path-design.md

全局约束

  • 所有命令均从仓库根目录执行；项目文件为 ClumsyPilot/ClumsyPilot.csproj。
  • 目标框架保持 netstandard2.0，不得直接使用 PriorityQueue、Math.Clamp、double.IsFinite 或依赖 record/init 的实现。
  • 地图边界、人工障碍和 TwoLeg 快照使用世界坐标 mm；规划内部统一使用 m、rad、1/m。
  • 地图只保存外部障碍物，不写入 AMR 自身足迹，不在地图侧添加车辆安全膨胀。
  • 安全余量只在碰撞检查时扩大车体矩形，防止地图膨胀与车体膨胀重复计算。
  • ResolutionMm 必须在 20～200 mm；地图最多 4,000,000 格，分配前使用 checked 检查。
  • 地图坐标采用 [XMin, XMax) × [YMin, YMax)；地图外始终按占据处理。
  • PrimitiveLengthMeters=0.50 表示原语最大长度；IntegrationStepMeters=0.05 表示积分最大步长。
  • 碰撞采样中心位移不得超过 min(0.025 m, Map.ResolutionMeters / 2)。
  • 默认终点容差为 0.15 m 和 5°。每个原语在内部积分点逐点检查目标，第一次满足条件即截断为终点候选。
  • 生成终点候选时不能立即成功；候选必须进入 Open List，作为当前最佳有效条目出队时才成功。
  • 第一版只支持恒曲率前进、倒车及原语边界换向；不支持蟹行、横移、原地旋转、Reeds-Shepp 精确连接、平滑、速度规划或底盘控制。
  • PowerShell 脚本首行设置 $ErrorActionPreference = 'Stop'，统一加载 bin/Debug/netstandard2.0/ClumsyPilot.dll。
  • 用户已明确不需要 Git 自检。本计划不包含 git diff、git add、git commit 等步骤；版本管理由用户另行处理。

P0 与 P1 完成定义

阶段 必须完成的结果 阶段出口
P0-MAP：当前首要工作 Map 文件结构、统一障碍来源、TwoLeg 投影、栅格化、距离场、快照缓存、PNG 迁移、自动测试和 MovementTest.MapTest 实际调用 Map 独立构建成功；3 个 Map 脚本通过；Clumsy MapTest 只通过 PlanningMapFactory.Create 完成建图与显示
P0-PLAN：Map Gate 之后 CoarsePath 契约、保守碰撞、原语截断、Open List、Hybrid A*、输出校验和一次调用门面 6 个非 UI P0 脚本全部通过；固定场景可由 CoarsePathPlanningService 返回经最终复核的粗路径
P1：集成与优化 CoarsePath MovementTest、性能基准、旧地图退役和 README 7 个功能脚本及 Release 基准通过；UI 入口只做调试展示；旧地图类型不再成为规划运行时入口

执行顺序固定为“P0-MAP 独立闭环 → P0-PLAN → P1”。除了 Map 自己的 MovementTest.MapTest 和 PNG 调试辅助外，在 Map Gate 通过前不编写粗路径搜索或 CoarsePath UI，避免旧 MovementTest.Trapmaptest.cs 的传感器和渲染依赖进入核心模块。

当前首要里程碑：P0-MAP 独立闭环

当前阶段只处理 Utils 中被 Map 使用的无状态工具、完整 Map 目录、Map 自动化测试和 Map 实际调用。不得提前创建 CoarsePath/Search、CoarsePath/Vehicle 或 CoarsePath/Output 运行时代码。

Map 实施顺序

  1. 建立 Map/Core、Map/Obstacles、Map/Sources、Map/Planning、Map/Test/Visualization 目录和命名空间。
  2. 完成 MapBoundsMm、连续行优先 EnvironmentGridMap、圆/矩形 DTO 和唯一栅格化器。
  3. 完成统一 IMapObstacleSource、人工来源、TwoLeg 快照来源和事务式 EnvironmentMapBuilder。
  4. 完成 PlanningGridMap、精确 EDT、保守距离场和 mm→m 适配。
  5. 完成 PlanningMapFactory、容量 4 的两级快照缓存和地图变化判断。
  6. 将旧 TrapMapImageExporter.cs 能力拆到 Map/Test/Visualization，只消费只读快照。
  7. 完成三个 Map PowerShell 脚本和 MovementTest.MapTest，用真实入口验证建图、复用和显示。

对应详细任务的执行次序为：Task 1 → Task 3 → Task 4 → Task 5 → Task 6 → Task 13 → Task 14 的 MapTest 部分 → Map Gate。Task 2 和 Task 7～12 在 Map Gate 之后执行。

Map 实际调用契约

PlanningMapFactory 应由长期存在的服务或测试对象持有，不能每次调用都重新 new，否则容量 4 的快照缓存无法跨调用复用：

private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();

public PlanningMapBuildResult CreateCurrentMap(
    MapBoundsMm bounds,
    long manualVersion,
    IReadOnlyList<IMapObstacle> manualObstacles,
    long twoLegVersion,
    TwoLegProjectionInput twoLegSnapshot)
{
    return _mapFactory.Create(new PlanningMapRequest
    {
        Bounds = bounds,
        ResolutionMm = 50f,
        ObstacleSources = new IMapObstacleSource[]
        {
            new ManualObstacleSource(
                "manual", manualVersion, true, manualObstacles),
            new TwoLegObstacleSource(
                "two-leg", twoLegVersion, false, twoLegSnapshot),
        },
        AllowExplicitEmptyMap = false,
    });
}

调用方只判断统一结果，不接触 builder、rasterizer、投影器或 adapter：

PlanningMapBuildResult result = CreateCurrentMap(
    bounds, manualVersion, manualObstacles, twoLegVersion, twoLegSnapshot);

if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
    return;

PlanningGridMap map = result.Map;
bool worldOriginIsOccupied = map.IsOccupiedWorld(0d, 0d);
double worldOriginClearance =
    map.GetConservativeObstacleDistanceMeters(0d, 0d);

Map Gate

以下条件必须全部满足，才开始 CoarsePath 契约和搜索：

  • 新 Map 运行时代码不读取 TwoLegDetect、定位、Painter、Toast、UI 或系统时间。
  • 人工、TwoLeg 以及未来来源全部经 IMapObstacleSource.ProjectToWorld() 输出世界 mm 几何，再由唯一 rasterizer 写图。
  • 地图不写 AMR 自身，不使用旧默认 300 mm 膨胀。
  • PlanningMapFactory.Create 对完整输入命中、占据命中和占据变化给出可区分结果。
  • MovementTest.MapTest 只调用 PlanningMapFactory 和可选 PNG exporter，不复制地图构造逻辑。
  • 下列命令全部通过：
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

P0-MAP 执行状态（2026-07-27）

  • 已完成 Task 1、Task 3～6、Task 13，以及 Task 14 的 MovementTest.MapTest 部分。
  • 已建立新的 Map 目录结构、统一障碍物来源与 TwoLeg 检测时位姿投影；Map 核心不读取传感器、UI 或系统时钟。
  • 已完成只读规划快照、保守 EDT 距离场、容量 4 的 LRU 缓存、PNG 可视化拆分和实际 MapTest 调用。
  • 已通过上述 Map Gate 命令；另外 verify_planning_utils.ps1 也已通过。
  • 未执行 Git 自检、暂存或提交。

最终目录与文件职责

ClumsyPilot/ParkrobTrajplanner/
├── Initial_plan/                         ------ 方案与实施文档，不放运行时代码
├── Utils/                                ------ 通用、无状态、确定性数值工具
│   ├── AngleMath.cs                      ------ 角度归一化、最短角差和航向离散索引
│   ├── UnitConverter.cs                  ------ mm/m、deg/rad 和半径/曲率转换
│   ├── CoordinateTransform.cs            ------ 车体坐标与世界坐标二维刚体变换
│   ├── NumericGuard.cs                   ------ 有限值、正值和参数范围校验
│   └── GridIndex.cs                      ------ 不可变行列索引值对象
├── Map/                                  ------ MultiWheelC.TrajectoryPlanning.Mapping
│   ├── Core/
│   │   ├── EnvironmentGridMap.cs         ------ 只保存外部障碍物的 mm 占据栅格
│   │   ├── MapBoundsMm.cs                ------ 有限、非退化的 mm 地图边界
│   │   ├── MapBuildRequest.cs            ------ 内部环境图构建请求
│   │   ├── EnvironmentMapBuildResult.cs  ------ 环境图、来源摘要和失败原因
│   │   └── EnvironmentMapBuilder.cs      ------ 校验来源并事务式合并障碍图层
│   ├── Obstacles/
│   │   ├── IMapObstacle.cs               ------ 世界坐标障碍物公共几何契约
│   │   ├── AxisAlignedRectangleObstacle.cs ------ 轴对齐矩形障碍 DTO
│   │   ├── CircleObstacle.cs             ------ 圆形障碍 DTO
│   │   └── MapObstacleRasterizer.cs      ------ 唯一占据栅格写入器
│   ├── Sources/
│   │   ├── IMapObstacleSource.cs         ------ 纯快照障碍来源统一接口
│   │   ├── ObstacleSourceStatus.cs       ------ Applied/Empty/Unavailable/Invalid
│   │   ├── ObstacleProjectionResult.cs   ------ 来源版本、状态、诊断和障碍集合
│   │   ├── ManualObstacleSource.cs       ------ 输出人工圆和矩形
│   │   ├── TwoLegProjectionInput.cs      ------ 两腿端点、检测状态和检测位姿快照
│   │   ├── TwoLegObstacleSource.cs       ------ TwoLeg 统一来源适配器
│   │   └── TwoLegObstacleProjector.cs    ------ 车体系两腿端点投影到世界系
│   ├── Planning/
│   │   ├── PlanningGridMap.cs            ------ 只读 m 占据图、距离场和快照元数据
│   │   ├── EuclideanDistanceTransform.cs ------ 线性时间精确二维欧氏距离变换
│   │   ├── ObstacleDistanceField.cs      ------ 不高估真实净空的距离下界
│   │   ├── PlanningMapCache.cs           ------ 容量 4 的线程安全两级快照缓存
│   │   └── PlanningMapAdapter.cs         ------ 占据深拷贝、mm→m 和距离场生成
│   ├── PlanningMapRequest.cs             ------ Map 模块统一输入
│   ├── PlanningMapBuildResult.cs         ------ Map 模块统一输出和缓存命中类型
│   ├── PlanningMapFactory.cs             ------ Map 模块唯一公共创建入口
│   └── Test/
│       ├── MovementTest.MapTest.cs       ------ Clumsy UI 地图构建/显示入口
│       └── Visualization/
│           ├── PlanningMapImageExportRequest.cs ------ 快照、叠加层和输出选项
│           ├── PlanningMapImageExportResult.cs ------ PNG 状态、路径、尺寸和诊断
│           ├── PlanningMapImageExporter.cs ------ 校验、渲染编排和原子发布
│           ├── PlanningMapImageRenderer.cs ------ 地图/车辆/路径绘制到 RGBA
│           └── ValidatedPngWriter.cs     ------ Stb 编码、PNG 结构和 CRC 校验
├── CoarsePath/                           ------ MultiWheelC.TrajectoryPlanning.CoarsePath
│   ├── Contracts/
│   │   ├── Pose2D.cs                     ------ m/rad 不可变二维位姿
│   │   ├── TravelDirection.cs            ------ Forward/Reverse
│   │   ├── GoalDirectionConstraint.cs    ------ Any/Forward/Reverse
│   │   ├── VehicleParameters.cs          ------ 尺寸、安全余量和最大曲率
│   │   ├── PlanningRequest.cs            ------ 纯规划器输入
│   │   ├── HybridAStarConfiguration.cs   ------ 原语、离散、代价、限额和容差
│   │   ├── PlanningResult.cs             ------ 状态、诊断、稠密路径和分段
│   │   ├── PlanningStatus.cs             ------ 输入、碰撞、搜索和验证状态
│   │   ├── PlanningDiagnostics.cs        ------ 节点、堆、耗时、路径和终止统计
│   │   ├── CoarsePathPoint.cs            ------ 位姿、弧长、方向、曲率和净空
│   │   ├── CoarsePathPointSource.cs      ------ Start/MotionPrimitive/GoalTruncation
│   │   └── PathSegment.cs                ------ 包含式方向分段索引
│   ├── Vehicle/
│   │   ├── VehicleKinematics.cs          ------ 解析保守最大曲率
│   │   ├── VehicleFootprint.cs           ------ 扩大矩形、AABB 和外接圆
│   │   ├── OrientedRectangleCellIntersection.cs ------ 旋转矩形与格矩形精确相交
│   │   └── FootprintCollisionChecker.cs  ------ 边界、快速放行、精确和扫掠检查
│   ├── Search/
│   │   ├── MotionPrimitive.cs            ------ 恒曲率原语和实际截断长度
│   │   ├── MotionPrimitiveGenerator.cs   ------ 解析积分并保留内部采样点
│   │   ├── BinaryMinHeap.cs              ------ netstandard2.0 确定性 Open List
│   │   ├── SearchCostCalculator.cs       ------ 统一计算等效米搜索代价
│   │   ├── HybridAStarNode.cs            ------ 连续状态、代价、父索引和原语
│   │   ├── HybridAStarNodeKey.cs         ------ 位置/航向/方向/曲率离散键
│   │   ├── GoalToleranceChecker.cs       ------ 位置、航向和进入方向判断
│   │   ├── GridDijkstraHeuristic.cs      ------ 八邻域、禁止切角的二维启发
│   │   └── HybridAStarSearch.cs          ------ 扩展、重开、限额和候选管理
│   ├── Output/
│   │   ├── PathBacktracker.cs            ------ 根据父索引重建原语内部点
│   │   ├── CoarsePathAssembler.cs        ------ 弧长、换向点和方向分段
│   │   └── CoarsePathValidator.cs        ------ 数值、碰撞、曲率、终点和分段复核
│   ├── HybridAStarPlanner.cs             ------ 只消费 PlanningGridMap 的下层门面
│   ├── Facade/
│   │   ├── CoarsePathPlanningJob.cs      ------ 一次调用所需全部输入
│   │   ├── CoarsePathPlanningJobResult.cs ------ 地图结果与粗路径结果
│   │   ├── PlanningDebugOptions.cs       ------ 地图、路径、碰撞调试开关
│   │   ├── IPlanningDebugSink.cs         ------ 不改变规划状态的调试消费接口
│   │   └── CoarsePathPlanningService.cs  ------ 建图、搜索和调试的一次调用入口
│   └── Test/
│       ├── CoarsePathScenarioFactory.cs  ------ 固定、可复现的地图与规划场景
│       └── MovementTest.CoarsePathTest.cs ------ 后台规划并在 Clumsy UI 显示
│   └── README.md                         ------ 粗规划模块边界、调用示例和文档链接

ClumsyPilot/tests/
├── verify_planning_utils.ps1             ------ 工具与公共契约
├── verify_planning_map_factory.ps1       ------ 来源、事务、缓存与快照
├── verify_planning_map_adapter.ps1       ------ 栅格、适配和距离场
├── verify_planning_map_image.ps1         ------ PNG 渲染与原子发布
├── verify_coarse_path_collision.ps1      ------ 亚栅格、擦边和扫掠碰撞
├── verify_coarse_path_search.ps1         ------ 原语截断、堆、代价和搜索
├── verify_coarse_path_integration.ps1    ------ 一次调用端到端验证
└── benchmark_coarse_path.ps1             ------ Release 性能与内存门槛

固定公共调用方式

常规业务代码只保留一个长期存在的服务实例：

var result = planningService.Plan(new CoarsePathPlanningJob
{
    MapRequest = new PlanningMapRequest
    {
        Bounds = bounds,
        ResolutionMm = 50f,
        ObstacleSources = new IMapObstacleSource[]
        {
            new ManualObstacleSource(
                "manual", manualVersion, true, manualObstacles),
            new TwoLegObstacleSource("two-leg", twoLegVersion, false, twoLegSnapshot),
        },
        AllowExplicitEmptyMap = true,
    },
    Start = startPose,
    Goal = goalPose,
    Vehicle = vehicle,
    Configuration = configuration,
    StartVehicleCurvature = 0d,
    StartDirection = null,
    GoalDirection = GoalDirectionConstraint.Any,
    Debug = new PlanningDebugOptions
    {
        VisualizeMap = true,
        VisualizePath = true,
        VisualizeCollisionChecks = false,
    },
}, cancellationToken);

已有地图快照复用或纯搜索测试才直接调用 HybridAStarPlanner.Plan(PlanningRequest, CancellationToken)。业务调用方不得直接拼装 builder、rasterizer、碰撞器、运动原语或搜索节点。

P0：正确可用

Task 1：建立 Utils 与测试基座

文件：

  • Create: ClumsyPilot/ParkrobTrajplanner/Utils/AngleMath.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/Utils/UnitConverter.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/Utils/CoordinateTransform.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/Utils/NumericGuard.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/Utils/GridIndex.cs
  • Create: ClumsyPilot/tests/verify_planning_utils.ps1

产出接口：

double AngleMath.NormalizeRadians(double radians);
double AngleMath.ShortestSignedDifference(double from, double to);
int AngleMath.ToHeadingIndex(double heading, double resolution, int binCount);
double UnitConverter.MillimetersToMeters(double value);
double UnitConverter.DegreesToRadians(double value);
bool NumericGuard.IsFinite(double value);
readonly struct GridIndex { int Row; int Col; }
  • 写失败测试：断言 2π→0、179°→-179° 最短角差为 2°、1250 mm→1.25 m、车体系 (1000,0) 在世界位姿 (2000,3000,90°) 后得到 (2,4) m，并拒绝 NaN/Infinity。
  • 执行以下命令，预期构建成功、脚本因新类型不存在返回非零。
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
  • 实现：角度统一到 [-π,π)；航向索引先归一化再 floor；刚体变换使用 world=origin+R(heading)×local；GridIndex 实现值相等和稳定哈希。
  • 重跑脚本，预期输出 Planning utility checks passed.。

Task 2：锁定 CoarsePath 公共契约与默认参数

文件：

  • Create: CoarsePath/Contracts/ 下目录树列出的 12 个契约文件
  • Modify: ClumsyPilot/tests/verify_planning_utils.ps1

固定默认值：

PrimitiveLengthMeters = 0.50;
IntegrationStepMeters = 0.05;
MaximumCollisionCheckStepMeters = 0.025;
HeadingResolutionRadians = Math.PI / 36d;
CurvatureLevelCount = 5;
GoalPositionToleranceMeters = 0.15;
GoalHeadingToleranceRadians = Math.PI / 36d;
MaximumExpandedNodes = 200000;
SearchTimeout = TimeSpan.FromSeconds(5);
HeuristicWeight = 1.0;
ReverseCostMultiplier = 1.5;
GearSwitchPenaltyMeters = 1.0;
CurvatureMagnitudeWeight = 0.10;
CurvatureChangePenaltyMetersPerLevel = 0.05;
ClearanceCostWeight = 0.20;
ClearanceCostDistanceMeters = 0.50;
  • 扩展失败测试：断言上述默认值，三个方向/来源枚举，以及 PlanningRequest 的起始曲率、起始方向、目标进入方向。
  • 断言 PlanningStatus 包含：Success、Cancelled、InvalidRequest、InvalidMap、MapNotReady、InvalidVehicleParameters、InvalidCurvatureConfiguration、StartOutsideMap、StartInCollision、GoalOutsideMap、GoalInCollision、SearchTimeout、SearchNodeLimitExceeded、NoFeasiblePath、BacktrackingFailed、FinalValidationFailed、InternalError。
  • 实现不可变 Pose2D；结果工厂只允许成功结果携带路径，失败结果路径为空。
  • PlanningDiagnostics 固定记录扩展节点数、生成节点数、重新打开节点数、陈旧堆条目数、Open List 峰值、总路径长度、最小保守净空、耗时和终止原因。
  • CoarsePathPoint 固定包含 X/Y、Heading/UnwrappedHeading、ArcLength、Direction、VehicleCurvature、BodyClearance、IsGearSwitchPoint 和 Source；PathSegment 固定包含方向和包含式 StartIndex/EndIndex。
  • 重跑工具脚本，预期契约和默认值全部通过。

Task 3：实现环境栅格、边界与统一栅格化

文件：

  • Create: Map/Core/MapBoundsMm.cs
  • Create: Map/Core/EnvironmentGridMap.cs
  • Create: Map/Obstacles/IMapObstacle.cs
  • Create: Map/Obstacles/AxisAlignedRectangleObstacle.cs
  • Create: Map/Obstacles/CircleObstacle.cs
  • Create: Map/Obstacles/MapObstacleRasterizer.cs
  • Create: ClumsyPilot/tests/verify_planning_map_adapter.ps1

上述 Map/CoarsePath 相对路径均位于 ClumsyPilot/ParkrobTrajplanner/。

  • 写边界和栅格失败测试：20/200 mm 合法，范围外失败；4,000,001 格分配前失败；XMax/YMax 排他；非完整末格裁剪；地图外占据；圆/矩形与格边或格角接触时保守占据；完全在地图外的合法障碍不写格。
  • 运行脚本，预期新 Map 类型不存在。
  • 实现私有行优先 byte[]，索引固定为 row*Cols+col；世界转格使用 floor((value-min)/resolution)；不得返回内部缓冲区。
  • 实现唯一栅格化器：圆使用“圆心到格矩形最近点距离”，轴对齐矩形使用闭区间相交，只遍历裁剪后的候选包围盒。
  • 确认不存在 MarkVehicleFootprint 或安全距离参数，重跑脚本通过。

Task 4：统一人工与 TwoLeg 障碍来源

文件：

  • Create: Map/Sources/ 下目录树列出的 7 个文件
  • Create: Map/Core/MapBuildRequest.cs
  • Create: Map/Core/EnvironmentMapBuildResult.cs
  • Create: Map/Core/EnvironmentMapBuilder.cs
  • Create: ClumsyPilot/tests/verify_planning_map_factory.ps1

固定接口：

public interface IMapObstacleSource
{
    string SourceId { get; }
    long SourceVersion { get; }
    bool IsRequired { get; }
    ObstacleProjectionResult ProjectToWorld();
}
  • 构造函数统一为 ManualObstacleSource(string sourceId, long sourceVersion, bool isRequired, IReadOnlyList<IMapObstacle> obstacles) 和 TwoLegObstacleSource(string sourceId, long sourceVersion, bool isRequired, TwoLegProjectionInput input)；计划内所有实际调用均使用这两个签名。
  • 写来源事务失败测试：人工 Applied/Empty、TwoLeg 两个圆、可选来源 Unavailable/Invalid 保留人工图层、必需来源失败导致整图失败、重复 SourceId 失败、输入顺序不同但结果相同。
  • 校验 SourceId 非空且一次请求内唯一，SourceVersion 非负；来源快照内容发生变化时，上层必须递增版本。
  • 写 TwoLeg 坐标测试：使用检测时车辆位姿完成车体系 mm 到世界系 mm 变换，不得使用规划开始时位姿。
  • 实现纯快照投影：ProjectToWorld() 不能调用 TwoLegDetect、定位、UI 或系统时间；过期判断由上层采集适配器在构造 DTO 前完成。
  • builder 按 SourceId 排序；可选失败只记诊断；必需失败不发布地图；全部成功几何交给唯一 rasterizer。
  • 重跑工厂脚本，预期来源状态、投影和事务断言通过。

Task 5：生成不可变 PlanningGridMap 和保守距离场

文件：

  • Create: Map/Planning/EuclideanDistanceTransform.cs

  • Create: Map/Planning/ObstacleDistanceField.cs

  • Create: Map/Planning/PlanningGridMap.cs

  • Create: Map/Planning/PlanningMapAdapter.cs

  • Modify: ClumsyPilot/tests/verify_planning_map_adapter.ps1

  • 扩展失败测试：mm→m、占据深拷贝、地图外距离为零、障碍格距离为零、空图距离正无穷、末格裁剪，以及所有样本距离不高于暴力几何距离。

  • 实现两次一维平方距离变换，复杂度 O(Rows×Cols)；不得逐自由格遍历全部障碍格。

  • 对精确栅格中心距离应用：

Math.Max(0d, centerDistanceMeters - Math.Sqrt(2d) * resolutionMeters)
  • 查询使用点所在格的保守值，不做可能抬高结果的插值；空图跳过 EDT；边界检查独立执行。
  • PlanningGridMap 私有保存连续占据/距离数组，不提供写入口和缓冲区引用。
  • 重跑地图适配脚本，预期栅格、深拷贝、空图和保守距离全部通过。

Task 6：实现 PlanningMapFactory 与两级快照缓存

文件：

  • Create: Map/Planning/PlanningMapCache.cs
  • Create: Map/PlanningMapRequest.cs
  • Create: Map/PlanningMapBuildResult.cs
  • Create: Map/PlanningMapFactory.cs
  • Modify: Map/Planning/PlanningGridMap.cs
  • Modify: ClumsyPilot/tests/verify_planning_map_factory.ps1
public sealed class PlanningMapFactory
{
    public PlanningMapBuildResult Create(PlanningMapRequest request);
}

PlanningMapRequest 固定包含 MapBoundsMm Bounds、float ResolutionMm、IReadOnlyList<IMapObstacleSource> ObstacleSources、bool AllowExplicitEmptyMap。

  • 写缓存测试：完全相同请求返回同一 PlanningGridMap；来源版本变化但最终占据未变化时产生新 SnapshotId 并复用占据/距离缓冲；占据变化时重建距离场。
  • 写规划可用性测试：至少一个成功来源提供有效障碍语义时 PlanningReady=true；所有来源均为空时仅 AllowExplicitEmptyMap=true 可用；必需来源失败或未明确空图语义时 PlanningReady=false 并填写 PlanningBlockReason，规划器随后返回 MapNotReady。
  • 验证失效边界：起终点、车辆、规划参数和可视化开关不进入地图指纹；边界、分辨率、空图策略、来源状态/版本或规范化几何变化必须进入。
  • 实现确定性 InputFingerprint：来源先按 ID 排序，字段固定顺序，浮点按 IEEE 位模式；命中后仍比较规范化结构。
  • 栅格化后计算 OccupancyHash；命中后仍比较地图几何、数组长度和逐字节内容，不能只信任哈希。
  • 实现容量 4 的线程安全 LRU；完整命中返回同一快照，占据命中共享不可变缓冲但生成新元数据，只有占据变化才运行 EDT。
  • PlanningMapBuildResult 固定返回 Succeeded、失败原因、来源摘要、缓存命中类型和成功时的 PlanningGridMap；快照固定保存 PlanningReady、PlanningBlockReason、来源版本摘要、InputFingerprint、OccupancyHash、单调 SnapshotId。
  • 增加 16 个并发相同请求测试和 LRU 淘汰测试，重跑脚本通过。

Task 7：实现连续车体足迹和保守碰撞检查

文件：

  • Create: CoarsePath/Vehicle/ 下目录树列出的 4 个文件
  • Create: ClumsyPilot/tests/verify_coarse_path_collision.ps1

固定接口：

bool IsPoseCollisionFree(
    Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
    double additionalMarginMeters, out double bodyClearanceMeters);

bool IsSweptMotionCollisionFree(
    Pose2D from, Pose2D to, PlanningGridMap map, VehicleParameters vehicle,
    double maximumCenterStepMeters, out double minimumBodyClearanceMeters);
  • 写失败测试：正交、45°、任意航向、栅格中心/亚栅格中心、边角接触、薄障碍、地图边界、距离场快速放行，以及两个无碰撞端点之间有障碍的扫掠场景。
  • 实现以几何中心为参考的扩大车体；安全余量加到长度和宽度两侧；最大曲率与最小转弯半径并存时取更保守限制。
  • 使用分离轴定理精确判断连续旋转矩形与占据格矩形相交，接触视为碰撞；不得使用离散航向模板。
  • 检查顺序：扩大车体边界 → 保守距离严格大于外接圆时快速放行 → AABB 内占据格 SAT。
  • 扫掠采样中心步长不超过 min(configuredStep,map.Resolution/2)；每段临时附加余量为：
0.5 × (centerDisplacement
       + circumscribedRadius × abs(headingDelta))
  • 重跑碰撞脚本，预期所有亚栅格、擦边和扫掠案例通过。

Task 8：实现解析恒曲率原语和内部终点截断

文件：

  • Create: CoarsePath/Search/MotionPrimitive.cs
  • Create: CoarsePath/Search/MotionPrimitiveGenerator.cs
  • Create: CoarsePath/Search/GoalToleranceChecker.cs
  • Create/Modify: ClumsyPilot/tests/verify_coarse_path_search.ps1

解析积分：

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;
}
  • 写原语测试：直行、圆弧、倒车、五级曲率、相邻曲率最多变化一级、最大长度 0.50 m、实际采样步长不超过 min(IntegrationStep,CollisionStep,MapResolution/2)。
  • 写用户提出的案例：目标距起点 0.30 m、原语最大 0.50 m、收紧容差；断言第一个满足目标的内部点截断，后续点不生成，来源为 GoalTruncation。
  • 每个内部点严格按“有限值 → 扫掠碰撞 → 目标条件”检查；碰撞必须先于目标。
  • 起点已满足目标时创建零长度候选，不生成原语。
  • 重跑搜索脚本，预期几何、碰撞采样和 0.30 m 截断案例通过。

Task 9：实现 Open List、统一代价和二维启发

文件：

  • Create: CoarsePath/Search/BinaryMinHeap.cs
  • Create: CoarsePath/Search/SearchCostCalculator.cs
  • Create: CoarsePath/Search/GridDijkstraHeuristic.cs
  • Modify: ClumsyPilot/tests/verify_coarse_path_search.ps1

固定代价：

primitiveCost =
    lengthMeters
  × directionMultiplier
  × (1
     + CurvatureMagnitudeWeight × abs(curvature / maximumCurvature)
     + ClearanceCostWeight × max(0, 1 - clearance / ClearanceCostDistanceMeters))
  + gearSwitchPenalty
  + CurvatureChangePenaltyMetersPerLevel × abs(curvatureLevelDelta)
  • 写堆顺序测试：较小 F、较小 H、较大 G、较小插入序号；相同输入重复运行顺序一致。
  • 写代价测试：前进、倒车、换向、曲率幅值、曲率变化和净空项；拒绝负数及非有限权重。
  • 写 Dijkstra 测试：八邻域直/斜代价；两个正交邻格任一占据时禁止对角切角；二维不可达返回明确状态。
  • 实现专用二叉最小堆，不依赖 PriorityQueue；允许旧条目由搜索层惰性丢弃。
  • HeuristicWeight=1 使用 F=G+H；大于 1 时只承诺可行性。
  • 重跑搜索脚本，预期顺序、公式和切角限制通过。

Task 10：实现 Hybrid A* 节点、重开和终点候选管理

文件：

  • Create: CoarsePath/Search/HybridAStarNode.cs

  • Create: CoarsePath/Search/HybridAStarNodeKey.cs

  • Create: CoarsePath/Search/HybridAStarSearch.cs

  • Modify: ClumsyPilot/tests/verify_coarse_path_search.ps1

  • 写搜索测试：空图前进、单矩形绕行、允许倒车的狭窄场景、起始曲率、目标进入方向、±π 容差、无解、取消、超时、节点上限、重开和确定性。

  • 写候选排序测试：先生成较大 F 的终点候选时不得结束；较小 F 普通节点先出队；候选成为最佳有效条目后才成功。

  • 离散键固定为位置格、航向格、方向和曲率等级；连续位姿保留在节点中。

  • Dictionary<HybridAStarNodeKey,double> 保存普通状态最佳 G；更小 G 允许重开；旧普通堆条目惰性丢弃。

  • 终点候选放入同一堆，但不能仅因另一个连续位姿落入相同离散键且 G 更低而被删除；候选出队时重新验证目标和末段碰撞。

  • 搜索循环在每次扩展前按顺序检查取消、5 s 超时、200,000 节点上限，再弹出有效条目；Open List 为空返回 NoFeasiblePath。

  • 重跑搜索脚本，预期候选顺序、重开、限额和所有场景通过。

Task 11：回溯、装配、最终复核和 HybridAStarPlanner

文件：

  • Create: CoarsePath/Output/PathBacktracker.cs
  • Create: CoarsePath/Output/CoarsePathAssembler.cs
  • Create: CoarsePath/Output/CoarsePathValidator.cs
  • Create: CoarsePath/HybridAStarPlanner.cs
  • Create: ClumsyPilot/tests/verify_coarse_path_integration.ps1
public sealed class HybridAStarPlanner
{
    public PlanningResult Plan(
        PlanningRequest request,
        CancellationToken cancellationToken = default(CancellationToken));
}
  • 写失败状态测试：请求、地图就绪、车辆参数、曲率配置、起终点越界/碰撞，以及搜索失败到结果的无异常映射。
  • 写输出测试：首点弧长零；弧长不递减；UnwrappedHeading 连续；终点截断来源正确；除换向对外无相邻重复点。
  • 写换向分段测试：换向处保留两个坐标/航向/弧长相同而方向不同的点；新方向点标记 IsGearSwitchPoint；包含式分段完整覆盖路径。
  • 搜索节点只存父索引、方向、曲率和实际原语长度；成功后使用相同解析积分和有效步长重建内部点。
  • 最终复核有限数值、曲率、扫掠碰撞、目标容差/方向、弧长、换向对和分段；失败返回 FinalValidationFailed 且不发布部分路径。
  • 重跑碰撞、搜索和集成脚本，预期全部通过。

Task 12：实现一次调用 CoarsePathPlanningService

文件：

  • Create: CoarsePath/Facade/ 下目录树列出的 5 个文件
  • Modify: ClumsyPilot/tests/verify_coarse_path_integration.ps1
public sealed class CoarsePathPlanningService
{
    public CoarsePathPlanningJobResult Plan(
        CoarsePathPlanningJob job,
        CancellationToken cancellationToken = default(CancellationToken));
}
  • 写一次调用测试：固定执行 PlanningMapFactory.Create → HybridAStarPlanner.Plan → debug sink；地图失败不启动搜索；结果同时保留地图和规划结果。
  • 写旁路隔离测试：可视化开关不改变地图指纹、占据哈希、规划状态或路径；debug sink 异常只写调试诊断。
  • 服务持有同一 PlanningMapFactory，多次调用共享容量 4 缓存；默认 debug sink 为空行为。
  • 重跑以下 P0 验收，预期构建成功且 6 个脚本退出码为 0。
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

P0-MAP 收尾与 P1：集成、迁移和性能

Task 13（P0-MAP）：拆分迁移 TrapMapImageExporter

现有来源：

  • Read/Migrate: ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs

新文件：

  • Create: Map/Test/Visualization/PlanningMapImageExportRequest.cs

  • Create: Map/Test/Visualization/PlanningMapImageExportResult.cs

  • Create: Map/Test/Visualization/PlanningMapImageExporter.cs

  • Create: Map/Test/Visualization/PlanningMapImageRenderer.cs

  • Create: Map/Test/Visualization/ValidatedPngWriter.cs

  • Create: ClumsyPilot/tests/verify_planning_map_image.ps1

  • 写新图片测试：输入只允许 PlanningGridMap；覆盖关闭导出、非法尺寸、唯一命名、临时文件清理、PNG 签名/IHDR/IEND 和每个 chunk CRC。

  • 保留现有有效限制：PixelsPerCell=4、MaximumImageEdgePixels=4000、MaximumFileSizeBytes=50 MiB、OutputDpi=300、最多 1024 次重名重试、StbImageWriteSharp 1.16.7。

  • 按职责迁移：Request/Result 只放 DTO；Renderer 生成 RGBA；Writer 负责编码/CRC；Exporter 校验、独占临时文件和原子发布。

  • 新导出器不得依赖 GridMapData、TrapMapVehiclePose、TwoLeg 状态、传感器或地图构造器；车辆、起终点和路径只作可选叠加层。

  • 运行图片脚本通过后先保留旧文件，Task 16 确认等价覆盖后再退役。

Task 14：增加地图与粗路径 MovementTest

文件：

  • Create: Map/Test/MovementTest.MapTest.cs

  • Create: CoarsePath/Test/CoarsePathScenarioFactory.cs

  • Create: CoarsePath/Test/MovementTest.CoarsePathTest.cs

  • Modify: ClumsyPilot/tests/verify_coarse_path_integration.ps1

  • P0-MAP 部分： 先创建 MovementTest.MapTest.cs。它只调用长期持有的 PlanningMapFactory，显示来源状态、栅格、快照 ID 和缓存命中，并可选调用新 PNG 导出器；通过 Map Gate 后即可结束当前首要里程碑。

  • P1 部分： Map Gate 和 P0-PLAN 均通过后，再创建 CoarsePathScenarioFactory.cs 与 MovementTest.CoarsePathTest.cs。

  • 场景工厂提供显式空图、单矩形绕行、人工圆+矩形+TwoLeg、相同地图缓存命中、倒车换向和无解案例。

  • CoarsePath 案例使用同一 CoarsePathPlanningService；测试类不得直接实例化 rasterizer、碰撞器、原语生成器或搜索节点。

  • MovementTest.CoarsePathTest 在后台任务调用同步 Plan，绘制起点、目标、路径、换向点和扩大车体检查点。

  • TestStop 先取消专用 CancellationTokenSource，再清理任务和 Painter；两个入口不得发送底盘运动命令。

  • Clumsy UI 手动运行时不阻塞界面，停止后无后台规划残留，调试开关不改变结果。

Task 15（P1）：性能、资源和确定性验收

文件：

  • Create: ClumsyPilot/tests/benchmark_coarse_path.ps1

  • Modify: Map/Planning/PlanningMapCache.cs

  • Modify: Map/Planning/EuclideanDistanceTransform.cs

  • Modify: CoarsePath/Search/BinaryMinHeap.cs

  • Modify: CoarsePath/Search/HybridAStarSearch.cs

  • Modify: CoarsePath/Contracts/PlanningDiagnostics.cs

  • 基准脚本输出地图规模、缓存命中、状态、耗时、扩展/生成/重开节点、陈旧堆条目、Open List 峰值和托管内存增量，超限返回非零。

  • 地图参考：20 m×20 m、0.05 m、160,000 格、100 障碍；20 次后完整构建 P95≤200 ms，完整缓存命中 P95≤5 ms。

  • 地图极限：4,000,000 格、100 障碍；3 s 内成功或明确失败；成功时内存增量≤160 MB，无溢出和部分快照。

  • 规划参考：12 m×8 m、0.05 m、矩形阻断直线、距离≥8 m；20 次 P95≤2 s，内存增量≤256 MB。

  • 规划压力：20 m×20 m、0.05 m；成功或无解均在 5 s、200,000 节点和 512 MB 增量内返回。

  • 优化只针对查询分配、重复 EDT、堆扩容和稠密点保存；不得降低碰撞保守性、跳过最终复核或放宽失败状态。

  • 运行：

dotnet build .\ClumsyPilot\ClumsyPilot.csproj -c Release --no-restore
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\benchmark_coarse_path.ps1 -Configuration Release

预期：全部指标达标，脚本退出码为 0。

Task 16（P1）：退役旧地图入口并完成文档

文件：

  • Modify/Delete after equivalent coverage: ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/MovementTest.Trapmaptest.cs

  • Delete after equivalent coverage: ClumsyPilot/ParkrobTrajplanner/Occupancygird_Map/Map_test/TrapMapImageExporter.cs

  • Update/Delete after equivalent coverage: ClumsyPilot/tests/verify_trapmap_grid.ps1

  • Update/Delete after equivalent coverage: ClumsyPilot/tests/verify_trapmap_inputs.ps1

  • Update/Delete after equivalent coverage: ClumsyPilot/tests/verify_trapmap_lifecycle.ps1

  • Update/Delete after equivalent coverage: ClumsyPilot/tests/verify_trapmap_image.ps1

  • Create: ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md

  • 建立覆盖表：TrapMapBounds→MapBoundsMm、GridMapData→EnvironmentGridMap/PlanningGridMap、TrapMapLayerComposer→EnvironmentMapBuilder、TrapMapBuilder.Get→上层采集+CoarsePathPlanningService、旧 exporter→五个 Visualization 文件。

  • 等价脚本全部通过后再删除旧实现；不保留车辆写图、默认 300 mm 膨胀或地图构建器直接调用 TwoLegDetect 的兼容开关。

  • README 写明一次调用、下层门面、mm/m-rad 边界、快照变化判断、调试开关不参与指纹、P0/P1 命令和非目标。

  • 执行旧引用搜索：

rg -n "GridMapData|TrapMapBuilder|TrapMapLayerComposer|TrapMapImageExporter|MarkVehicleFootprint" .\ClumsyPilot

预期：仅迁移说明或历史文档可命中；运行时代码和新测试不得命中旧类型。

  • 运行最终回归：
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_planning_map_image.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

预期：Debug 构建成功，7 个功能脚本退出码均为 0；随后重跑 Task 15 Release 基准并通过。

最终验收清单

  • CoarsePathPlanningService.Plan(job) 是推荐的一次调用入口。
  • PlanningMapFactory 和 HybridAStarPlanner 仅作为可独立测试的下层门面。
  • 人工、TwoLeg 和未来障碍通过同一个 IMapObstacleSource 进入唯一栅格化器。
  • 地图未变化时复用完整快照或占据/距离缓冲；可视化、起终点和车辆参数不参与地图变化判断。
  • 地图不包含车辆自身和安全膨胀，碰撞检查使用扩大车辆矩形。
  • 距离场是净空下界，不能因高估而跳过精确碰撞。
  • 0.50 m 原语可在任意内部采样点截断；终点候选按 Open List 顺序出队后才终止。
  • best-G、重开、陈旧条目、候选保护和堆排序均有自动化测试。
  • 输出包含稠密点、保守净空、实际曲率、换向点和完整方向分段。
  • PNG 和 MovementTest 只消费只读快照，不成为核心规划依赖。
  • 失败、取消、超时和限额均返回空路径及明确状态，不发布部分结果。
  • Debug 与 Release 验收全部通过，且未执行 Git 自检或提交。
Reference in New Issue View Git Blame Copy Permalink