ParkingRobot/ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md

CoarsePath 粗路径规划（P0/P1）

CoarsePath 在 Map 提供的不可变 PlanningGridMap 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。

地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 Map/README.md 说明。本模块唯一建议的业务调用入口是：

CoarsePathPlanningService.Plan(job, cancellationToken)

模块说明（Module Overview）

模块	负责内容	不负责内容
Map	外部障碍物快照、占据栅格、保守障碍距离、地图缓存	车辆足迹、运动原语、路径搜索、控制
CoarsePath	车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段	传感器读取、定位读取、速度规划、路径跟踪、底盘命令
Facade	将建图、搜索、取消、总预算和可选调试旁路编排为一次调用	修改地图内容、写入 AMR 占据、执行轨迹
Test	固定回归场景、P1 手动测试入口与规划结果可视化	真实作业地图、实时重规划或车辆控制

安全余量只由 VehicleParameters.SafetyMarginMeters 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 PlanningGridMap 可由不同车辆参数重复使用。

文件结构（File Structure）

CoarsePath/
├── README.md                                      # 本模块说明：结构、数据流、调用与测试
├── HybridAStarPlanner.cs                          # 公开规划门面后的核心编排：校验、搜索、回溯和复核
├── Contracts/
│   ├── Pose2D.cs                                  # 车辆几何中心位姿：m / rad
│   ├── PlanningRequest.cs                         # 已有 PlanningGridMap 上的一次内部搜索请求
│   ├── PlanningResult.cs                          # 不可变规划结果：成功路径或空结果
│   ├── PlanningStatus.cs                          # 成功、取消、无解、输入和资源限制状态
│   ├── CoarsePathPoint.cs                         # 稠密路径点、方向、曲率、净空和换向标记
│   ├── PathSegment.cs                             # 前进或倒车的包含式路径索引段
│   ├── VehicleParameters.cs                       # 车体尺寸、安全余量和曲率限制
│   ├── HybridAStarConfiguration.cs                # 原语、离散、代价、容差和资源上限
│   └── GoalDirectionConstraint.cs                 # 目标进入方向约束
├── Vehicle/
│   ├── VehicleKinematics.cs                       # 恒曲率车辆运动学积分
│   ├── VehicleFootprint.cs                        # 扩大后的车辆矩形几何
│   ├── OrientedRectangleCellIntersection.cs       # 旋转矩形与占据格相交判定
│   └── FootprintCollisionChecker.cs               # 连续扫掠的足迹碰撞检查
├── Search/
│   ├── BinaryMinHeap.cs                           # 可更新优先级的 Open List
│   ├── GridDijkstraHeuristic.cs                   # 二维栅格可达性和距离启发式
│   ├── GoalToleranceChecker.cs                    # 目标位置、航向和方向约束判定
│   ├── MotionPrimitive.cs                         # 单个前进或倒车恒曲率原语
│   ├── MotionPrimitiveGenerator.cs                # 原语离散与连续积分点生成
│   ├── SearchCostCalculator.cs                    # 长度、倒车、换向、曲率和净空代价
│   ├── HybridAStarNode.cs                         # 搜索节点与父链信息
│   ├── HybridAStarNodeKey.cs                      # 离散状态键
│   └── HybridAStarSearch.cs                       # Hybrid A* 主搜索循环
├── Output/
│   ├── PathBacktracker.cs                         # 从终点节点安全回溯父链
│   ├── CoarsePathAssembler.cs                     # 组装稠密路径和方向段
│   └── CoarsePathValidator.cs                     # 对最终输出重新进行连续复核
├── Facade/
│   ├── CoarsePathPlanningJob.cs                   # 一次完整业务输入：地图请求、位姿、车辆、配置
│   ├── CoarsePathPlanningJobResult.cs             # 同时包含地图结果和规划结果的不可变输出
│   ├── CoarsePathPlanningService.cs               # 唯一业务调用门面
│   ├── PlanningDebugOptions.cs                    # 可选调试旁路配置
│   └── IPlanningDebugSink.cs                      # 调试旁路接收器契约
└── Test/
    ├── CoarsePathScenarioFactory.cs               # 六个固定场景和手动目标演示请求工厂
    └── MovementTest.CoarsePathTest.cs              # 七个 Clumsy 入口、后台取消和 Painter 绘制

规划数据流（Planning Data Flow）

CoarsePathPlanningJob
        │  MapRequest 使用 mm；Pose2D/车辆使用 m、rad
        ▼
CoarsePathPlanningService.Plan(job, cancellationToken)
        │
        ├── PlanningMapFactory.Create(job.MapRequest)
        │       │
        │       ├── 失败、取消或超时
        │       │       └── MapResult + 空路径 PlanningResult，搜索不启动
        │       │
        │       └── 成功：不可变 PlanningGridMap
        ▼
PlanningRequest
        │
        ▼
HybridAStarPlanner
        ├── 车辆扩大足迹与连续碰撞检查
        ├── GridDijkstraHeuristic + Hybrid A* 搜索
        ├── PathBacktracker + CoarsePathAssembler
        └── CoarsePathValidator 最终复核
        ▼
PlanningResult + MapResult
        ▼
CoarsePathPlanningJobResult

调用方只创建 CoarsePathPlanningJob 并消费 CoarsePathPlanningJobResult。PlanningRequest、HybridAStarPlanner、原语和碰撞检查器属于模块内部协作对象，不应由 UI、传感器或 MovementTest 直接拼接。

构建状态与停止（Build Status and Stop）

必须一起处理 MapResult 和 PlanningResult。MapResult.Status 的类型是 PlanningMapBuildStatus；地图失败时，门面返回对应的空路径结果，并且不会启动 Hybrid A*。

情况	MapResult	PlanningResult	调用方处理
地图和搜索成功	Success 且 Map 非空	Success，发布完整路径和方向段	消费粗路径；后续模块仍需自行进行平滑、速度和控制
地图输入/来源失败	Failed	InvalidMap	读取 FailureReason，修复地图输入
调用被取消	Cancelled	Cancelled	不重试为普通无解；不会发布地图或部分路径
总预算耗尽	TimedOut	SearchTimeout	根据上层策略调整预算或稍后重试
搜索无解	地图成功	NoFeasiblePath	当前地图、车体和运动约束下无可行路径
节点或搜索资源受限	地图成功	SearchNodeLimitExceeded 或 SearchTimeout	读取诊断后调整配置或上层策略

除 PlanningStatus.Success 外，PlanningResult.Path 与 PlanningResult.Segments 始终为空。取消、超时、无解、输入错误和最终复核失败都不能作为“部分可执行路径”使用。

总预算与取消

HybridAStarConfiguration.SearchTimeout 是从门面开始计时的一次总预算，依次覆盖建图、距离场、二维启发式和 Hybrid A*。同一个 CancellationToken 会沿调用链传递，取消优先于超时。

总耗时与路径搜索耗时

PlanningDiagnostics.Elapsed 是从 CoarsePathPlanningService.Plan 开始的总耗时，包含地图来源、缓存、栅格化、距离场和路径规划。PathSearchElapsed（路径搜索耗时）从地图和起终点预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；搜索开始前失败时为零。

坐标与单位（Coordinates and Units）

数据	单位	说明
PlanningMapRequest.Bounds、分辨率、障碍物几何	mm	来自 Map 的世界坐标；边界采用 [min, max)
Pose2D.X、Pose2D.Y、路径位置、车辆尺寸、安全余量、弧长	m	CoarsePath 的连续世界坐标和长度
Pose2D.Heading、航向容差	rad	核心一律使用弧度
曲率、起步曲率	1/m	最大曲率或最小转弯半径至少提供一个
PlanningGridMap 世界查询参数	m	越界位置按占据处理，净距为 0
P1 的 AMR/手动目标 X/Y	mm	仅在 UI 边界读取，进入核心前除以 1000
P1 的 AMR/手动目标航向	deg	仅在 UI 边界转换为 deg * PI / 180 -> rad

起点和终点都表示车辆几何中心。若上游定位参考点是雷达、天线或其他安装点，必须先在上游应用安装外参；不要在 CoarsePath 内猜测偏移。车辆外扩由 VehicleParameters.SafetyMarginMeters 表达，不要把余量写入 Map 障碍物。

最小调用示例（Minimal Call Example）

以下示例明确允许空图，因而只适合算法或单位演示。真实作业必须通过 IMapObstacleSource 提供有效障碍物快照；如何构造来源请阅读 Map/README.md。

using System;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.Mapping;

var service = new CoarsePathPlanningService(); // 长期持有，保留地图缓存

var job = new CoarsePathPlanningJob
{
    MapRequest = new PlanningMapRequest
    {
        Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
        ResolutionMm = 50f,
        ObstacleSources = Array.Empty<IMapObstacleSource>(),
        AllowExplicitEmptyMap = true, // 仅演示时明确允许
    },
    Start = new Pose2D(1d, 1d, 0d),
    Goal = new Pose2D(3d, 1d, 0d),
    Vehicle = new VehicleParameters
    {
        LengthMeters = 0.80d,
        WidthMeters = 0.60d,
        SafetyMarginMeters = 0.05d,
        MaximumCurvaturePerMeter = 1d / 1.20d,
    },
    Configuration = new HybridAStarConfiguration(),
    GoalDirection = GoalDirectionConstraint.Forward,
};

CoarsePathPlanningJobResult result =
    service.Plan(job, CancellationToken.None);

if (!result.MapResult.Succeeded)
    throw new InvalidOperationException(result.MapResult.FailureReason);

if (result.PlanningResult.Status != PlanningStatus.Success)
    throw new InvalidOperationException(
        result.PlanningResult.Diagnostics.TerminationReason);

foreach (CoarsePathPoint point in result.PlanningResult.Path)
    Console.WriteLine(point.X + "," + point.Y + "," + point.Heading);

缓存与 SourceVersion（Cache and SourceVersion）

CoarsePathPlanningService 在生命周期内长期持有 PlanningMapFactory，因此重复调用时能够复用地图缓存。不要每次规划都新建服务，否则会失去缓存收益。

缓存层级	条件	结果
Input	边界、分辨率、空图策略、来源 ID、SourceVersion、必需性和来源结果相同	返回同一个不可变 PlanningGridMap
Occupancy	输入版本变化，但最终占据栅格相同	复用占据/距离数组，生成新的快照元数据
None	占据内容变化	重建规划快照和距离场

障碍来源内容改变时，调用方必须递增该来源的 SourceVersion。仅修改起终点、车辆、搜索配置、调试开关或调试接收器不会改变地图输入；改变障碍物却不递增版本则可能错误复用旧快照。

详细使用指南（Detailed Usage Guide）

本节说明调用方如何从一个地图输入得到可消费的粗路径。所有业务调用都通过 CoarsePathPlanningService.Plan(job, cancellationToken) 完成。

第 1 步：长期持有服务

服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：

private readonly CoarsePathPlanningService _coarsePathService =
    new CoarsePathPlanningService();

第 2 步：准备地图请求

创建 PlanningMapRequest，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 Map/README.md 将它们包装为 IMapObstacleSource，并为内容变化递增 SourceVersion。

var mapRequest = new PlanningMapRequest
{
    Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
    ResolutionMm = 50f,
    ObstacleSources = sources,
    AllowExplicitEmptyMap = false,
};

AllowExplicitEmptyMap = true 只在调用方明确确认空地图安全时使用。未提供有效障碍物且未显式允许空图时，地图不会进入规划。

第 3 步：填写起点、终点和方向约束

将车辆几何中心的世界 X/Y 从 mm 转为 m，并将航向转换为 rad 后创建 Pose2D。StartDirection = null 表示允许从前进或倒车开始；GoalDirection 可以限制最终进入目标的方向。

var start = new Pose2D(startXmm / 1000d, startYmm / 1000d,
    startHeadingDeg * Math.PI / 180d);
var goal = new Pose2D(goalXmm / 1000d, goalYmm / 1000d,
    goalHeadingDeg * Math.PI / 180d);

第 4 步：填写车辆参数

车辆尺寸和安全余量全部为 m。曲率限制可填写 MaximumCurvaturePerMeter，或填写 MinimumTurningRadiusMeters；至少必须提供一个有效限制。

var vehicle = new VehicleParameters
{
    LengthMeters = 0.80d,
    WidthMeters = 0.60d,
    SafetyMarginMeters = 0.05d,
    MaximumCurvaturePerMeter = 1d / 1.20d,
};

第 5 步：调整搜索配置

默认 HybridAStarConfiguration 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：MaximumCollisionCheckStepMeters 不能以牺牲连续碰撞检查精度为代价随意增大。

var configuration = new HybridAStarConfiguration
{
    SearchTimeout = TimeSpan.FromSeconds(5d),
    MaximumExpandedNodes = 200000,
};

第 6 步：调用并消费成功结果

只有 Success 可以发布完整路径。Path 是稠密点序列；Segments 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。

var job = new CoarsePathPlanningJob
{
    MapRequest = mapRequest,
    Start = start,
    Goal = goal,
    Vehicle = vehicle,
    Configuration = configuration,
    GoalDirection = GoalDirectionConstraint.Any,
};

CoarsePathPlanningJobResult result = _coarsePathService.Plan(job, cancellationToken);
if (!result.MapResult.Succeeded)
    ReportMapFailure(result.MapResult.FailureReason);
else if (result.PlanningResult.Status == PlanningStatus.Success)
    ConsumeCoarsePath(result.PlanningResult.Path, result.PlanningResult.Segments);
else
    ReportPlanningFailure(result.PlanningResult.Diagnostics.TerminationReason);

CoarsePathPoint.IsGearSwitchPoint 为 true 表示该点是新方向段开始处。换向位置会保留一对位置、航向与弧长相同、方向不同的相邻点；UnwrappedHeading 用于跨越 -pi/pi 时保持显示连续。

P1 手动测试与可视化（P1 Manual Tests and Visualization）

P1 在 Test/MovementTest.CoarsePathTest.cs 提供只读测试入口。它们共用一个长期存活的 CoarsePathPlanningService，只提交规划并绘制结果；不发送底盘、速度或转向命令。

MovementTest 名称	场景	预期
粗路径规划-显式空图	显式允许的空图	前进直达成功
粗路径规划-单矩形绕行	中央矩形阻断直线	成功绕障
粗路径规划-多来源障碍	手工圆形、矩形和 TwoLeg 快照	证明多来源经过同一门面
粗路径规划-缓存命中	重复相同地图输入	后续调用显示 Input 缓存命中
粗路径规划-倒车换向	前进起步、倒车到达	成功路径含 IsGearSwitchPoint
粗路径规划-无解	贯穿地图的障碍带	返回 NoFeasiblePath 且不发布路径
粗路径规划	当前 AMR 位姿、人工终点和可选人工障碍物	验证手动障碍物、边界、路径与可视化

固定案例的实时 AMR 锚定

粗路径规划-显式空图、粗路径规划-单矩形绕行、粗路径规划-多来源障碍、粗路径规划-缓存命中、粗路径规划-倒车换向 和 粗路径规划-无解 是六个固定案例。它们不再以写死的世界起点运行：共享运行器在前台仅调用一次 DetourInterface.getCartLocation()，校验并冻结本次 AMR 的世界 X(mm)、Y(mm) 与航向 deg，再创建本次规划请求；后台规划期间不会再次读取定位。

Create(CoarsePathTestScenario scenario, double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)

该入口把基准案例的起点映射为冻结的 AMR 位姿，并以相同的 ΔX/ΔY 平移地图边界、目标、圆形/矩形障碍，以及 TwoLeg 的检测原点。因此，固定案例始终在当前 AMR 附近保留原有的相对几何关系。起点航向严格使用冻结的 AMR 航向；终点航向保持基准案例的“终点航向减起点航向”差值，叠加到当前 AMR 航向后规范化到 [-pi, pi]。TwoLeg 只平移检测原点，DetectionHeadingRadians 不会因 AMR 航向发生旋转。

粗路径规划-缓存命中 只有两次运行冻结到相同的 AMR X/Y、从而形成相同的平移后地图输入时，才作为缓存命中场景；AMR 位置移动后，地图输入正常未命中并重建快照。仅 AMR 航向变化不会改变固定案例的地图输入，地图缓存仍可命中。若定位读取为空、抛出异常，或 X、Y、航向含有 NaN/无穷值，运行器不会提交后台规划，状态与 Toast 会显示以“AMR 位姿不可用”开头的诊断原因。

手动 粗路径规划 的人工终点、障碍物和超时输入流程保持不变；它不套用固定案例的整体平移规则。

AMR 位姿、手动终点与障碍物

CoarsePathPlanningTest 启动时读取一次 DetourInterface.getCartLocation()，将当前 AMR 世界位姿冻结为起点；随后依次输入目标世界 X(mm)、Y(mm)、航向 deg，以及障碍物数量 0-20。每个障碍物再依次输入类型和几何参数：

类型输入	形状	输入参数（全部为 mm）
1	圆形	几何中心 X、Y 与半径；半径必须大于 0
2	轴对齐矩形	几何中心 X、Y、X 向长度、Y 向宽度；两个尺寸必须大于 0

矩形只支持 AxisAlignedRectangle，不提供旋转角；其中心和长宽由 ManualCoarsePathObstacle.AxisAlignedRectangle 表达。圆形由 ManualCoarsePathObstacle.Circle 表达。所有无穷、NaN、非数字或不合法尺寸都会在输入阶段拒绝。

CoarsePathScenarioFactory.CreateManualObstacleDemo 在唯一边界完成转换：

AMR/目标 X、Y：mm / 1000 -> m
AMR/目标航向：deg * PI / 180 -> rad

当数量为 0 时，入口使用显式空图；CreateManualGoalDemo 保留为同一零障碍物场景的兼容帮助方法。数量大于 0 时，工厂将 ManualCoarsePathObstacle 快照封装成来源 ID 为 manual-user-input 的地图输入，并为每次手动快照分配新的 SourceVersion，避免错误复用地图缓存。规划边界覆盖起点、终点和每个障碍物的完整轮廓，再增加 8000 mm 余量并按 50 mm 对齐。

手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 TimeSpan 可表示范围内的有限正数秒；0、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖本次 CoarsePathPlanningJob.Configuration.SearchTimeout，不会改变固定场景或全局默认值。

此入口使用固定演示车辆：长 0.80 m、宽 0.60 m、四周安全余量 0.05 m、最小转弯半径 1.20 m。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。

getCartLocation 在无有效定位时可能阻塞，因此应在定位准备完成的测试环境使用。手动障碍物是测试输入，不能替代现场障碍物来源；零障碍物的显式空图也绝不代表现场不存在障碍物。

后台执行、停止与图层

每次测试启动时会创建独立的 CancellationTokenSource，以 Task.Run 调用门面，并先取消旧会话。Test() 不等待任务，也不读取 Task.Result；TestStop 取消当前令牌、使会话失效并清空图层。已取消任务完成后不会覆盖新会话，也不会显示部分路径。

专用世界坐标 Painter 图层为 CoarsePathPlanningV1。它直接读取本次 PlanningGridMap 的 Bounds、ResolutionMm、SnapshotId 和 IsOccupied(row, col)，因此边界、抽稀网格和占据格与实际规划快照一致，而不是重新绘制原始障碍物。

状态图层显示规划状态、总耗时、PathSearchElapsed（路径搜索耗时）、扩展/生成节点数、Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加 TerminationReason，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。

颜色	可视化元素
灰白	地图边界与栅格网络
暗红	占据格
绿色	起点、前进路径和方向箭头
橙色	终点、航向和位置容差圈
天蓝	倒车路径和方向箭头
紫色	换向点
金色	已纳入安全余量的车辆检查框

自动化已检查 UI 入口的后台、取消和数据来源结构。仍需在实际 Clumsy 界面手动运行“粗路径规划-单矩形绕行”和“粗路径规划”，确认图层交互显示与停止按钮效果。

常见错误（Common Errors）

现象	原因	处理
起点、终点或障碍物位置相差 1000 倍	将 mm 直接传给 Pose2D 或把 m 传给地图输入	Map 输入使用 mm；Pose2D、车辆和路径使用 m
路径朝向错误或旋转异常	将 P1 的 deg 直接当作核心 rad	在 UI/上层边界执行 deg * PI / 180，核心只保存 rad
障碍物已经变化却复用旧地图	内容变更后没有递增 SourceVersion	每次来源快照内容变化后增加对应版本号
地图创建成功但规划被阻止	没有有效障碍物且未显式允许空图	提供有效来源；仅在确认安全的演示中设置 AllowExplicitEmptyMap = true
无解、取消或超时后仍尝试使用路径	没有检查 PlanningStatus.Success	仅成功时消费 Path 和 Segments；其他状态读取诊断
将粗路径直接下发给车辆	粗路径不包含速度、时间、执行控制或实时安全闭环	在后续阶段增加平滑、时间参数化、跟踪和独立安全控制
P1 手动终点表现为空场地安全	手动入口使用显式空图演示	真实作业必须提供真实障碍物快照，不能复用空图语义

第一版限制（First-Version Limits）

当前 P0/P1 已提供安全、确定性的粗路径核心和手动结果可视化，但不包含：

• 路径平滑或曲率连续优化；
• Reeds-Shepp 或 Dubins 精确终点连接；
• 速度、加速度、时间标注、时间轨迹和路径跟踪控制；
• 底盘命令、避障闭环、现场传感器采集或实时重规划调度；
• 横移、蟹行或其他非前进/倒车运动原语；
• 原地旋转；
• 真实作业地图接入、交互式场景编辑和 Release 性能/资源/确定性基准。

当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。

因此，调用方只能把 Success 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。