feat: validate published EM trajectories

2026-08-04 11:43:41 +08:00
parent 1d58864908
commit 7410654e68
2 changed files with 525 additions and 0 deletions
@@ -1,7 +1,9 @@
 using System;
 using System.Collections.Generic;
+using System.Reflection;
 using EMPlannerVerificationHost;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

@@ -12,6 +14,8 @@ internal static class TrajectoryChecks
        VerifiesForwardFieldsExactTerminalAndHold();
        VerifiesReverseTravelVelocityAndUnwrappedYaw();
        VerifiesPublishedListsAreImmutable();
+        VerifiesWorldSpacePublicationMutationsAreRejected();
+        VerifiesReverseSpeedLimitUsesReverseConfiguration();
    }

    private static void VerifiesForwardFieldsExactTerminalAndHold()
@@ -51,6 +55,185 @@ internal static class TrajectoryChecks
            "trajectory public point list is immutable");
    }

+    private static void VerifiesWorldSpacePublicationMutationsAreRejected()
+    {
+        ValidationContext context = CreateValidationContext();
+        EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
+        EmTrajectoryValidationResult accepted = new EmTrajectoryValidator().Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
+        Verification.True(accepted.IsValid, "valid world-space trajectory is publishable: " + accepted.Message);
+
+        AssertRejected(context, CorruptDouble(valid, 1, "X", double.NaN), EmTrajectoryValidationFailure.NonFinite, 1,
+            "non-finite point");
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], timeFromStart: valid.Points[0].TimeFromStart)),
+            EmTrajectoryValidationFailure.TimeNotStrictlyIncreasing, 1, "non-increasing time");
+        AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], pathS: 0.0005d)),
+            EmTrajectoryValidationFailure.PathSDecreased, 2, "decreasing PathS");
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: -valid.Points[1].Speed)),
+            EmTrajectoryValidationFailure.DirectionSignMismatch, 1, "direction sign");
+        AssertRejected(context, CorruptDouble(valid, 1, "Speed", valid.Points[1].Speed + 0.01d),
+            EmTrajectoryValidationFailure.RedundantSpeedMismatch, 1, "redundant speed");
+        AssertRejected(context, CorruptDouble(valid, 1, "VelocityX", valid.Points[1].VelocityX + 0.01d),
+            EmTrajectoryValidationFailure.WorldVelocityMismatch, 1, "world velocity");
+        AssertRejected(context, CorruptDouble(valid, 1, "YawRate", valid.Points[1].YawRate + 0.01d),
+            EmTrajectoryValidationFailure.YawRateMismatch, 1, "yaw rate");
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.30d)),
+            EmTrajectoryValidationFailure.SpeedLimitExceeded, 1, "speed limit");
+        AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], signedSpeed: 0.19d)),
+            EmTrajectoryValidationFailure.AccelerationLimitExceeded, 2, "acceleration limit");
+
+        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");
+
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 2d)),
+            EmTrajectoryValidationFailure.CurvatureLimitExceeded, 1, "curvature limit");
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], vehicleCurvature: 0.75d)),
+            EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, 1, "curvature-rate limit");
+
+        const int terminalIndex = 8;
+        AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], boundaryType: EmBoundaryType.None)),
+            EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex, "missing exact terminal anchor");
+        AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], signedSpeed: 0.01d)),
+            EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex, "terminal speed");
+        AssertRejected(context, CorruptDouble(valid, terminalIndex, "YawRate", 0.01d),
+            EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex, "terminal yaw rate");
+
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], x: 1d)), context.PoseCollisionMap,
+            EmTrajectoryValidationFailure.PoseCollision, 1, "pose collision");
+        AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], x: 1d)), context.SweptCollisionMap,
+            EmTrajectoryValidationFailure.SweptCollision, 1, "swept collision");
+        EmTrajectoryValidationResult beyondSegment = new EmTrajectoryValidator().Validate(valid, context.EmptyMap, context.Vehicle,
+            context.Configuration, 2, 0.004d, EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, beyondSegment.Failure,
+            "segment-boundary failure code");
+        Verification.Equal(4, beyondSegment.PointIndex, "segment-boundary first point");
+    }
+
+    private static void VerifiesReverseSpeedLimitUsesReverseConfiguration()
+    {
+        ValidationContext context = CreateValidationContext();
+        context.Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
+        context.Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 0.02d;
+        EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(
+            CreateValidationTrajectory(TravelDirection.Reverse), context.EmptyMap, context.Vehicle, context.Configuration,
+            2, 0.0055d, EmBoundaryType.Goal);
+        Verification.Equal(EmTrajectoryValidationFailure.SpeedLimitExceeded, result.Failure,
+            "reverse speed uses the configured reverse limit");
+        Verification.Equal(0, result.PointIndex, "reverse speed first over-limit point");
+    }
+
+    private static void AssertRejected(ValidationContext context, EmTrajectory trajectory,
+        EmTrajectoryValidationFailure expectedFailure, int expectedIndex, string name)
+    {
+        AssertRejected(context, trajectory, context.EmptyMap, expectedFailure, expectedIndex, name);
+    }
+
+    private static void AssertRejected(ValidationContext context, EmTrajectory trajectory, PlanningGridMap map,
+        EmTrajectoryValidationFailure expectedFailure, int expectedIndex, string name)
+    {
+        EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, map, context.Vehicle,
+            context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
+        Verification.True(!result.IsValid, name + " is rejected");
+        Verification.Equal(expectedFailure, result.Failure, name + " failure code");
+        Verification.Equal(expectedIndex, result.PointIndex, name + " failure index");
+    }
+
+    private static ValidationContext CreateValidationContext()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        return new ValidationContext(configuration, new VehicleParameters
+        {
+            LengthMeters = 0.01d,
+            WidthMeters = 0.01d,
+            SafetyMarginMeters = 0d,
+            MaximumCurvaturePerMeter = 1d,
+        },
+        CreateValidationMap(Array.Empty<IMapObstacle>()),
+        CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(990f, 1010f, -10f, 10f) }),
+        CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(490f, 510f, -10f, 10f) }));
+    }
+
+    private static PlanningGridMap CreateValidationMap(IReadOnlyList<IMapObstacle> obstacles)
+    {
+        IMapObstacleSource[] sources = obstacles.Count == 0
+            ? Array.Empty<IMapObstacleSource>()
+            : new IMapObstacleSource[] { new ManualObstacleSource("trajectory-validator", 1L, true, obstacles) };
+        PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            ResolutionMm = 20f,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = obstacles.Count == 0,
+        });
+        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
+            "trajectory-validator map builds: " + result.FailureReason);
+        return result.Map!;
+    }
+
+    private static EmTrajectory CreateValidationTrajectory(TravelDirection direction)
+    {
+        double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d };
+        double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
+            0.00546875d, 0.0055d };
+        double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d };
+        double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d };
+        double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d };
+        var candidate = new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
+        var result = new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
+        var path = new LateralPath(new[]
+        {
+            new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d),
+            new LateralPathPoint(1d, 0.0055d, 0d, 0d, 0d, 0d, 0.0055d, 0d, 0d, 0d, 0d, 0d),
+        }, true);
+        return new EmTrajectoryAssembler().Assemble(path, result, CreateMetadata(direction, EmTerminalType.Goal));
+    }
+
+    private static EmTrajectory Replace(EmTrajectory trajectory, int index, EmTrajectoryPoint replacement)
+    {
+        var points = new List<EmTrajectoryPoint>(trajectory.Points);
+        points[index] = replacement;
+        return new EmTrajectory(trajectory.Metadata, points);
+    }
+
+    private static EmTrajectory CorruptDouble(EmTrajectory trajectory, int index, string propertyName, double value)
+    {
+        EmTrajectoryPoint replacement = Clone(trajectory.Points[index]);
+        FieldInfo field = typeof(EmTrajectoryPoint).GetField("<" + propertyName + ">k__BackingField",
+            BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Missing backing field " + propertyName);
+        field.SetValue(replacement, value);
+        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,
+        EmBoundaryType? boundaryType = null)
+    {
+        return new EmTrajectoryPoint(x ?? point.X, point.Y, 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);
+    }
+
+    private sealed class ValidationContext
+    {
+        public ValidationContext(EmPlannerConfiguration configuration, VehicleParameters vehicle, PlanningGridMap emptyMap,
+            PlanningGridMap poseCollisionMap, PlanningGridMap sweptCollisionMap)
+        {
+            Configuration = configuration;
+            Vehicle = vehicle;
+            EmptyMap = emptyMap;
+            PoseCollisionMap = poseCollisionMap;
+            SweptCollisionMap = sweptCollisionMap;
+        }
+
+        public EmPlannerConfiguration Configuration { get; }
+        public VehicleParameters Vehicle { get; }
+        public PlanningGridMap EmptyMap { get; }
+        public PlanningGridMap PoseCollisionMap { get; }
+        public PlanningGridMap SweptCollisionMap { get; }
+    }
+
    private static void VerifyKinematicFields(EmTrajectory trajectory, TravelDirection direction, string name)
    {
        double directionSign = direction == TravelDirection.Forward ? 1d : -1d;