feat: detail trajectory jerk validation failures

2026-08-05 15:25:18 +08:00
parent 0eeda2d824
commit 32083db4d0
2 changed files with 32 additions and 2 deletions
@@ -85,6 +85,19 @@ internal static class TrajectoryChecks
        EmTrajectory jerkMutated = Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.035d));
        jerkMutated = Replace(jerkMutated, 2, Clone(jerkMutated.Points[2], signedSpeed: 0.03d));
        AssertRejected(context, jerkMutated, EmTrajectoryValidationFailure.JerkLimitExceeded, 2, "jerk limit");
+        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);
+        }

        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 2d)),
            EmTrajectoryValidationFailure.CurvatureLimitExceeded, 1, "curvature limit");