docs: plan EM closed-loop movement test

This commit is contained in:
梁薄云
2026-08-10 13:14:17 +08:00
parent f98fc759af
commit f923affc8f
@@ -0,0 +1,508 @@
# EM Closed-Loop Movement Test 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:** Add an `EM闭环测试` MovementTest that plans one complete EM direction segment once, freezes it, adapts it to `Trajectory2D`, and tracks it with the imported Stanley/PID/GCP controller until that segment stops.
**Architecture:** Keep the planning and controller models separate. Restore the reference controller's smallest portable runtime dependency set inside `ClumsyPilot`, add a pure `EmTrajectory`-to-`Trajectory2D` adapter, then make a session runner reuse the existing trajectory-observation bootstrap and one `TrajectoryObservationController.StartCycle` call before handing the frozen trajectory to `TrajectoryTrackingMovement`.
**Tech Stack:** C# 10, .NET Standard 2.0 plugin, .NET 10 Windows verification host, Clumsy `MovementTest`/`DriveTask`, existing EM planner and OSQP runtime.
## Global Constraints
- The MovementTest registration name is exactly `EM闭环测试`.
- Plan exactly once in `EmPlanningScope.FullDirectionSegment`; do not replan or replace the trajectory while driving.
- Execute only the first direction segment. A gear-switch terminal stops and ends this test.
- Shared/control boundaries use SI units: m, m/s, rad, rad/s; body X is forward, body Y is left, counter-clockwise is positive.
- Preserve signed longitudinal velocity: forward positive and reverse negative.
- Do not change velocity/steering signs, CAN IDs, remote mappings, mechanical limits, or mode-switch policy.
- Do not overwrite unrelated dirty files. Stage only the exact files named by each task.
- Real-vehicle validation must remain low-speed, short-distance, in an open area, with hardware emergency stop available.
---
## File Map
### Existing user files retained and brought under version control
- `ClumsyPilot/Control/**/*.cs` — Stanley/PID/GCP controller implementation already copied from the reference project.
- `ClumsyPilot/Trajectory/**/*.cs` — controller-side trajectory and projection implementation already copied from the reference project.
### Portable runtime dependencies copied from the reference project
- `ClumsyPilot/Shared/Mathematics/AngleMath.cs` — angle normalization and conversion.
- `ClumsyPilot/Shared/Mathematics/InterpolationMath.cs` — scalar interpolation.
- `ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs` — world/body transforms.
- `ClumsyPilot/Shared/Models/ChassisCommand.cs``Pose2D`, `Twist2D`, and chassis command value types.
- `ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs` — SI/GCP boundary over the existing `MultiWheelChassis`.
- `ClumsyPilot/StateEstimation/IVehicleStateProvider.cs` — controller state-source boundary.
- `ClumsyPilot/StateEstimation/VehicleState.cs` — immutable controller state.
- `ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs` — velocity filter primitive.
- `ClumsyPilot/StateEstimation/VelocityEstimator2D.cs` — Detour velocity estimation.
- `ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs` — live Detour state source.
- `ClumsyPilot/Movements/TrajectoryTrackingMovement.cs``DriveTask` movement wrapper around the geometric controller.
### New EM closed-loop files
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs` — pure model adapter.
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs` — UI input, session ownership, one-shot planning, controller handoff, and stop behavior.
- `ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs` — adapter behavior checks.
- `ClumsyPilot/tests/verify_em_closed_loop_movement.ps1` — MovementTest registration and one-shot lifecycle checks.
- `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs` — add the `em-control-adapter` verification command.
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md` — operator procedure and test boundary.
---
### Task 1: Restore the controller's portable runtime dependency chain
**Files:**
- Add and track: `ClumsyPilot/Control/**/*.cs`
- Add and track: `ClumsyPilot/Trajectory/**/*.cs`
- Create: `ClumsyPilot/Shared/Mathematics/AngleMath.cs`
- Create: `ClumsyPilot/Shared/Mathematics/InterpolationMath.cs`
- Create: `ClumsyPilot/Shared/Mathematics/FrameTransform2D.cs`
- Create: `ClumsyPilot/Shared/Models/ChassisCommand.cs`
- Create: `ClumsyPilot/Shared/Chassis/MultiWheelChassisAdapter.cs`
- Create: `ClumsyPilot/StateEstimation/IVehicleStateProvider.cs`
- Create: `ClumsyPilot/StateEstimation/VehicleState.cs`
- Create: `ClumsyPilot/StateEstimation/FirstOrderLowPassFilter.cs`
- Create: `ClumsyPilot/StateEstimation/VelocityEstimator2D.cs`
- Create: `ClumsyPilot/StateEstimation/DetourVehicleStateProvider.cs`
- Create: `ClumsyPilot/Movements/TrajectoryTrackingMovement.cs`
**Interfaces:**
- Produces: `MyParking.Shared.Pose2D`, `Twist2D`, `AngleMath`, `MultiWheelChassisAdapter`.
- Produces: `MultiWheelC.StateEstimation.IVehicleStateProvider` and `DetourVehicleStateProvider`.
- Produces: `MultiWheelC.Trajectory.Trajectory2D` and `TrajectoryPoint`.
- Produces: `MultiWheelC.TrajectoryTrackingMovement : MovementDefinition` with public `Trajectory`, `StateProvider`, `CycleObserver`, and safety-limit fields.
- [ ] **Step 1: Re-run the failing integration build**
Run:
```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```
Expected: FAIL with missing `MyParking.Shared`, `MultiWheelC.StateEstimation`, `VehicleState`, and `MultiWheelChassisAdapter`. This proves the partial controller import is not silently excluded from the plugin build.
- [ ] **Step 2: Add the exact portable reference sources**
Read each source from:
```text
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\Shared\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\StateEstimation\...
D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC\Movements\TrajectoryTrackingMovement.cs
```
Add the files at the destination paths in this task without changing namespaces or logic. Do not import `FleetKinematics`, experiment recorders, composite movements, rotation movements, or wheel-feedback providers because `TrajectoryTrackingMovement` does not consume them.
- [ ] **Step 3: Prove existing controller/trajectory files still match the reference implementation**
Run:
```powershell
$reference = 'D:\Users\Desktop\项目\prakrobot\停车机器人-合并测试\parkr_shen\MultiWheelC'
$local = (Resolve-Path '.\ClumsyPilot').Path
Get-ChildItem '.\ClumsyPilot\Control','.\ClumsyPilot\Trajectory' -File -Recurse | ForEach-Object {
$relative = $_.FullName.Substring($local.Length + 1)
$source = Join-Path $reference $relative
if (-not (Test-Path $source) -or (Get-FileHash $_.FullName).Hash -ne (Get-FileHash $source).Hash) {
throw "Reference mismatch: $relative"
}
}
```
Expected: exit 0 and no mismatch.
- [ ] **Step 4: Run the build to verify the dependency chain compiles**
Run:
```powershell
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```
Expected: PASS with zero errors. Record warnings verbatim; do not hide them.
- [ ] **Step 5: Commit only the controller runtime integration**
```powershell
git add -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements/TrajectoryTrackingMovement.cs
git diff --cached --check
git commit -m "feat: integrate trajectory tracking controller runtime"
```
---
### Task 2: Adapt a frozen EM trajectory to `Trajectory2D`
**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs`
- Create: `ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs`
**Interfaces:**
- Consumes: `EmTrajectory` with validated SI-unit `EmTrajectoryPoint` values.
- Produces: `public sealed class EmControlTrajectoryAdapter`.
- Produces: `public Trajectory2D Create(EmTrajectory trajectory)`.
- Duplicate-position threshold: `1e-6 m`, matching `Trajectory2D`'s minimum valid projection segment.
- [ ] **Step 1: Register the adapter check before implementing the adapter**
Extend `Program.cs` argument validation with `em-control-adapter`, and add:
```csharp
if (args[0] == "em-control-adapter" || args[0] == "em-all")
{
EmControlTrajectoryAdapterChecks.Run();
Console.WriteLine("PASS em-control-adapter");
}
```
Create `EmControlTrajectoryAdapterChecks.cs` with checks that build real `EmTrajectory` objects and assert:
```csharp
internal static void Run()
{
MapsFieldsAndRebuildsGeometricArcLength();
KeepsReverseVelocityNegative();
CollapsesTerminalHoldAndKeepsItsZeroSpeed();
RejectsFewerThanTwoDistinctPositions();
}
```
The forward mapping check must assert all of these exact relations:
```csharp
Trajectory2D actual = new EmControlTrajectoryAdapter().Create(source);
Require(actual.Count == 3, "adapter point count");
RequireClose(actual[0].ArcLengthMeters, 0d, "first arc");
RequireClose(actual[1].ArcLengthMeters, 0.5d, "second arc");
RequireClose(actual[2].ArcLengthMeters, 1.0d, "third arc");
RequireClose(actual[1].PoseInWorld.XMeters, source.Points[1].X, "x");
RequireClose(actual[1].PoseInWorld.YMeters, source.Points[1].Y, "y");
RequireClose(actual[1].PoseInWorld.YawRadians, source.Points[1].Yaw, "yaw");
RequireClose(actual[1].CurvaturePerMeter, source.Points[1].VehicleCurvature, "curvature");
RequireClose(actual[1].ReferenceSpeedMetersPerSecond,
source.Points[1].SignedLongitudinalVelocity, "signed speed");
```
For the terminal-hold case, provide two final EM samples at the same pose where the later sample has zero speed, then assert that the output contains one terminal position and `EndPoint.ReferenceSpeedMetersPerSecond == 0d`.
- [ ] **Step 2: Run the new check and verify RED**
Run:
```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
```
Expected: FAIL to compile because `EmControlTrajectoryAdapter` does not exist.
- [ ] **Step 3: Implement the minimum adapter**
Implement this public surface and algorithm:
```csharp
public sealed class EmControlTrajectoryAdapter
{
private const double MinimumSegmentLengthMeters = 1e-6;
public Trajectory2D Create(EmTrajectory trajectory)
{
if (trajectory == null)
throw new ArgumentNullException(nameof(trajectory));
var points = new List<TrajectoryPoint>(trajectory.Points.Count);
double arcLength = 0d;
for (int index = 0; index < trajectory.Points.Count; index++)
{
EmTrajectoryPoint source = trajectory.Points[index];
var pose = new MyParking.Shared.Pose2D(source.X, source.Y, source.Yaw);
var converted = new TrajectoryPoint(
arcLength, pose, source.VehicleCurvature, source.SignedLongitudinalVelocity);
if (points.Count == 0)
{
points.Add(converted);
continue;
}
TrajectoryPoint previous = points[points.Count - 1];
double dx = pose.XMeters - previous.PoseInWorld.XMeters;
double dy = pose.YMeters - previous.PoseInWorld.YMeters;
double distance = Math.Sqrt(dx * dx + dy * dy);
if (distance < MinimumSegmentLengthMeters)
{
points[points.Count - 1] = new TrajectoryPoint(
previous.ArcLengthMeters, pose, source.VehicleCurvature,
source.SignedLongitudinalVelocity);
continue;
}
arcLength += distance;
points.Add(new TrajectoryPoint(
arcLength, pose, source.VehicleCurvature,
source.SignedLongitudinalVelocity));
}
if (points.Count < 2)
throw new ArgumentException(
"EM轨迹至少需要包含两个不同位置的有效控制点。", nameof(trajectory));
return new Trajectory2D(points);
}
}
```
Include `/// <summary>` comments on the class and public method, explicitly stating the unit and signed-speed boundary.
- [ ] **Step 4: Run adapter checks and the existing EM suite**
Run:
```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation
```
Expected: both commands PASS.
- [ ] **Step 5: Commit the adapter and checks**
```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/EmControlTrajectoryAdapter.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmControlTrajectoryAdapterChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
git diff --cached --check
git commit -m "feat: adapt EM trajectory for geometric controller"
```
---
### Task 3: Add the one-shot, single-direction `EM闭环测试`
**Files:**
- Create: `ClumsyPilot/tests/verify_em_closed_loop_movement.ps1`
- Create: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs`
**Interfaces:**
- Consumes: `TrajectoryObservationSetupFactory.CreateBootstrapJob(...)`.
- Consumes: `TrajectoryObservationBootstrapper.Bootstrap(...)`.
- Consumes exactly one call to `TrajectoryObservationController.StartCycle(...)`.
- Consumes: `EmControlTrajectoryAdapter.Create(...)`.
- Produces: `[MovementTest(name = "EM闭环测试")] public sealed class EmClosedLoopMovementTest : MovementTest`.
- Produces: `Test()` for session start and `TestStop()` for idempotent cancellation/stop.
- [ ] **Step 1: Write the structural/lifecycle verification first**
Create `verify_em_closed_loop_movement.ps1` so it reads the new source file and fails unless all conditions hold:
```powershell
$sourcePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\tarjplanner_movementtest\MovementTest.EmClosedLoopTest.cs'
if (-not (Test-Path -LiteralPath $sourcePath)) { throw 'EM closed-loop MovementTest source is missing.' }
$source = Get-Content -Raw -Encoding UTF8 -LiteralPath $sourcePath
$required = @(
'\[MovementTest\(name = "EM闭环测试"\)\]',
'EmPlanningScope\.FullDirectionSegment',
'TrajectoryObservationBootstrapper',
'\.StartCycle\(',
'EmControlTrajectoryAdapter',
'TrajectoryTrackingMovement',
'public override void TestStop\(\)',
'\.Cancel\(\)',
'\.Stop\(\)'
)
foreach ($pattern in $required) {
if ($source -notmatch $pattern) { throw "Missing required pattern: $pattern" }
}
if ([regex]::Matches($source, '\.StartCycle\(').Count -ne 1) {
throw 'EM closed-loop test must contain exactly one planning-cycle call site.'
}
if ($source -match 'TrajectoryObservationLoop|ReplanPeriod|while\s*\(true\).*StartCycle') {
throw 'EM closed-loop test must not contain a replanning loop.'
}
Write-Output 'EM closed-loop MovementTest checks passed.'
```
- [ ] **Step 2: Run the verification and confirm RED**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
```
Expected: FAIL with `EM closed-loop MovementTest source is missing.`
- [ ] **Step 3: Implement the MovementTest input surface**
Add `EmClosedLoopMovementTest` with the same goal, map, obstacle, vehicle, solver, and output-step defaults used by `TrajectoryObservationMovementTest`. Set control defaults explicitly:
```csharp
public double MaximumCommandSpeedMetersPerSecond = 0.20d;
public double MaximumDistanceToTrajectoryMeters = 0.30d;
public double ExecutionTimeoutSeconds = 120d;
public float WheelAlignmentToleranceDegrees = 2f;
```
`Test()` must validate inputs, create a validated settings snapshot with:
```csharp
PlanningScope = EmPlanningScope.FullDirectionSegment
```
and start one session through `EmClosedLoopMovementTestRunner.Start(...)`. `TestStop()` must call only:
```csharp
EmClosedLoopMovementTestRunner.Stop();
```
Use the existing observation test's culture-aware finite-number and bounded-obstacle input behavior. Keep those helpers private to the new test so the user's modified observation file is not edited.
- [ ] **Step 4: Implement idempotent session ownership and one-shot planning**
Use a static runner guarded by one lock. Store one `CancellationTokenSource`, one background `Task`, one active `DriveTask`, and a monotonically increasing session ID.
The planning body must follow this exact sequence:
```csharp
VehicleMotionState initialState = ReadPlanningState();
CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
TrajectoryObservationBootstrapResult bootstrap =
new TrajectoryObservationBootstrapper().Bootstrap(job, token);
if (!bootstrap.Succeeded)
throw new InvalidOperationException(bootstrap.FailureReason);
var controller = new TrajectoryObservationController(
bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
"em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
PlanningCycleResult cycle = await controller.StartCycle(
initialState.CapturedAtUtc, initialState, token).ConfigureAwait(false);
if (!cycle.Published || controller.PublishedTrajectory == null)
throw new InvalidOperationException(
string.IsNullOrWhiteSpace(cycle.Result.FailureReason)
? cycle.Diagnostic
: cycle.Result.FailureReason);
Trajectory2D trajectory =
new EmControlTrajectoryAdapter().Create(controller.PublishedTrajectory);
```
There must be no observation loop and no second `StartCycle` call. Log the EM point count, controller point count, geometric length, segment index, direction, and terminal type before control starts.
Create `EmClosedLoopWheelSafety.AreWheelsForward(float toleranceDegrees)` in the same file using `MultiWheelChassisAdapter.AreParallelWheelsAligned(0d, toleranceRadians)`. If this check fails, throw before constructing `DriveTask`.
Create and run the controller movement as:
```csharp
var movement = new TrajectoryTrackingMovement
{
Trajectory = trajectory,
MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
ExecutionTimeoutSeconds = executionTimeoutSeconds
};
var driveTask = new DriveTask(movement.Get());
```
Publish `driveTask` under the session lock only if the session is still current and not cancelled. Otherwise stop it immediately. For a current session call `driveTask.Wait()` on the background worker, then report that the first direction segment ended. A `GearSwitch` terminal is reported as “stopped at gear-switch boundary; next segment was not started.”
`Stop()` must atomically clear the active references, call `CancellationTokenSource.Cancel()`, and call `DriveTask.Stop()`. The completion continuation must clear only references that still belong to its own session.
- [ ] **Step 5: Run structural verification and compile**
Run:
```powershell
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```
Expected: structural check PASS; build PASS with zero errors.
- [ ] **Step 6: Commit the MovementTest**
```powershell
git add -- ClumsyPilot/tests/verify_em_closed_loop_movement.ps1 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.EmClosedLoopTest.cs
git diff --cached --check
git commit -m "feat: add one-shot EM closed-loop movement test"
```
---
### Task 4: Document operation and run the complete non-hardware verification
**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md`
**Interfaces:**
- Documents: test selection, inputs, one-shot/single-direction boundary, `TestStop`, low-speed first-run procedure, and limits of automated validation.
- [ ] **Step 1: Add the operator-facing section**
Add a section named `EM闭环测试` containing these exact operational facts:
```text
- 启动时只规划一次,并冻结首次成功的完整方向段轨迹。
- 控制执行期间不重规划、不切换轨迹。
- 到达目标或换向边界后停车;本测试不启动下一方向段。
- 默认控制速度上限为 0.20 m/s。
- 规划、适配或车轮方向检查失败时不会启动底盘控制。
- TestStop 会同时取消规划并停止活动控制任务。
- 首次实车测试使用空旷环境、短距离单方向目标,并确保硬件急停可用。
```
- [ ] **Step 2: Run all targeted checks**
Run:
```powershell
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- em-control-adapter
dotnet run --project .\ClumsyPilot\tests\EMPlannerVerificationHost\EMPlannerVerificationHost.csproj -- trajectory-observation
powershell -NoProfile -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_em_closed_loop_movement.ps1
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
```
Expected: every command exits 0. Report warning counts from the build.
- [ ] **Step 3: Verify scope and source hygiene**
Run:
```powershell
git diff --check
git status --short
git diff --no-ext-diff -- ClumsyPilot/Control ClumsyPilot/Trajectory ClumsyPilot/Shared ClumsyPilot/StateEstimation ClumsyPilot/Movements ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest ClumsyPilot/tests
```
Expected: no whitespace errors; only files named by this plan are staged or changed by this work. Existing unrelated modifications remain untouched.
- [ ] **Step 4: Commit documentation**
```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
git diff --cached --check
git commit -m "docs: explain EM closed-loop test operation"
```
- [ ] **Step 5: Hand off real-vehicle validation without claiming it passed**
Report separately:
```text
Automated status: adapter checks, observation regressions, structural lifecycle check, and plugin build.
Hardware status: not run by automated verification; requires a low-speed, short, single-direction field test with emergency stop available.
```