docs: plan workstation-bounded trap map
This commit is contained in:
梁薄云
@@ -0,0 +1,279 @@
|
||||
# Workstation-Bounded Layered Trap Map 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:** Build a task-level world-coordinate grid bounded by the current vehicle, an adjustable workstation, and independent X/Y margins; tire detection remains an optional layer.
|
||||
|
||||
**Architecture:** `TrapMapBuilder` validates required inputs, reads the Detour pose, calculates the vehicle/workstation rectangle, creates the base grid, and marks the self layer before attempting tire detection. Tire acquisition records layer health and optionally adds occupancy without controlling map success. `GridMapData` remains the merged output so a future point-cloud layer can rasterize into the same map through a separate step.
|
||||
|
||||
**Tech Stack:** C# 10, .NET Standard 2.0, ClumsyCore/ClumsyDance, PowerShell reflection/contract tests, `dotnet msbuild`.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Do not inspect or modify `TrajPlanner`.
|
||||
- Workstation coordinates use the Detour world frame and millimetres.
|
||||
- Defaults: `WorkstationX=10000f`, `WorkstationY=0f`, `MapMarginX=3000f`, `MapMarginY=3000f`.
|
||||
- `Free` means “not marked by an integrated source,” not sensor-confirmed obstacle-free.
|
||||
- Do not invent a point-cloud API or send chassis commands.
|
||||
- Reject maps above `4_000_000` cells before allocating `byte[,]`.
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Executable task-bound calculation
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/MovementTest.Trapmaptest.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_trapmap_grid.ps1`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: vehicle/workstation coordinates, X/Y margins, resolution, cell limit.
|
||||
- Produces: `TrapMapBounds.TryCreate(float carX, float carY, float workstationX, float workstationY, float marginX, float marginY, float resolutionMm, int maxCellCount, out TrapMapBounds bounds, out string failureReason)` plus `XMin`, `XMax`, `YMin`, `YMax`, `Rows`, `Cols`, `CellCount`.
|
||||
|
||||
- [ ] **Step 1: Write failing reflection tests**
|
||||
|
||||
Append to `verify_trapmap_grid.ps1`:
|
||||
|
||||
```powershell
|
||||
$boundsType = $assembly.GetType('MultiWheelC.TrapMapBounds', $true)
|
||||
$tryCreate = $boundsType.GetMethod('TryCreate')
|
||||
function Invoke-Bounds([single]$carX, [single]$carY, [single]$stationX, [single]$stationY,
|
||||
[single]$marginX, [single]$marginY, [single]$resolution, [int]$maxCells) {
|
||||
$args = @($carX, $carY, $stationX, $stationY, $marginX, $marginY,
|
||||
$resolution, $maxCells, $null, $null)
|
||||
$ok = $tryCreate.Invoke($null, $args)
|
||||
[pscustomobject]@{ Ok=$ok; Bounds=$args[8]; Reason=$args[9] }
|
||||
}
|
||||
$result = Invoke-Bounds 2000 -1000 10000 0 3000 4000 50 4000000
|
||||
Assert-Equal $true $result.Ok 'Valid bounds must succeed.'
|
||||
Assert-Equal ([single]-1000) $result.Bounds.XMin 'Wrong XMin.'
|
||||
Assert-Equal ([single]13000) $result.Bounds.XMax 'Wrong XMax.'
|
||||
Assert-Equal ([single]-5000) $result.Bounds.YMin 'Wrong YMin.'
|
||||
Assert-Equal ([single]4000) $result.Bounds.YMax 'Wrong YMax.'
|
||||
Assert-Equal 280 $result.Bounds.Cols 'Wrong column count.'
|
||||
Assert-Equal 180 $result.Bounds.Rows 'Wrong row count.'
|
||||
Assert-Equal $false (Invoke-Bounds 0 0 100000 100000 0 0 20 4000000).Ok 'Oversized map must fail.'
|
||||
Assert-Equal $false (Invoke-Bounds 0 0 10000 0 -1 3000 50 4000000).Ok 'Negative margin must fail.'
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Prove the new test fails**
|
||||
|
||||
Run:
|
||||
|
||||
```powershell
|
||||
dotnet msbuild ClumsyPilot\ClumsyPilot.csproj /t:Compile /p:RestoreIgnoreFailedSources=true /v:minimal
|
||||
powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_grid.ps1
|
||||
```
|
||||
|
||||
Expected: compile succeeds; the script fails because `MultiWheelC.TrapMapBounds` is absent.
|
||||
|
||||
- [ ] **Step 3: Implement the bounds object**
|
||||
|
||||
Add `TrapMapBounds` beside the input models. `TryCreate` must validate every float with `TrapMapValue.IsFinite`, require margins `>=0`, resolution and limit `>0`, calculate:
|
||||
|
||||
```csharp
|
||||
float xMin = Math.Min(carX, workstationX) - marginX;
|
||||
float xMax = Math.Max(carX, workstationX) + marginX;
|
||||
float yMin = Math.Min(carY, workstationY) - marginY;
|
||||
float yMax = Math.Max(carY, workstationY) + marginY;
|
||||
int cols = (int)Math.Ceiling((xMax - xMin) / resolutionMm);
|
||||
int rows = (int)Math.Ceiling((yMax - yMin) / resolutionMm);
|
||||
long cellCount = (long)rows * cols;
|
||||
```
|
||||
|
||||
Reject non-finite/degenerate boundaries and `cellCount > maxCellCount` before returning a populated immutable-result object. Return a concrete Chinese reason for every rejection.
|
||||
|
||||
- [ ] **Step 4: Recompile and run behavior tests**
|
||||
|
||||
Run the Step 2 commands. Expected final line: `TrapMap GridMapData behavior checks passed.`
|
||||
|
||||
- [ ] **Step 5: Commit**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/MovementTest.Trapmaptest.cs ClumsyPilot/tests/verify_trapmap_grid.ps1
|
||||
git commit -m "feat: calculate workstation-bounded trap map"
|
||||
```
|
||||
|
||||
### Task 2: Base-map-first lifecycle and optional tire layer
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/MovementTest.Trapmaptest.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_trapmap_inputs.ps1`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Task 1 `TrapMapBounds.TryCreate` with its declared ten parameters, Detour pose, car dimensions, optional `TwoLegDetect` output.
|
||||
- Produces: `WorkstationX`, `WorkstationY`, `MapMarginX`, `MapMarginY`, `WorkstationWorld`, `TrapMapTireLayerStatus TireLayerStatus`, and `string TireLayerMessage`.
|
||||
|
||||
- [ ] **Step 1: Add failing lifecycle contracts**
|
||||
|
||||
Add to `verify_trapmap_inputs.ps1`:
|
||||
|
||||
```powershell
|
||||
if ($source -match '\bMapHalfSizeMm\b') { $failures.Add('Legacy MapHalfSizeMm remains.') }
|
||||
foreach ($name in 'WorkstationX','WorkstationY','MapMarginX','MapMarginY','TrapMapBounds','TireLayerStatus','TireLayerMessage') {
|
||||
if ($source -notmatch "\b$name\b") { $failures.Add("Missing layered-map member: $name") }
|
||||
}
|
||||
if ($source -notmatch 'WorkstationX\s*=\s*10000f' -or $source -notmatch 'WorkstationY\s*=\s*0f' -or
|
||||
$source -notmatch 'MapMarginX\s*=\s*3000f' -or $source -notmatch 'MapMarginY\s*=\s*3000f') {
|
||||
$failures.Add('Required task-map defaults are missing.')
|
||||
}
|
||||
$createIndex = $source.IndexOf('TryCreateGridMap(')
|
||||
$detectIndex = $source.IndexOf('ReadTireLayer(')
|
||||
if ($createIndex -lt 0 -or $detectIndex -lt 0 -or $createIndex -gt $detectIndex) {
|
||||
$failures.Add('Base map must precede optional tire acquisition.')
|
||||
}
|
||||
if ($source -notmatch 'TRAPMAP_FREE_SEMANTICS') { $failures.Add('Free semantics are undocumented.') }
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Run the contract and confirm failure**
|
||||
|
||||
Run `powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_inputs.ps1`.
|
||||
|
||||
Expected: legacy half-size, missing members, and ordering assertions fail.
|
||||
|
||||
- [ ] **Step 3: Add task-map configuration and status types**
|
||||
|
||||
Replace `MapHalfSizeMm` with:
|
||||
|
||||
```csharp
|
||||
public float WorkstationX { get; set; } = 10000f;
|
||||
public float WorkstationY { get; set; } = 0f;
|
||||
public float MapMarginX { get; set; } = 3000f;
|
||||
public float MapMarginY { get; set; } = 3000f;
|
||||
public const int MaxCellCount = 4_000_000;
|
||||
public Vector2 WorkstationWorld { get; private set; }
|
||||
public TrapMapTireLayerStatus TireLayerStatus { get; private set; }
|
||||
public string TireLayerMessage { get; private set; }
|
||||
```
|
||||
|
||||
Define enum values `NotAttempted`, `Populated`, `NoDetection`, `Unavailable`, `Simulated`. Validate finite workstation coordinates and non-negative finite margins.
|
||||
|
||||
- [ ] **Step 4: Reorder `Get()` and split population**
|
||||
|
||||
Implement this required sequence:
|
||||
|
||||
```csharp
|
||||
VehiclePose = vehiclePose;
|
||||
WorkstationWorld = new Vector2(WorkstationX, WorkstationY);
|
||||
if (!TryCreateGridMap(out var gridMap, out failureReason) ||
|
||||
!TryPopulateSelfLayer(gridMap, out failureReason)) {
|
||||
Fail(failureReason);
|
||||
yield break;
|
||||
}
|
||||
ReadTireLayer(self);
|
||||
TryPopulateTireLayer(gridMap);
|
||||
```
|
||||
|
||||
`TryCreateGridMap` calls Task 1's ten-parameter `TrapMapBounds.TryCreate` and constructs `GridMapData` from the returned limits. The self method marks only the vehicle. Tire population may log a rasterization problem but must not clear or suppress the valid base map.
|
||||
|
||||
- [ ] **Step 5: Make tire acquisition non-blocking**
|
||||
|
||||
Rename the required-input method to `ReadTireLayer`. Every exit assigns an empty or populated list plus status/message. Use:
|
||||
|
||||
```csharp
|
||||
private void SetEmptyTireLayer(TrapMapTireLayerStatus status, string message)
|
||||
{
|
||||
DetectedObstacles = new List<TrapMapObstacle>();
|
||||
TireLayerStatus = status;
|
||||
TireLayerMessage = message;
|
||||
InputSource = "无轮胎障碍输入";
|
||||
DLog.Log($"轮胎层为空: status={status}, reason={message}", "TrapMapTest");
|
||||
}
|
||||
```
|
||||
|
||||
Real-mode empty lidar/config errors/exceptions/illegal coordinates become `Unavailable`; `detected == null` becomes `NoDetection`; valid endpoints become `Populated`. Ghost simulation remains `Simulated` and is never a real-mode fallback.
|
||||
|
||||
Add beside base-map creation:
|
||||
|
||||
```csharp
|
||||
// TRAPMAP_FREE_SEMANTICS: 当前 Free 仅表示尚未被已接入层标记，
|
||||
// 不表示传感器确认现实中无障碍。未来实时点云应作为独立障碍层；
|
||||
// 引入 Unknown 后，未观测栅格不应默认允许规划通行。
|
||||
```
|
||||
|
||||
- [ ] **Step 6: Verify and commit**
|
||||
|
||||
Run `powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_inputs.ps1`, `dotnet msbuild ClumsyPilot\ClumsyPilot.csproj /t:Compile /p:RestoreIgnoreFailedSources=true /v:minimal`, and `powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_grid.ps1`. Expected: both scripts exit 0 and compile has no TrapMap errors. Then:
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/MovementTest.Trapmaptest.cs ClumsyPilot/tests/verify_trapmap_inputs.ps1
|
||||
git commit -m "fix: build trap map before optional tire layer"
|
||||
```
|
||||
|
||||
### Task 3: Adjustable test entry and workstation visualization
|
||||
|
||||
**Files:**
|
||||
- Modify: `ClumsyPilot/MovementTest.Trapmaptest.cs`
|
||||
- Modify: `ClumsyPilot/tests/verify_trapmap_inputs.ps1`
|
||||
|
||||
**Interfaces:**
|
||||
- Consumes: Task 2 builder configuration/status.
|
||||
- Produces: editable test defaults, non-occupying target marker, exact bounds/layer logs.
|
||||
|
||||
- [ ] **Step 1: Add failing UI/source contracts**
|
||||
|
||||
```powershell
|
||||
if ($source -notmatch '_workstationX\s*=\s*10000f' -or $source -notmatch '_workstationY\s*=\s*0f' -or
|
||||
$source -notmatch '_mapMarginX\s*=\s*3000f' -or $source -notmatch '_mapMarginY\s*=\s*3000f') {
|
||||
$failures.Add('MovementTest adjustable defaults are missing.')
|
||||
}
|
||||
if ($source -notmatch 'DrawWorkstationMarker\(') { $failures.Add('Target visualization is missing.') }
|
||||
if ($source -match 'MarkObstacle\(Workstation|MarkOccupied\([^\r\n]*Workstation') {
|
||||
$failures.Add('Workstation must not become occupancy.')
|
||||
}
|
||||
```
|
||||
|
||||
- [ ] **Step 2: Prove the contract fails**
|
||||
|
||||
Run the input script. Expected: missing test defaults and target visualization are reported.
|
||||
|
||||
- [ ] **Step 3: Replace manual-edit fields and pass them to builder**
|
||||
|
||||
```csharp
|
||||
private const float _gridResolutionMm = 50f;
|
||||
private const float _workstationX = 10000f;
|
||||
private const float _workstationY = 0f;
|
||||
private const float _mapMarginX = 3000f;
|
||||
private const float _mapMarginY = 3000f;
|
||||
private const float _safetyMarginMm = 300f;
|
||||
```
|
||||
|
||||
Pass all values into `TrapMapBuilder`. Log vehicle, workstation, limits, dimensions, cells, `TireLayerStatus`, and `TireLayerMessage`. Remove all half-size/20m-window documentation.
|
||||
|
||||
- [ ] **Step 4: Draw the workstation without marking occupancy**
|
||||
|
||||
```csharp
|
||||
private void DrawWorkstationMarker(Painter painter)
|
||||
{
|
||||
const float radius = 250f;
|
||||
painter.DrawCircle(Color.Lime, WorkstationWorld.X, WorkstationWorld.Y, radius);
|
||||
painter.DrawLine(Color.Lime, WorkstationWorld.X - radius, WorkstationWorld.Y,
|
||||
WorkstationWorld.X + radius, WorkstationWorld.Y, width: 3);
|
||||
painter.DrawLine(Color.Lime, WorkstationWorld.X, WorkstationWorld.Y - radius,
|
||||
WorkstationWorld.X, WorkstationWorld.Y + radius, width: 3);
|
||||
painter.DrawText(Color.Lime, "Workstation",
|
||||
WorkstationWorld.X + radius, WorkstationWorld.Y + radius);
|
||||
}
|
||||
```
|
||||
|
||||
Call it from visualization; never pass `WorkstationWorld` to occupancy methods.
|
||||
|
||||
- [ ] **Step 5: Update comments and run final verification**
|
||||
|
||||
Document formulas, defaults, task-level “global” meaning, layer order, optional tire behavior, and future independent point-cloud layer. Run:
|
||||
|
||||
```powershell
|
||||
powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_inputs.ps1
|
||||
dotnet msbuild ClumsyPilot\ClumsyPilot.csproj /t:Compile /p:RestoreIgnoreFailedSources=true /v:minimal
|
||||
powershell -ExecutionPolicy Bypass -File ClumsyPilot\tests\verify_trapmap_grid.ps1
|
||||
rg -n "MapHalfSizeMm|GetSensor\(|GetPointCloud\(|PredefinedDriveStop\(|SendMotion\(|SendRotateMotion\(" ClumsyPilot\MovementTest.Trapmaptest.cs
|
||||
git diff --check
|
||||
```
|
||||
|
||||
Expected: scripts pass, compile has no TrapMap errors, forbidden search has no matches, and diff check is clean.
|
||||
|
||||
- [ ] **Step 6: Commit**
|
||||
|
||||
```powershell
|
||||
git add ClumsyPilot/MovementTest.Trapmaptest.cs ClumsyPilot/tests/verify_trapmap_inputs.ps1
|
||||
git commit -m "feat: expose workstation trap map inputs"
|
||||
```
|
||||
Reference in New Issue
Block a user