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
@@ -124,15 +124,22 @@ public sealed class EmPlanningService : IEmPlanningService
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
                request.PlanningScope);
-            EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
+            EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path,
+                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
+            if (assemblyStatus != EmPlanningStatus.Success)
+                return Failure(assemblyStatus, request, assemblyFailure);
            EmitDebug(request, "trajectory assembly succeeded");

+            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
+            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
+                ? TerminalPose(lateral.Path)
+                : null;
            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
-                slice.TerminalBoundary.BoundaryType);
+                terminalPose, terminalBoundary);
            if (!publication.IsValid)
            {
-                return Failure(EmPlanningStatus.ValidationFailed, request,
+                return Failure(MapPublicationFailure(publication.Failure), request,
                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
            }
            EmitDebug(request, "world-space publication validation succeeded");
@@ -225,6 +232,24 @@ public sealed class EmPlanningService : IEmPlanningService
        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
    }

+    internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure)
+    {
+        return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch
+            ? EmPlanningStatus.TerminalPoseMismatch
+            : EmPlanningStatus.ValidationFailed;
+    }
+
+    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
+    }
+
+    private static Pose2D TerminalPose(LateralPath path)
+    {
+        LateralPathPoint terminal = path.Points[path.Points.Count - 1];
+        return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw);
+    }
+
    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
    {
        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));
@@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler
 {
    private readonly double outputTimeStepSeconds;
    private readonly double zeroSpeedHoldSeconds;
+    private readonly int maximumPublishedSampleCount;

    public EmTrajectoryAssembler()
        : this(EmPlannerConfiguration.CreateDefault())
@@ -21,8 +22,9 @@ public sealed class EmTrajectoryAssembler
            throw new ArgumentNullException(nameof(configuration));
        outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
        zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
+        maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
-            zeroSpeedHoldSeconds < 0d)
+            zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
        {
            throw new ArgumentOutOfRangeException(nameof(configuration));
        }
@@ -30,6 +32,18 @@ public sealed class EmTrajectoryAssembler

    public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
    {
+        EmPlanningStatus status = TryAssemble(path, longitudinal, metadata, out EmTrajectory trajectory,
+            out string failureReason);
+        if (status != EmPlanningStatus.Success)
+            throw new ArgumentException(failureReason, nameof(longitudinal));
+        return trajectory;
+    }
+
+    public EmPlanningStatus TryAssemble(LateralPath path, LongitudinalPlanningResult longitudinal,
+        EmTrajectoryMetadata metadata, out EmTrajectory trajectory, out string failureReason)
+    {
+        trajectory = null;
+        failureReason = string.Empty;
        if (longitudinal == null || longitudinal.Candidate == null ||
            (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
        {
@@ -40,8 +54,17 @@ public sealed class EmTrajectoryAssembler

        var interpolator = new LateralPathInterpolator(path);
        bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
-        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds,
-            isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
+        double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds;
+        if (TrajectorySampleSchedule.ExceedsMaximumSampleCount(longitudinal.Candidate, outputTimeStepSeconds,
+                holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, maximumPublishedSampleCount,
+                out int requiredSampleCount))
+        {
+            failureReason = "FullSegmentResourceLimitExceeded: MaximumPublishedSampleCount=" +
+                maximumPublishedSampleCount + ";required-at-least=" + requiredSampleCount;
+            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
+        }
+        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
+            metadata.LongitudinalMode, isFullDirectionSegment);
        double terminalPathS = path.Points[path.Points.Count - 1].PathS;
        var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
        double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;
@@ -62,7 +85,8 @@ public sealed class EmTrajectoryAssembler
                boundaryType, sample.Acceleration, sample.Jerk));
        }

-        return new EmTrajectory(metadata, points);
+        trajectory = new EmTrajectory(metadata, points);
+        return EmPlanningStatus.Success;
    }

    private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)
@@ -86,6 +86,50 @@ internal sealed class TrajectorySampleSchedule
    public IReadOnlyList<TrajectorySample> Samples { get; }
    public int TerminalAnchorSampleIndex { get; }

+    internal static bool ExceedsMaximumSampleCount(LongitudinalCandidate candidate, double outputTimeStepSeconds,
+        double holdDurationSeconds, EmLongitudinalMode mode, bool resampleMotion, int maximumSampleCount,
+        out int requiredSampleCount)
+    {
+        requiredSampleCount = 0;
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (maximumSampleCount < 1)
+            throw new ArgumentOutOfRangeException(nameof(maximumSampleCount));
+
+        if (resampleMotion)
+        {
+            double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1];
+            for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance;
+                sampleTime += outputTimeStepSeconds)
+            {
+                if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                    return true;
+            }
+            if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                return true;
+        }
+        else
+        {
+            for (int index = 0; index < candidate.KnotTimes.Count; index++)
+            {
+                if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                    return true;
+            }
+        }
+
+        if (mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return false;
+
+        double holdElapsed = 0d;
+        while (holdElapsed < holdDurationSeconds - ZeroTolerance)
+        {
+            holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds);
+            if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount))
+                return true;
+        }
+        return false;
+    }
+
    private static void AddSample(TrajectorySample sample, ICollection<TrajectorySample> samples,
        ref double previousPathS, LongitudinalCandidate candidate)
    {
@@ -97,6 +141,12 @@ internal sealed class TrajectorySampleSchedule
        previousPathS = sample.PathS;
    }

+    private static bool IncrementAndExceedsMaximum(ref int sampleCount, int maximumSampleCount)
+    {
+        checked { sampleCount++; }
+        return sampleCount > maximumSampleCount;
+    }
+
    private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval)
    {
        int lastKnot = candidate.KnotTimes.Count - 1;
@@ -18,6 +18,7 @@ public enum EmTrajectoryValidationFailure
    TerminalSpeedNotZero,
    TerminalAccelerationNotZero,
    TerminalYawRateNotZero,
+    TerminalPoseMismatch,
    DirectionMismatch,
    DirectionSignMismatch,
    RedundantSpeedMismatch,
@@ -79,6 +80,22 @@ public sealed class EmTrajectoryValidator

    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary)
+    {
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null,
+            terminalBoundary, false);
+    }
+
+    public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
+        EmBoundaryType terminalBoundary)
+    {
+        return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose,
+            terminalBoundary, true);
+    }
+
+    private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
+        EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose,
+        EmBoundaryType terminalBoundary, bool requiresTerminalPose)
    {
        if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
            configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
@@ -151,6 +168,31 @@ public sealed class EmTrajectoryValidator
                    return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, index,
                        "Exact-stop tail stored acceleration must remain zero.");
            }
+            if (IsRealTerminalBoundary(terminalBoundary))
+            {
+                if (requiresTerminalPose && terminalPose == null)
+                {
+                    return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex,
+                        "A real terminal anchor requires its expected world pose.");
+                }
+                if (terminalPose != null)
+                {
+                    if (!IsFinite(terminalPose.X) || !IsFinite(terminalPose.Y) || !IsFinite(terminalPose.Heading))
+                    {
+                        return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex,
+                            "The expected terminal world pose is non-finite.");
+                    }
+                    double dx = terminal.X - terminalPose.X;
+                    double dy = terminal.Y - terminalPose.Y;
+                    double positionError = Math.Sqrt(dx * dx + dy * dy);
+                    double yawError = Math.Abs(NormalizeAngle(terminal.Yaw - terminalPose.Heading));
+                    if (positionError > limits.TerminalPositionTolerance || yawError > limits.TerminalYawTolerance)
+                    {
+                        return Reject(EmTrajectoryValidationFailure.TerminalPoseMismatch, terminalIndex,
+                            "TerminalPoseMismatch: position=" + Format(positionError) + ";yaw=" + Format(yawError));
+                    }
+                }
+            }
        }

        double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
@@ -268,6 +310,18 @@ public sealed class EmTrajectoryValidator
        return -1;
    }

+    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
+    }
+
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2d * Math.PI;
+        while (angle < -Math.PI) angle += 2d * Math.PI;
+        return angle;
+    }
+
    private static int FindFirstTerminalPathIndex(EmTrajectory trajectory, double terminalPathS, double spatialTolerance)
    {
        for (int index = 0; index < trajectory.Points.Count; index++)
@@ -315,6 +369,10 @@ public sealed class EmTrajectoryValidator
            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond <= 0d ||
            !IsFinite(configuration.Validation.SpatialToleranceMeters) || configuration.Validation.SpatialToleranceMeters < 0d ||
            !IsFinite(configuration.Validation.KinematicTolerance) || configuration.Validation.KinematicTolerance < 0d ||
+            !IsFinite(configuration.Validation.TerminalPositionToleranceMeters) ||
+            configuration.Validation.TerminalPositionToleranceMeters < 0d ||
+            !IsFinite(configuration.Validation.TerminalYawToleranceRadians) ||
+            configuration.Validation.TerminalYawToleranceRadians < 0d ||
            !IsFinite(configuration.Corridor.MaximumCollisionCheckStepMeters) ||
            configuration.Corridor.MaximumCollisionCheckStepMeters <= 0d)
        {
@@ -327,6 +385,8 @@ public sealed class EmTrajectoryValidator
            configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
            configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond,
            configuration.Validation.SpatialToleranceMeters, configuration.Validation.KinematicTolerance,
+            configuration.Validation.TerminalPositionToleranceMeters,
+            configuration.Validation.TerminalYawToleranceRadians,
            configuration.Corridor.MaximumCollisionCheckStepMeters);
        return true;
    }
@@ -356,7 +416,8 @@ public sealed class EmTrajectoryValidator
    {
        public ValidationLimits(double maximumSpeed, double maximumCurvature, double maximumAcceleration,
            double maximumDeceleration, double maximumJerk, double maximumCurvatureRate, double spatialTolerance,
-            double kinematicTolerance, double configuredCollisionStepMeters)
+            double kinematicTolerance, double terminalPositionTolerance, double terminalYawTolerance,
+            double configuredCollisionStepMeters)
        {
            MaximumSpeed = maximumSpeed;
            MaximumCurvature = maximumCurvature;
@@ -366,6 +427,8 @@ public sealed class EmTrajectoryValidator
            MaximumCurvatureRate = maximumCurvatureRate;
            SpatialTolerance = spatialTolerance;
            KinematicTolerance = kinematicTolerance;
+            TerminalPositionTolerance = terminalPositionTolerance;
+            TerminalYawTolerance = terminalYawTolerance;
            ConfiguredCollisionStepMeters = configuredCollisionStepMeters;
        }

@@ -377,6 +440,8 @@ public sealed class EmTrajectoryValidator
        public double MaximumCurvatureRate { get; }
        public double SpatialTolerance { get; }
        public double KinematicTolerance { get; }
+        public double TerminalPositionTolerance { get; }
+        public double TerminalYawTolerance { get; }
        public double ConfiguredCollisionStepMeters { get; }
    }
 }