feat: validate EM terminal world pose

2026-08-07 08:01:18 +08:00
parent 0bba8d7e61
commit e844fe00a8
6 changed files with 249 additions and 11 deletions
@@ -22,6 +22,7 @@ internal static class EmPlanningServiceChecks
        VerifiesNoProgressPublishesNoTrajectory();
        VerifiesTimeoutFallbackAndCancellationSemantics();
        VerifiesPublicationFailureAndDebugIsolation();
+        VerifiesPublicationGateStatusMappings();
    }

    private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic()
@@ -427,6 +428,20 @@ internal static class EmPlanningServiceChecks
        VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
    }

+    private static void VerifiesPublicationGateStatusMappings()
+    {
+        Verification.Equal(EmPlanningStatus.TerminalPoseMismatch,
+            EmPlanningService.MapPublicationFailure(EmTrajectoryValidationFailure.TerminalPoseMismatch),
+            "terminal-pose validator failure has its precise service status");
+
+        EmPlanningRequest sampleLimited = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        sampleLimited.Configuration.Scheduling.MaximumPublishedSampleCount = 2;
+        EmPlanningResult sampleLimitResult = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
+            sampleLimited, CancellationToken.None);
+        VerifyFailure(sampleLimitResult, EmPlanningStatus.FullSegmentResourceLimitExceeded,
+            "sample-limit publication failure");
+    }
+
    private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference()
    {
        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
@@ -17,6 +17,7 @@ internal static class TrajectoryChecks
        VerifiesRollingTrajectoryHasNoSyntheticStopTail();
        VerifiesExactStopHoldHasNoJerkDiscontinuity();
        VerifiesWorldSpacePublicationMutationsAreRejected();
+        VerifiesTerminalPoseAndSampleLimitPublicationGates();
        VerifiesReverseSpeedLimitUsesReverseConfiguration();
    }

@@ -184,6 +185,64 @@ internal static class TrajectoryChecks
        Verification.Equal(4, beyondSegment.PointIndex, "segment-boundary first point");
    }

+    private static void VerifiesTerminalPoseAndSampleLimitPublicationGates()
+    {
+        ValidationContext context = CreateValidationContext();
+        EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
+        int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
+        EmTrajectoryPoint terminal = valid.Points[terminalIndex];
+        var validator = new EmTrajectoryValidator();
+
+        EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw),
+            EmBoundaryType.Goal);
+        Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted");
+        EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw),
+            EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
+            "3.1-centimetre terminal position error is rejected");
+
+        double degrees = Math.PI / 180d;
+        EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees));
+        EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees),
+            EmBoundaryType.Goal);
+        Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error");
+        EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees),
+            EmBoundaryType.Goal);
+        Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted");
+        EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees),
+            EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
+            "5.1-degree terminal yaw error is rejected");
+
+        EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
+                EmLongitudinalMode.RollingContinuation));
+        EmTrajectoryValidationResult rollingResult = validator.Validate(rolling, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.08d, new Pose2D(99d, 99d, Math.PI), EmBoundaryType.RollingSafetyStop);
+        Verification.True(rollingResult.IsValid, "rolling-safety window end does not use the terminal-pose gate");
+
+        EmPlannerConfiguration limitedConfiguration = EmPlannerConfiguration.CreateDefault();
+        limitedConfiguration.Scheduling.MaximumPublishedSampleCount = 8;
+        EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(limitedConfiguration).TryAssemble(
+            CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
+            CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal), out EmTrajectory limitedTrajectory,
+            out string assemblyFailure);
+        Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, assemblyStatus,
+            "sample schedule one above the limit is rejected");
+        Verification.True(limitedTrajectory == null, "sample-limit rejection does not assemble a partial trajectory");
+        Verification.True(assemblyFailure.IndexOf("MaximumPublishedSampleCount", StringComparison.Ordinal) >= 0,
+            "sample-limit rejection preserves a diagnostic");
+        Verification.True(assemblyFailure.IndexOf("required-at-least=9", StringComparison.Ordinal) >= 0,
+            "sample-limit fixture exceeds the cap by exactly one sample");
+    }
+
    private static void VerifiesReverseSpeedLimitUsesReverseConfiguration()
    {
        ValidationContext context = CreateValidationContext();
@@ -284,11 +343,11 @@ internal static class TrajectoryChecks
        return Replace(trajectory, index, replacement);
    }

-    private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? timeFromStart = null,
-        double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
+    private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? y = null, double? yaw = null,
+        double? timeFromStart = null, double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
        EmBoundaryType? boundaryType = null)
    {
-        return new EmTrajectoryPoint(x ?? point.X, point.Y, point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity,
+        return new EmTrajectoryPoint(x ?? point.X, y ?? point.Y, yaw ?? point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity,
            timeFromStart ?? point.TimeFromStart, vehicleCurvature ?? point.VehicleCurvature, point.SegmentIndex,
            pathS ?? point.SegmentLocalS, pathS ?? point.PathS, point.Direction, boundaryType ?? point.BoundaryType,
            0d, 0d);