docs: plan EM closed-loop movement test
This commit is contained in:
@@ -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.
|
||||||
|
```
|
||||||
Reference in New Issue
Block a user