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
@@ -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; }
    }
 }