838 lines
36 KiB
Markdown
838 lines
36 KiB
Markdown
# Coarse Path Manual Test Diagnostics 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(name = "粗路径规划")]` 支持每次输入正数秒总预算，并在失败时显示真实终止原因、搜索统计、总耗时和路径搜索耗时。
|
||
|
||
**Architecture:** `HybridAStarSearchResult` 在搜索边界保留原始终止原因，`HybridAStarPlanner` 负责把原因、资源限制和节点统计装配成公开诊断。`PlanningDiagnostics` 增加兼容的 `PathSearchElapsed`，MovementTest 只覆盖本次任务的超时配置并在图层和 Toast 中显示诊断；搜索、安全和路径发布规则保持不变。
|
||
|
||
**Tech Stack:** C# / .NET Standard 2.0、`System.Diagnostics.Stopwatch`、PowerShell 反射回归脚本、Clumsy `MovementTest`/Painter。
|
||
|
||
## Global Constraints
|
||
|
||
- 普通规划失败继续通过 `PlanningResult` 返回，不把超时、无解、碰撞或资源上限改成异常。
|
||
- `PlanningDiagnostics.Elapsed` 继续表示从 `CoarsePathPlanningService.Plan` 开始、包含建图的总耗时。
|
||
- `PlanningDiagnostics.PathSearchElapsed` 从搜索前预检通过后开始，包含二维启发式、Hybrid A*、回溯、装配和最终复核，不包含建图。
|
||
- 搜索前失败的 `PathSearchElapsed` 为 `TimeSpan.Zero`；搜索后的所有出口满足 `TimeSpan.Zero <= PathSearchElapsed <= Elapsed`。
|
||
- 手动超时只接受 `TimeSpan` 可表示范围内的有限正数秒；`0` 不表示不限时。
|
||
- 不修改碰撞步长、终点容差、Open List 排序、地图边界策略、倒车开关或成功路径发布条件。
|
||
- 不实现 Reeds-Shepp/Dubins 精确连接、横移、蟹行、原地旋转、平滑、速度规划或控制。
|
||
- 手动入口继续使用长 `0.80 m`、宽 `0.60 m`、安全余量 `0.05 m`、最小转弯半径 `1.20 m` 的固定演示车辆参数。
|
||
- 工作区已有未提交内容；每次提交只能暂存任务中明确列出的文件，不得暂存其他路径。
|
||
|
||
## File Structure
|
||
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs` — 搜索结果保留发生位置一致的原始终止原因。
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs` — 公开地图就绪后的独立路径搜索耗时。
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs` — 计时搜索/发布阶段并组合原因、资源上限和节点统计。
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs` — 读取单次超时并在图层/Toast 显示诊断。
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md` — 记录手动预算、演示车辆、两类耗时和第一版能力边界。
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1` — 验证搜索原始原因和内部异常不再静默。
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1` — 验证规划器诊断、路径搜索耗时和慢可行场景。
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_ui.ps1` — 验证手动超时、图层/Toast 和 README 文本。
|
||
|
||
---
|
||
|
||
### Task 1: Preserve Search Termination Reasons
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_search.ps1:302-391`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs:15-318`
|
||
|
||
**Interfaces:**
|
||
- Consumes: `HybridAStarSearch.Search(PlanningRequest, CancellationToken)` 和内部 `Search(PlanningRequest, PlanningOperationBudget)`。
|
||
- Produces: `HybridAStarSearchResult.TerminationReason : string`，成功时为空，所有失败状态非空。
|
||
- Produces: `CreateResult(..., int? successNodeIndex, string terminationReason = null)`，未显式提供原因时按状态生成稳定中文原因。
|
||
|
||
- [ ] **Step 1: Add failing search-result reason assertions**
|
||
|
||
在搜索结果属性断言后加入：
|
||
|
||
```powershell
|
||
Assert-True ($searchResultType.GetProperty('TerminationReason') -ne $null) `
|
||
'Search result must expose its original termination reason.'
|
||
```
|
||
|
||
在取消、节点上限和超时状态断言后分别加入：
|
||
|
||
```powershell
|
||
Assert-True (-not [string]::IsNullOrWhiteSpace($cancelledResult.TerminationReason)) `
|
||
'Cancelled search must retain a non-empty reason.'
|
||
Assert-True (-not [string]::IsNullOrWhiteSpace($limitedResult.TerminationReason)) `
|
||
'Node-limited search must retain a non-empty reason.'
|
||
Assert-True (-not [string]::IsNullOrWhiteSpace($timedOutResult.TerminationReason)) `
|
||
'Timed-out search must retain a non-empty reason.'
|
||
Assert-False ($cancelledResult.TerminationReason -eq $limitedResult.TerminationReason) `
|
||
'Cancelled and node-limited searches must retain different reasons.'
|
||
Assert-False ($limitedResult.TerminationReason -eq $timedOutResult.TerminationReason) `
|
||
'Node-limited and timed-out searches must retain different reasons.'
|
||
```
|
||
|
||
在脚本末尾通过内部预算重载制造一个可重复的未预期错误，并断言异常类型没有被吞掉：
|
||
|
||
```powershell
|
||
$internalSearchMethod = $searchType.GetMethods([Reflection.BindingFlags]'Instance,NonPublic') |
|
||
Where-Object {
|
||
$_.Name -eq 'Search' -and
|
||
$_.GetParameters().Length -eq 2 -and
|
||
$_.GetParameters()[1].ParameterType -eq $operationBudgetType
|
||
} |
|
||
Select-Object -First 1
|
||
Assert-True ($internalSearchMethod -ne $null) `
|
||
'Search must retain its internal shared-budget overload.'
|
||
$internalErrorResult = $internalSearchMethod.Invoke($searcher, @($searchRequest, $null))
|
||
Assert-Equal 'InternalError' $internalErrorResult.Status.ToString() `
|
||
'A missing internal budget must be mapped to InternalError.'
|
||
Assert-True ($internalErrorResult.TerminationReason.Contains('ArgumentNullException')) `
|
||
'Internal search errors must retain the exception type.'
|
||
```
|
||
|
||
- [ ] **Step 2: Run the search script and verify RED**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||
```
|
||
|
||
Expected: build succeeds; the script fails first with `Search result must expose its original termination reason.`
|
||
|
||
- [ ] **Step 3: Extend `HybridAStarSearchResult`**
|
||
|
||
Add the final constructor parameter, assignment, and public property:
|
||
|
||
```csharp
|
||
internal HybridAStarSearchResult(
|
||
PlanningStatus status,
|
||
IEnumerable<HybridAStarNode> nodes,
|
||
int expandedNodeCount,
|
||
int generatedNodeCount,
|
||
int reopenedNodeCount,
|
||
int staleOpenListEntryCount,
|
||
int peakOpenListCount,
|
||
int? successNodeIndex,
|
||
string terminationReason)
|
||
{
|
||
Status = status;
|
||
Nodes = new ReadOnlyCollection<HybridAStarNode>(
|
||
new List<HybridAStarNode>(nodes ?? Array.Empty<HybridAStarNode>()));
|
||
ExpandedNodeCount = expandedNodeCount;
|
||
GeneratedNodeCount = generatedNodeCount;
|
||
ReopenedNodeCount = reopenedNodeCount;
|
||
StaleOpenListEntryCount = staleOpenListEntryCount;
|
||
PeakOpenListCount = peakOpenListCount;
|
||
SuccessNodeIndex = status == PlanningStatus.Success ? successNodeIndex : null;
|
||
TerminationReason = status == PlanningStatus.Success
|
||
? string.Empty
|
||
: terminationReason ?? string.Empty;
|
||
}
|
||
|
||
/// <summary>搜索边界记录的原始终止原因；成功时为空字符串，失败时非空。</summary>
|
||
public string TerminationReason { get; }
|
||
```
|
||
|
||
- [ ] **Step 4: Centralize default reasons and retain exception details**
|
||
|
||
Replace `CreateResult` with:
|
||
|
||
```csharp
|
||
private static HybridAStarSearchResult CreateResult(
|
||
PlanningStatus status,
|
||
IEnumerable<HybridAStarNode> nodes,
|
||
int expandedNodeCount,
|
||
int generatedNodeCount,
|
||
int reopenedNodeCount,
|
||
int staleOpenListEntryCount,
|
||
int peakOpenListCount,
|
||
int? successNodeIndex,
|
||
string terminationReason = null)
|
||
{
|
||
string reason = status == PlanningStatus.Success
|
||
? string.Empty
|
||
: terminationReason ?? GetDefaultTerminationReason(status);
|
||
return new HybridAStarSearchResult(status, nodes, expandedNodeCount, generatedNodeCount, reopenedNodeCount,
|
||
staleOpenListEntryCount, peakOpenListCount, successNodeIndex, reason);
|
||
}
|
||
|
||
private static string GetDefaultTerminationReason(PlanningStatus status)
|
||
{
|
||
switch (status)
|
||
{
|
||
case PlanningStatus.Cancelled:
|
||
return "Hybrid A* 搜索已取消。";
|
||
case PlanningStatus.InvalidRequest:
|
||
return "Hybrid A* 搜索请求缺少必要对象或包含非法数值。";
|
||
case PlanningStatus.InvalidMap:
|
||
return "Hybrid A* 搜索地图结构无效。";
|
||
case PlanningStatus.MapNotReady:
|
||
return "Hybrid A* 搜索地图尚未准备好。";
|
||
case PlanningStatus.InvalidVehicleParameters:
|
||
return "Hybrid A* 搜索车辆参数无效。";
|
||
case PlanningStatus.InvalidCurvatureConfiguration:
|
||
return "Hybrid A* 搜索曲率、离散、代价或资源配置无效。";
|
||
case PlanningStatus.StartOutsideMap:
|
||
return "Hybrid A* 搜索起始扩大车体不完全位于地图内。";
|
||
case PlanningStatus.StartInCollision:
|
||
return "Hybrid A* 搜索起始扩大车体与障碍物相交或擦边。";
|
||
case PlanningStatus.GoalOutsideMap:
|
||
return "Hybrid A* 搜索目标扩大车体不完全位于地图内。";
|
||
case PlanningStatus.GoalInCollision:
|
||
return "Hybrid A* 搜索目标扩大车体与障碍物相交或擦边。";
|
||
case PlanningStatus.SearchTimeout:
|
||
return "Hybrid A* 搜索使用的总规划预算已耗尽。";
|
||
case PlanningStatus.SearchNodeLimitExceeded:
|
||
return "Hybrid A* 搜索达到扩展节点上限。";
|
||
case PlanningStatus.NoFeasiblePath:
|
||
return "Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。";
|
||
case PlanningStatus.BacktrackingFailed:
|
||
return "Hybrid A* 成功节点无法回溯为完整父链。";
|
||
case PlanningStatus.FinalValidationFailed:
|
||
return "Hybrid A* 路径未通过最终复核。";
|
||
case PlanningStatus.InternalError:
|
||
return "Hybrid A* 搜索发生未预期内部错误。";
|
||
default:
|
||
return "Hybrid A* 搜索以未识别状态终止：" + status + "。";
|
||
}
|
||
}
|
||
```
|
||
|
||
Change the two `NoFeasiblePath` exits so the caller can distinguish their location:
|
||
|
||
```csharp
|
||
if (openList.Count == 0)
|
||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount,
|
||
reopenedNodeCount, staleOpenListEntryCount, peakOpenListCount, null,
|
||
"二维启发式标记起点不可达目标，或起始方向无法进入 Open List。");
|
||
```
|
||
|
||
```csharp
|
||
return CreateResult(PlanningStatus.NoFeasiblePath, nodes, expandedNodeCount, generatedNodeCount,
|
||
reopenedNodeCount, staleOpenListEntryCount, peakOpenListCount, null,
|
||
"Hybrid A* 搜索的 Open List 已耗尽，未找到满足运动和碰撞约束的路径。");
|
||
```
|
||
|
||
Replace the catch block with:
|
||
|
||
```csharp
|
||
catch (Exception exception)
|
||
{
|
||
string reason = "Hybrid A* 搜索内部错误：" + exception.GetType().Name +
|
||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||
return CreateResult(PlanningStatus.InternalError, nodes, expandedNodeCount, generatedNodeCount,
|
||
reopenedNodeCount, staleOpenListEntryCount, peakOpenListCount, null, reason);
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 5: Run the search script and verify GREEN**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
|
||
```
|
||
|
||
Expected:
|
||
|
||
```text
|
||
Coarse path search primitive checks passed.
|
||
Coarse path Hybrid A star search checks passed.
|
||
```
|
||
|
||
- [ ] **Step 6: Commit the search reason contract**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs ClumsyPilot/tests/verify_coarse_path_search.ps1
|
||
git commit -m "fix: retain coarse path search failure reasons"
|
||
```
|
||
|
||
### Task 2: Add Planner-Level Diagnostics and Path Search Timing
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1:75-135,189-210,357-360`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs:6-60`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs:1-226`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 1 `HybridAStarSearchResult.TerminationReason`.
|
||
- Produces: `PlanningDiagnostics.PathSearchElapsed : TimeSpan`.
|
||
- Produces: `BuildSearchFailureReason(HybridAStarSearchResult, HybridAStarConfiguration) : string`.
|
||
- Contract: planner success and search-stage failure satisfy `0 <= PathSearchElapsed <= Elapsed`; map/preflight failure remains zero.
|
||
|
||
- [ ] **Step 1: Add failing planner-diagnostic assertions**
|
||
|
||
After the existing successful planner diagnostic assertions add:
|
||
|
||
```powershell
|
||
Assert-True ($result.Diagnostics.GetType().GetProperty('PathSearchElapsed') -ne $null) `
|
||
'Planner diagnostics must expose path-search elapsed time.'
|
||
Assert-True ($result.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) `
|
||
'Successful planning must retain non-negative path-search time.'
|
||
Assert-True ($result.Diagnostics.PathSearchElapsed -le $result.Diagnostics.Elapsed) `
|
||
'Path-search time must not exceed total elapsed time.'
|
||
```
|
||
|
||
After the invalid-map result assertions add:
|
||
|
||
```powershell
|
||
Assert-Equal ([TimeSpan]::Zero) $mapFailureResult.PlanningResult.Diagnostics.PathSearchElapsed `
|
||
'Map failure must report zero path-search time.'
|
||
```
|
||
|
||
After creating a valid planner request, add a search-stage node-limit case:
|
||
|
||
```powershell
|
||
$nodeLimitedRequest = [Activator]::CreateInstance($requestType)
|
||
$nodeLimitedRequest.Map = $request.Map
|
||
$nodeLimitedRequest.Start = $request.Start
|
||
$nodeLimitedRequest.Goal = $request.Goal
|
||
$nodeLimitedRequest.Vehicle = $request.Vehicle
|
||
$nodeLimitedRequest.Configuration = [Activator]::CreateInstance($configurationType)
|
||
$nodeLimitedRequest.Configuration.MaximumExpandedNodes = 0
|
||
$nodeLimitedRequest.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2)
|
||
$nodeLimitedRequest.Configuration.GoalPositionToleranceMeters = 0.001
|
||
$nodeLimitedRequest.Configuration.GoalHeadingToleranceRadians = 0.001
|
||
$nodeLimitedRequest.Configuration.AllowReverse = $false
|
||
$nodeLimitedRequest.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
||
$nodeLimitedResult = $plan.Invoke($planner, @($nodeLimitedRequest, [Threading.CancellationToken]::None))
|
||
Assert-Equal 'SearchNodeLimitExceeded' $nodeLimitedResult.Status.ToString() `
|
||
'A zero node limit must fail after planner preflight.'
|
||
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) `
|
||
'Search-stage node-limit failure must retain path-search time.'
|
||
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -le $nodeLimitedResult.Diagnostics.Elapsed) `
|
||
'Failed path-search time must not exceed total elapsed time.'
|
||
Assert-True (-not [string]::IsNullOrWhiteSpace($nodeLimitedResult.Diagnostics.TerminationReason)) `
|
||
'Planner diagnostics must retain a node-limit reason.'
|
||
```
|
||
|
||
Extend the no-path scenario assertions:
|
||
|
||
```powershell
|
||
Assert-True ($noPathResult.PlanningResult.Diagnostics.TerminationReason.Contains('Open List')) `
|
||
'No-path diagnostics must retain the exact search exhaustion reason.'
|
||
Assert-True ($noPathResult.PlanningResult.Diagnostics.TerminationReason.Contains('扩展=')) `
|
||
'No-path diagnostics must include search statistics.'
|
||
```
|
||
|
||
- [ ] **Step 2: Run integration checks and verify RED**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
Expected: build succeeds; the script fails first with `Planner diagnostics must expose path-search elapsed time.`
|
||
|
||
- [ ] **Step 3: Extend `PlanningDiagnostics` compatibly**
|
||
|
||
Append the optional constructor parameter:
|
||
|
||
```csharp
|
||
TimeSpan pathSearchElapsed = default(TimeSpan)
|
||
```
|
||
|
||
Assign it after `Elapsed`:
|
||
|
||
```csharp
|
||
Elapsed = elapsed;
|
||
PathSearchElapsed = pathSearchElapsed;
|
||
TerminationReason = terminationReason ?? string.Empty;
|
||
```
|
||
|
||
Add the property:
|
||
|
||
```csharp
|
||
/// <summary>
|
||
/// 地图和起终点预检通过后，二维启发式、Hybrid A*、回溯、装配和最终复核的耗时；
|
||
/// 不含建图，搜索前失败时为零。
|
||
/// </summary>
|
||
public TimeSpan PathSearchElapsed { get; }
|
||
```
|
||
|
||
Update the constructor XML comment so `elapsed` is described as total service elapsed time and `pathSearchElapsed` as the map-ready path-production elapsed time.
|
||
|
||
- [ ] **Step 4: Start the path-search stopwatch at the exact boundary**
|
||
|
||
Add:
|
||
|
||
```csharp
|
||
using System.Diagnostics;
|
||
using System.Globalization;
|
||
```
|
||
|
||
Declare the stopwatch before the `try`:
|
||
|
||
```csharp
|
||
Stopwatch pathSearchStopwatch = null;
|
||
```
|
||
|
||
Start it immediately before invoking the search:
|
||
|
||
```csharp
|
||
pathSearchStopwatch = Stopwatch.StartNew();
|
||
HybridAStarSearchResult searchResult = _search.Search(request, budget);
|
||
```
|
||
|
||
For `searchResult == null`, search failure, backtracking failure, assembly failure and final validation failure, pass `pathSearchStopwatch` into `Failure`. On success, pass its elapsed value:
|
||
|
||
```csharp
|
||
return PlanningResult.Success(path, segments, CreateDiagnostics(searchResult, budget.Elapsed,
|
||
pathLengthMeters, minimumClearanceMeters, string.Empty, pathSearchStopwatch.Elapsed));
|
||
```
|
||
|
||
Replace the planner catch block with:
|
||
|
||
```csharp
|
||
catch (Exception exception)
|
||
{
|
||
string reason = "规划内部错误：" + exception.GetType().Name +
|
||
(string.IsNullOrEmpty(exception.Message) ? "。" : "。" + exception.Message);
|
||
return Failure(PlanningStatus.InternalError,
|
||
budget ?? PlanningOperationBudget.Unlimited(CancellationToken.None),
|
||
reason, null, pathSearchStopwatch);
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 5: Compose actionable search diagnostics**
|
||
|
||
For non-success search results, use:
|
||
|
||
```csharp
|
||
if (searchResult.Status != PlanningStatus.Success)
|
||
return Failure(searchResult.Status, budget,
|
||
BuildSearchFailureReason(searchResult, request.Configuration),
|
||
searchResult, pathSearchStopwatch);
|
||
```
|
||
|
||
Add:
|
||
|
||
```csharp
|
||
private static string BuildSearchFailureReason(HybridAStarSearchResult searchResult,
|
||
HybridAStarConfiguration configuration)
|
||
{
|
||
string reason = string.IsNullOrEmpty(searchResult.TerminationReason)
|
||
? "Hybrid A* 搜索以 " + searchResult.Status + " 状态终止。"
|
||
: searchResult.TerminationReason;
|
||
string limit = string.Empty;
|
||
if (searchResult.Status == PlanningStatus.SearchTimeout)
|
||
{
|
||
limit = "总预算=" + configuration.SearchTimeout.TotalSeconds.ToString(
|
||
"F3", CultureInfo.InvariantCulture) + "s；";
|
||
}
|
||
else if (searchResult.Status == PlanningStatus.SearchNodeLimitExceeded)
|
||
{
|
||
limit = "节点上限=" + configuration.MaximumExpandedNodes.ToString(
|
||
CultureInfo.InvariantCulture) + "；";
|
||
}
|
||
|
||
return reason + limit +
|
||
"扩展=" + searchResult.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||
"生成=" + searchResult.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||
"重开=" + searchResult.ReopenedNodeCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||
"陈旧条目=" + searchResult.StaleOpenListEntryCount.ToString(CultureInfo.InvariantCulture) + "，" +
|
||
"Open List峰值=" + searchResult.PeakOpenListCount.ToString(CultureInfo.InvariantCulture) + "。";
|
||
}
|
||
```
|
||
|
||
Replace the two helper signatures and bodies:
|
||
|
||
```csharp
|
||
private static PlanningResult Failure(PlanningStatus status, PlanningOperationBudget budget, string reason,
|
||
HybridAStarSearchResult searchResult, Stopwatch pathSearchStopwatch = null)
|
||
{
|
||
TimeSpan pathSearchElapsed = pathSearchStopwatch == null
|
||
? TimeSpan.Zero
|
||
: pathSearchStopwatch.Elapsed;
|
||
return PlanningResult.Failure(status, CreateDiagnostics(searchResult, budget.Elapsed, 0d, 0d,
|
||
reason, pathSearchElapsed));
|
||
}
|
||
|
||
private static PlanningDiagnostics CreateDiagnostics(HybridAStarSearchResult searchResult, TimeSpan elapsed,
|
||
double pathLengthMeters, double minimumClearanceMeters, string reason, TimeSpan pathSearchElapsed)
|
||
{
|
||
return new PlanningDiagnostics(
|
||
searchResult == null ? 0 : searchResult.ExpandedNodeCount,
|
||
searchResult == null ? 0 : searchResult.GeneratedNodeCount,
|
||
searchResult == null ? 0 : searchResult.ReopenedNodeCount,
|
||
searchResult == null ? 0 : searchResult.StaleOpenListEntryCount,
|
||
searchResult == null ? 0 : searchResult.PeakOpenListCount,
|
||
pathLengthMeters,
|
||
minimumClearanceMeters,
|
||
elapsed,
|
||
reason,
|
||
pathSearchElapsed);
|
||
}
|
||
```
|
||
|
||
All search-preflight `Failure(...)` calls continue omitting the optional stopwatch and therefore report zero. All calls after `_search.Search` pass the running stopwatch.
|
||
|
||
- [ ] **Step 6: Run integration checks and verify GREEN**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
Expected:
|
||
|
||
```text
|
||
Coarse path integration checks passed.
|
||
Coarse path facade checks passed.
|
||
Coarse path P1 scenario checks passed.
|
||
```
|
||
|
||
- [ ] **Step 7: Commit planner diagnostics**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs ClumsyPilot/tests/verify_coarse_path_integration.ps1
|
||
git commit -m "feat: add actionable coarse path diagnostics"
|
||
```
|
||
|
||
### Task 3: Add Manual Timeout Input, UI Diagnostics, and Documentation
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_ui.ps1:19-92`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs:104-190,340-519`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md:114-116,285-359`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 2 `PlanningDiagnostics.PathSearchElapsed`.
|
||
- Produces: `ReadPositiveTimeoutInput(string) : TimeSpan`.
|
||
- Produces: `DrawStatus(string, CoarsePathPlanningJob, CoarsePathPlanningJobResult, PlanningGridMap)`.
|
||
- Contract: only the manual `[MovementTest(name = "粗路径规划")]` prompts for and overrides its request timeout; fixed scenarios keep their existing budgets.
|
||
|
||
- [ ] **Step 1: Add failing UI and README source checks**
|
||
|
||
After the current manual input assertions add:
|
||
|
||
```powershell
|
||
Assert-Match $source 'ReadPositiveTimeoutInput\s*\(' `
|
||
'The manual UI must read a finite positive timeout.'
|
||
Assert-Match $source 'Configuration\.SearchTimeout\s*=\s*searchTimeout' `
|
||
'The manual UI must apply the timeout to the current job.'
|
||
Assert-Match $source 'TimeSpan\.FromSeconds\s*\(' `
|
||
'The manual timeout must convert seconds to TimeSpan.'
|
||
Assert-Match $source 'timeoutSeconds\s*<=\s*0' `
|
||
'The manual timeout must reject zero and negative values.'
|
||
Assert-True (([regex]::Matches($source, 'PathSearchElapsed')).Count -ge 2) `
|
||
'The status layer and Toast must both show path-search elapsed time.'
|
||
Assert-Match $source 'BuildToastMessage[\s\S]*TerminationReason' `
|
||
'The failure Toast must include the termination reason.'
|
||
Assert-Match $source 'ExpandedNodeCount' `
|
||
'The status layer must show expanded-node statistics.'
|
||
Assert-Match $source 'VehicleKinematics\.TryGetMaximumCurvaturePerMeter' `
|
||
'The status layer must show the effective turning radius.'
|
||
```
|
||
|
||
Append these entries to `$readmeStructure`:
|
||
|
||
```powershell
|
||
'PathSearchElapsed',
|
||
'1.20 m',
|
||
'Reeds-Shepp',
|
||
```
|
||
|
||
- [ ] **Step 2: Run UI checks and verify RED**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
|
||
```
|
||
|
||
Expected: FAIL with `The manual UI must read a finite positive timeout.`
|
||
|
||
- [ ] **Step 3: Read and apply one-shot manual timeout**
|
||
|
||
Add:
|
||
|
||
```csharp
|
||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||
```
|
||
|
||
In `CoarsePathPlanningTest.Test()`, read the timeout after goal heading and apply it after creating the job:
|
||
|
||
```csharp
|
||
double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
|
||
TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
|
||
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);
|
||
job.Configuration.SearchTimeout = searchTimeout;
|
||
CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
|
||
```
|
||
|
||
Add:
|
||
|
||
```csharp
|
||
private static TimeSpan ReadPositiveTimeoutInput(string prompt)
|
||
{
|
||
double timeoutSeconds = ReadFiniteInput(prompt);
|
||
if (timeoutSeconds <= 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
|
||
try
|
||
{
|
||
return TimeSpan.FromSeconds(timeoutSeconds);
|
||
}
|
||
catch (OverflowException)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
|
||
}
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 4: Show timings, search counts, vehicle configuration, and failure reason**
|
||
|
||
Change the call site to:
|
||
|
||
```csharp
|
||
DrawStatus(scenarioName, job, result, map);
|
||
```
|
||
|
||
Replace `DrawStatus` with:
|
||
|
||
```csharp
|
||
private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
|
||
CoarsePathPlanningJobResult result, PlanningGridMap map)
|
||
{
|
||
float x = map == null ? 0f : map.Bounds.XMin + 150f;
|
||
float y = map == null ? -250f : map.Bounds.YMin + 150f;
|
||
string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
|
||
string resolution = map == null ? "无" :
|
||
map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
|
||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||
string reason = diagnostics.TerminationReason ?? string.Empty;
|
||
string turningRadius = "无";
|
||
if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(
|
||
job.Vehicle, out double maximumCurvaturePerMeter))
|
||
{
|
||
turningRadius = (1d / maximumCurvaturePerMeter).ToString(
|
||
"F2", CultureInfo.InvariantCulture) + " m";
|
||
}
|
||
|
||
Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
|
||
Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" +
|
||
result.MapResult.CacheHit + "，快照：" + snapshot, x, y + 120f);
|
||
Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" +
|
||
result.PlanningResult.Status + "，总耗时：" +
|
||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
|
||
" ms，路径搜索：" +
|
||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
|
||
" ms", x, y + 240f);
|
||
Painter.DrawText(Color.White, "节点：扩展=" +
|
||
diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) + "，生成=" +
|
||
diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
|
||
diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f);
|
||
if (job != null && job.Vehicle != null)
|
||
{
|
||
Painter.DrawText(Color.White, "演示车辆：长=" +
|
||
job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，宽=" +
|
||
job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
|
||
job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) +
|
||
" m，最小转弯半径=" + turningRadius, x, y + 480f);
|
||
}
|
||
if (!string.IsNullOrEmpty(reason))
|
||
Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f);
|
||
}
|
||
```
|
||
|
||
Replace `BuildToastMessage` with:
|
||
|
||
```csharp
|
||
private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
|
||
{
|
||
PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
|
||
string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status +
|
||
"，规划=" + result.PlanningResult.Status + "，总耗时=" +
|
||
diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) +
|
||
"ms，路径搜索=" +
|
||
diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
|
||
if (result.PlanningResult.Status != PlanningStatus.Success &&
|
||
!string.IsNullOrEmpty(diagnostics.TerminationReason))
|
||
{
|
||
message += "，原因=" + diagnostics.TerminationReason;
|
||
}
|
||
return message + "。";
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 5: Document the exact manual-test boundaries**
|
||
|
||
After “总预算与取消” add:
|
||
|
||
```markdown
|
||
### 总耗时与路径搜索耗时
|
||
|
||
`PlanningDiagnostics.Elapsed` 是从 `CoarsePathPlanningService.Plan` 开始的总耗时，包含
|
||
地图来源、缓存、栅格化、距离场和路径规划。`PathSearchElapsed` 是地图和起终点预检
|
||
通过后的路径搜索耗时，包含二维启发式、Hybrid A*、回溯、装配、方向分段和最终复核；
|
||
搜索开始前失败时为零。
|
||
```
|
||
|
||
In “AMR 位姿、手动终点与障碍物”, add:
|
||
|
||
```markdown
|
||
手动入口还要求输入一次“粗路径规划总超时”，单位为秒，只接受 `TimeSpan` 可表示范围内
|
||
的有限正数秒；`0`、负数、NaN、Infinity 或溢出值都会在启动规划前拒绝。该值只覆盖
|
||
本次 `CoarsePathPlanningJob.Configuration.SearchTimeout`，不会改变固定场景或全局默认值。
|
||
|
||
此入口使用固定演示车辆：长 `0.80 m`、宽 `0.60 m`、四周安全余量 `0.05 m`、最小转弯
|
||
半径 `1.20 m`。这些值不是从现场 AMR 配置读取的，判断现场可行性前必须确认车辆参数一致。
|
||
```
|
||
|
||
In “后台执行、停止与图层”, add:
|
||
|
||
```markdown
|
||
状态图层显示规划状态、总耗时、`PathSearchElapsed`（路径搜索耗时）、扩展/生成节点数、
|
||
Open List 峰值、失败原因和固定演示车辆参数。Toast 同时显示两种耗时，并在失败时附加
|
||
`TerminationReason`，因此超时、节点上限、无解、碰撞和内部错误不会只显示成泛化失败。
|
||
```
|
||
|
||
Extend “第一版限制” with:
|
||
|
||
```markdown
|
||
- Reeds-Shepp 或 Dubins 精确终点连接；
|
||
- 原地旋转。
|
||
|
||
当前只生成汽车式恒曲率前进/倒车原语，并允许在原语边界换向；未实现的 Reeds-Shepp、
|
||
横移、蟹行和原地旋转是整个粗规划核心的第一版能力边界，不是 MovementTest 单独关闭。
|
||
```
|
||
|
||
- [ ] **Step 6: Run UI checks and verify GREEN**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
|
||
```
|
||
|
||
Expected:
|
||
|
||
```text
|
||
Coarse path P1 UI source checks passed.
|
||
```
|
||
|
||
- [ ] **Step 7: Commit the manual-test UX**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md ClumsyPilot/tests/verify_coarse_path_ui.ps1
|
||
git commit -m "feat: expose coarse path test diagnostics"
|
||
```
|
||
|
||
### Task 4: Regress the Slow Feasible Case and Run the Full Suite
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/tests/verify_coarse_path_integration.ps1:273-369`
|
||
- Verify: all files modified by Tasks 1-3
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 3 manual scenario factory and configurable `SearchTimeout`.
|
||
- Produces: regression proof that the formerly 5-second-limited feasible pose succeeds under a caller-selected longer budget.
|
||
- Contract: the regression uses a 30 秒 upper bound without asserting an exact wall-clock duration.
|
||
|
||
- [ ] **Step 1: Add the slow feasible-case regression**
|
||
|
||
After the manual empty-map factory assertions, add:
|
||
|
||
```powershell
|
||
$slowFeasibleJob = $factoryManual.Invoke($null, @(
|
||
1000.0, 2000.0, 0.0, 1500.0, 2500.0, 90.0))
|
||
$slowFeasibleJob.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(30)
|
||
$slowFeasibleJob.Configuration.MaximumExpandedNodes = 1000000
|
||
$slowFeasibleService = [Activator]::CreateInstance($serviceType)
|
||
$slowFeasibleResult = $servicePlan.Invoke(
|
||
$slowFeasibleService, @($slowFeasibleJob, [Threading.CancellationToken]::None))
|
||
Assert-Equal 'Success' $slowFeasibleResult.PlanningResult.Status.ToString() `
|
||
'The previously five-second-limited feasible pose must succeed with a caller-selected longer budget.'
|
||
Assert-True ($slowFeasibleResult.PlanningResult.Diagnostics.PathSearchElapsed -le
|
||
$slowFeasibleResult.PlanningResult.Diagnostics.Elapsed) `
|
||
'Slow feasible path-search time must remain within total elapsed time.'
|
||
```
|
||
|
||
- [ ] **Step 2: Run the integration script and confirm the regression passes**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
|
||
```
|
||
|
||
Expected:
|
||
|
||
```text
|
||
Coarse path integration checks passed.
|
||
Coarse path facade checks passed.
|
||
Coarse path P1 scenario checks passed.
|
||
```
|
||
|
||
The slow case is allowed up to 30 seconds and should normally complete near the observed 10-second baseline; do not assert an exact wall-clock value.
|
||
|
||
- [ ] **Step 3: Run all coarse-path and map regressions**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_documentation.ps1
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_test_config.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
|
||
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_ui.ps1
|
||
```
|
||
|
||
Expected: build exits 0 and every script prints its existing `passed` summary without an unhandled exception.
|
||
|
||
- [ ] **Step 4: Inspect the final diff for scope and accidental edits**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
git diff -- ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/HybridAStarSearch.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/Contracts/PlanningDiagnostics.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/HybridAStarPlanner.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/Test/MovementTest.CoarsePathTest.cs ClumsyPilot/ParkrobTrajplanner/CoarsePath/README.md ClumsyPilot/tests/verify_coarse_path_search.ps1 ClumsyPilot/tests/verify_coarse_path_integration.ps1 ClumsyPilot/tests/verify_coarse_path_ui.ps1
|
||
```
|
||
|
||
Expected: only the approved reason propagation, timing, manual timeout, UI, documentation and tests are present; collision/search semantics and unrelated worktree files are unchanged.
|
||
|
||
- [ ] **Step 5: Perform the Clumsy UI acceptance**
|
||
|
||
Run `[MovementTest(name = "粗路径规划")]` with:
|
||
|
||
```text
|
||
Start from current AMR pose corresponding to: 1000 mm, 2000 mm, 0 deg
|
||
Goal X: 1500 mm
|
||
Goal Y: 2500 mm
|
||
Goal heading: 90 deg
|
||
Timeout: 30 seconds
|
||
Manual obstacle count: 0
|
||
```
|
||
|
||
Confirm:
|
||
|
||
```text
|
||
Planning status: Success
|
||
Toast and status layer both show total elapsed and path-search elapsed
|
||
Status layer shows expanded/generated/Open List peak and the fixed demo vehicle parameters
|
||
```
|
||
|
||
Then run an intentionally impossible barrier or a deliberately short positive timeout and confirm the Toast includes a non-empty reason.
|
||
|
||
- [ ] **Step 6: Commit the regression coverage**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/tests/verify_coarse_path_integration.ps1
|
||
git commit -m "test: cover slow feasible coarse path planning"
|
||
```
|