388 lines
24 KiB
Markdown
388 lines
24 KiB
Markdown
# CoarsePath 粗路径规划（P0/P1）
|
||
|
||
`CoarsePath` 在 `Map` 提供的不可变 `PlanningGridMap` 上执行 Hybrid A*，输出已经过连续碰撞、终点和输出不变量复核的粗路径。它只处理“能否安全地从当前车辆几何中心到达目标”的几何搜索；不读取传感器或定位，不绘制 UI，也不向底盘发送任何命令。
|
||
|
||
地图障碍物来源、世界坐标栅格化、距离场和缓存细节由 [Map/README.md](../Map/README.md) 说明。本模块唯一建议的业务调用入口是：
|
||
|
||
```csharp
|
||
CoarsePathPlanningService.Plan(job, cancellationToken)
|
||
```
|
||
|
||
## 模块说明（Module Overview）
|
||
|
||
| 模块 | 负责内容 | 不负责内容 |
|
||
| --- | --- | --- |
|
||
| `Map` | 外部障碍物快照、占据栅格、保守障碍距离、地图缓存 | 车辆足迹、运动原语、路径搜索、控制 |
|
||
| `CoarsePath` | 车辆扩大足迹、连续碰撞检查、前进/倒车原语、Hybrid A*、路径复核与方向分段 | 传感器读取、定位读取、速度规划、路径跟踪、底盘命令 |
|
||
| `Facade` | 将建图、搜索、取消、总预算和可选调试旁路编排为一次调用 | 修改地图内容、写入 AMR 占据、执行轨迹 |
|
||
| `Test` | 固定回归场景、P1 手动测试入口与规划结果可视化 | 真实作业地图、实时重规划或车辆控制 |
|
||
|
||
安全余量只由 `VehicleParameters.SafetyMarginMeters` 扩大车辆矩形。它不会回写到地图障碍物，因此同一个 `PlanningGridMap` 可由不同车辆参数重复使用。
|
||
|
||
## 文件结构（File Structure）
|
||
|
||
```text
|
||
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）
|
||
|
||
```text
|
||
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](../Map/README.md)。
|
||
|
||
```csharp
|
||
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 步：长期持有服务
|
||
|
||
服务持有地图工厂和规划器，应该作为规划业务、任务执行器或上层服务的长期字段，而不是在每次调用中创建：
|
||
|
||
```csharp
|
||
private readonly CoarsePathPlanningService _coarsePathService =
|
||
new CoarsePathPlanningService();
|
||
```
|
||
|
||
### 第 2 步：准备地图请求
|
||
|
||
创建 `PlanningMapRequest`，其边界、分辨率和障碍物仍使用 mm。使用手工圆形/矩形或 TwoLeg 快照时，应先按 [Map/README.md](../Map/README.md) 将它们包装为 `IMapObstacleSource`，并为内容变化递增 `SourceVersion`。
|
||
|
||
```csharp
|
||
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` 可以限制最终进入目标的方向。
|
||
|
||
```csharp
|
||
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`；至少必须提供一个有效限制。
|
||
|
||
```csharp
|
||
var vehicle = new VehicleParameters
|
||
{
|
||
LengthMeters = 0.80d,
|
||
WidthMeters = 0.60d,
|
||
SafetyMarginMeters = 0.05d,
|
||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||
};
|
||
```
|
||
|
||
### 第 5 步：调整搜索配置
|
||
|
||
默认 `HybridAStarConfiguration` 包含原语长度、积分步长、航向离散、终点容差、代价、节点上限和总超时。若业务需要覆盖默认值，应同时理解安全影响：`MaximumCollisionCheckStepMeters` 不能以牺牲连续碰撞检查精度为代价随意增大。
|
||
|
||
```csharp
|
||
var configuration = new HybridAStarConfiguration
|
||
{
|
||
SearchTimeout = TimeSpan.FromSeconds(5d),
|
||
MaximumExpandedNodes = 200000,
|
||
};
|
||
```
|
||
|
||
### 第 6 步：调用并消费成功结果
|
||
|
||
只有 `Success` 可以发布完整路径。`Path` 是稠密点序列；`Segments` 是覆盖整条路径的前进/倒车包含式索引段，可供后续的速度规划或显示模块消费。
|
||
|
||
```csharp
|
||
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` 在唯一边界完成转换：
|
||
|
||
```text
|
||
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` 结果视作后续模块的粗路径输入，不能把它当作可直接下发的时间轨迹。
|