docs: plan EM observation planning diagnostics
This commit is contained in:
梁薄云
@@ -0,0 +1,287 @@
|
||||
# EM Observation Planning Diagnostics Implementation Plan
|
||||
|
||||
> **For agentic workers:** REQUIRED SUB-SKILL: Use executing-plans to implement this plan task-by-task. Steps use checkbox syntax for tracking.
|
||||
|
||||
**Goal:** Print one effective EM planning configuration snapshot at observation-session startup and make trajectory validation failures and successes numerically diagnosable without changing planning behavior.
|
||||
|
||||
**Architecture:** EmTrajectoryValidator enriches its existing jerk rejection reason with the values used in the decision. TrajectoryObservationDiagnostics owns pure invariant-culture formatters for configuration and successful trajectory metrics. The MovementTest runner prints the configuration formatter once after it creates the session-local controller.
|
||||
|
||||
**Tech Stack:** C#/.NET, existing EMPlannerVerificationHost, OSQP-backed EM planner.
|
||||
|
||||
## Global Constraints
|
||||
|
||||
- Keep the MovementTest OBSERVE_ONLY; do not add actuator calls.
|
||||
- Do not change solver configuration, motion limits, QP formulation, horizon selection, or acceptance rules.
|
||||
- Print configuration once per session; do not put it in the recurring UI diagnostic or every planning cycle.
|
||||
- Format values with CultureInfo.InvariantCulture.
|
||||
- Failed publications keep returning no EmTrajectory; their numeric evidence belongs in the failure reason.
|
||||
|
||||
---
|
||||
|
||||
### Task 1: Detail a jerk-limit rejection with computed evidence
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs:132-199
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs:58-110
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: the existing EmTrajectoryValidator.Validate(...) method.
|
||||
- Produces: an unchanged JerkLimitExceeded result whose Message contains time, dt, previousAcceleration, acceleration, jerk, limit, excess, storedPreviousJerk, and storedCurrentJerk.
|
||||
|
||||
- [ ] **Step 1: Write the failing test**
|
||||
|
||||
In TrajectoryChecks.VerifiesWorldSpacePublicationMutationsAreRejected, retain the current jerkMutated trajectory and add:
|
||||
|
||||
EmTrajectoryValidationResult jerkResult = new EmTrajectoryValidator().Validate(
|
||||
jerkMutated, context.EmptyMap, context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
||||
Verification.Equal(EmTrajectoryValidationFailure.JerkLimitExceeded, jerkResult.Failure,
|
||||
"jerk diagnostic failure code");
|
||||
Verification.Equal(2, jerkResult.PointIndex, "jerk diagnostic point index");
|
||||
foreach (string field in new[]
|
||||
{
|
||||
"time=", "dt=", "previousAcceleration=", "acceleration=", "jerk=", "limit=", "excess=",
|
||||
"storedPreviousJerk=", "storedCurrentJerk=",
|
||||
})
|
||||
Verification.True(jerkResult.Message.Contains(field), "jerk diagnostic includes " + field);
|
||||
|
||||
- [ ] **Step 2: Run the test to confirm RED**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
|
||||
Expected: the new assertion fails because the current message contains only Trajectory finite-difference jerk exceeds its limit.
|
||||
|
||||
- [ ] **Step 3: Implement the message enrichment**
|
||||
|
||||
Add using System.Globalization; to EmTrajectoryValidator.cs. Calculate finiteDifferenceJerk before the condition and preserve the existing inequality:
|
||||
|
||||
double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt;
|
||||
if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk + limits.KinematicTolerance)
|
||||
{
|
||||
double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d;
|
||||
double storedCurrentJerk = previous.LongitudinalJerk;
|
||||
return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
|
||||
"Trajectory finite-difference jerk exceeds its limit" +
|
||||
" (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" +
|
||||
", previousAcceleration=" + Format(previousAcceleration) + "m/s2" +
|
||||
", acceleration=" + Format(acceleration) + "m/s2" +
|
||||
", jerk=" + Format(finiteDifferenceJerk) + "m/s3" +
|
||||
", limit=" + Format(limits.MaximumJerk) + "m/s3" +
|
||||
", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" +
|
||||
", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" +
|
||||
", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3).");
|
||||
}
|
||||
|
||||
Add this private method to the same class:
|
||||
|
||||
private static string Format(double value)
|
||||
{
|
||||
return value.ToString("G17", CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
storedPreviousJerk and storedCurrentJerk are evidence only and do not influence the validation decision.
|
||||
|
||||
- [ ] **Step 4: Confirm GREEN and commit**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
|
||||
Expected: host exit code 0.
|
||||
|
||||
Run:
|
||||
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
|
||||
git commit -m "feat: detail trajectory jerk validation failures"
|
||||
|
||||
### Task 2: Format and print the effective session configuration once
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs:188-241
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs:269-305
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:18-39,96-191
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: the effective EmPlannerConfiguration held privately by TrajectoryObservationController.
|
||||
- Produces: TrajectoryObservationDiagnostics.CreateConfiguration(EmPlannerConfiguration) and TrajectoryObservationController.CreateConfigurationDiagnostic().
|
||||
- Produces: one terminal planning configuration block before the observation loop begins.
|
||||
|
||||
- [ ] **Step 1: Write failing configuration tests**
|
||||
|
||||
Add VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration() to TrajectoryObservationChecks.Run(). Bootstrap a controller with:
|
||||
|
||||
var settings = new TrajectoryObservationSettings
|
||||
{
|
||||
ReplanPeriodSeconds = 0.25d,
|
||||
SolverTimeoutSeconds = 1.25d,
|
||||
MaximumOsqpIterations = 54321,
|
||||
TimeHorizonSeconds = 3.5d,
|
||||
OutputTimeStepSeconds = 0.10d,
|
||||
};
|
||||
var controller = new TrajectoryObservationController(bootstrap, settings,
|
||||
new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "config-diagnostic");
|
||||
string text = controller.CreateConfigurationDiagnostic().Text;
|
||||
foreach (string expected in new[]
|
||||
{
|
||||
"planning configuration:", "timeHorizon=3.50s", "distanceHorizon=5.00m", "outputTimeStep=0.10s",
|
||||
"outputFrequency=10.00Hz", "trajectoryKnots=36", "maximumOsqpIterations=54321",
|
||||
"solverTimeout=1.25s", "replanPeriod=0.25s",
|
||||
})
|
||||
Verification.True(text.Contains(expected), "configuration diagnostic includes " + expected);
|
||||
|
||||
Source-read MovementTest.TrajectoryObservationTest.cs and assert the runner call is singular:
|
||||
|
||||
int firstCall = runnerSource.IndexOf("CreateConfigurationDiagnostic()", StringComparison.Ordinal);
|
||||
Verification.True(firstCall >= 0, "runner prints configuration diagnostic");
|
||||
Verification.Equal(-1, runnerSource.IndexOf("CreateConfigurationDiagnostic()", firstCall + 1,
|
||||
StringComparison.Ordinal), "runner prints configuration diagnostic once");
|
||||
|
||||
- [ ] **Step 2: Run the test to confirm RED**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
|
||||
Expected: compilation fails because CreateConfigurationDiagnostic does not exist.
|
||||
|
||||
- [ ] **Step 3: Implement pure formatting and the one runner call**
|
||||
|
||||
Add to TrajectoryObservationDiagnostics:
|
||||
|
||||
public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
|
||||
{
|
||||
if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
|
||||
throw new ArgumentNullException(nameof(configuration));
|
||||
double horizon = configuration.Scheduling.TimeHorizonSeconds;
|
||||
double step = configuration.Scheduling.OutputTimeStepSeconds;
|
||||
int knots = checked((int)Math.Ceiling(horizon / step) + 1);
|
||||
return new TrajectoryObservationDiagnostic("planning configuration:\n" +
|
||||
"timeHorizon=" + Format(horizon, "F2") + "s\n" +
|
||||
"distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" +
|
||||
"outputTimeStep=" + Format(step, "F2") + "s\n" +
|
||||
"outputFrequency=" + Format(1d / step, "F2") + "Hz\n" +
|
||||
"trajectoryKnots=" + knots.ToString(CultureInfo.InvariantCulture) + "\n" +
|
||||
"maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
|
||||
"solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
|
||||
"replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s");
|
||||
}
|
||||
|
||||
private static string Format(double value, string format)
|
||||
{
|
||||
return value.ToString(format, CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
Add to TrajectoryObservationController:
|
||||
|
||||
public TrajectoryObservationDiagnostic CreateConfigurationDiagnostic()
|
||||
{
|
||||
return TrajectoryObservationDiagnostics.CreateConfiguration(configuration);
|
||||
}
|
||||
|
||||
Immediately after constructing controller in RunSessionAsync, before new TrajectoryObservationLoop(controller), add exactly:
|
||||
|
||||
LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text);
|
||||
|
||||
Do not call this method from the while loop or the UI drawing methods.
|
||||
|
||||
- [ ] **Step 4: Confirm GREEN and commit**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
|
||||
Expected: host exit code 0.
|
||||
|
||||
Run:
|
||||
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
git commit -m "feat: log EM observation session planning configuration"
|
||||
|
||||
### Task 3: Summarize each successfully published trajectory
|
||||
|
||||
**Files:**
|
||||
|
||||
- Modify: ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs:17-39
|
||||
- Modify: ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs:538-561
|
||||
|
||||
**Interfaces:**
|
||||
|
||||
- Consumes: latestCycle.Result.Trajectory only when latestCycle.Published is true.
|
||||
- Produces: a trajectory summary with ID, point count, duration, PathS length, max speed, max finite-difference acceleration, and max finite-difference jerk.
|
||||
|
||||
- [ ] **Step 1: Write the failing summary test**
|
||||
|
||||
Add VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary() to TrajectoryObservationChecks.Run():
|
||||
|
||||
EmTrajectory trajectory = CreatePublishedTrajectory(effectiveAt);
|
||||
var succeeded = new PlanningCycleResult(9L,
|
||||
new PlanningCycleIdentity(1L, "summary-reference", 2L, string.Empty, 0),
|
||||
new EmPlanningResult(EmPlanningStatus.Success, trajectory, string.Empty), true, string.Empty);
|
||||
string text = TrajectoryObservationDiagnostics.Create(
|
||||
succeeded, TimeSpan.FromMilliseconds(12d), false, trajectory).Text;
|
||||
foreach (string expected in new[]
|
||||
{
|
||||
"trajectory summary:", "trajectoryId=observer-published", "points=2", "duration=1.000s",
|
||||
"pathLength=1.000m", "maxSpeed=0.400m/s", "maxAcceleration=0.200m/s2", "maxJerk=0.000m/s3",
|
||||
})
|
||||
Verification.True(text.Contains(expected), "published diagnostic includes " + expected);
|
||||
|
||||
Also add this assertion to the existing failed-cycle test:
|
||||
|
||||
Verification.True(!diagnostic.Text.Contains("trajectory summary:"), "failed diagnostic has no stale trajectory summary");
|
||||
|
||||
- [ ] **Step 2: Run the test to confirm RED**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
|
||||
Expected: summary assertions fail because the formatter only writes trajectory=<id>.
|
||||
|
||||
- [ ] **Step 3: Implement summary formatting**
|
||||
|
||||
Replace the publishedTrajectory != null branch in TrajectoryObservationDiagnostics.Create(...) with a branch requiring latestCycle.Published && latestCycle.Result.Trajectory != null. Add CreateTrajectorySummary(EmTrajectory trajectory), which:
|
||||
|
||||
- reads immutable trajectory points;
|
||||
- calculates maximum absolute signed speed;
|
||||
- calculates acceleration and jerk by the same direction-aware finite-difference convention as EmTrajectoryValidator;
|
||||
- ignores a nonpositive or non-finite dt only in logging;
|
||||
- reports first and last PathS difference as actual pathLength;
|
||||
- uses Format(value, "F3") and CultureInfo.InvariantCulture;
|
||||
- returns exactly these two lines:
|
||||
|
||||
trajectory summary:
|
||||
trajectoryId=<id>, points=<count>, duration=<last-time>s, pathLength=<last-pathS-first-pathS>m
|
||||
maxSpeed=<absolute-signed-speed>m/s, maxAcceleration=<absolute-finite-difference>m/s2, maxJerk=<absolute-finite-difference>m/s3
|
||||
|
||||
Do not call this helper for an unsuccessful cycle, even when an older published trajectory is still observed.
|
||||
|
||||
- [ ] **Step 4: Confirm GREEN, inspect, and commit**
|
||||
|
||||
Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
git diff --check
|
||||
|
||||
Expected: host exit code 0 and no whitespace errors.
|
||||
|
||||
Run:
|
||||
|
||||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs
|
||||
git commit -m "feat: summarize published EM observation trajectories"
|
||||
|
||||
## Final verification
|
||||
|
||||
- [ ] Run:
|
||||
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj
|
||||
git diff --check
|
||||
git log -3 --oneline
|
||||
|
||||
Expected: verification exits 0, no whitespace errors exist, and three focused diagnostics commits follow the already committed design document.
|
||||
Reference in New Issue
Block a user