using System; using System.Collections.Generic; using System.Reflection; using EMPlannerVerificationHost; using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.Mapping; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; internal static class TrajectoryChecks { public static void Run() { VerifiesForwardFieldsExactTerminalAndHold(); VerifiesReverseTravelVelocityAndUnwrappedYaw(); VerifiesPublishedListsAreImmutable(); VerifiesWorldSpacePublicationMutationsAreRejected(); VerifiesReverseSpeedLimitUsesReverseConfiguration(); } private static void VerifiesForwardFieldsExactTerminalAndHold() { EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( CreatePath(TravelDirection.Forward, 0d, Math.PI / 2d), CreateLongitudinalResult(), CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal)); VerifyKinematicFields(trajectory, TravelDirection.Forward, "forward"); VerifyTerminalAndHold(trajectory, EmBoundaryType.Goal, "forward"); } private static void VerifiesReverseTravelVelocityAndUnwrappedYaw() { EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( CreatePath(TravelDirection.Reverse, 3.10d, -3.10d), CreateLongitudinalResult(), CreateMetadata(TravelDirection.Reverse, EmTerminalType.GearSwitch)); VerifyKinematicFields(trajectory, TravelDirection.Reverse, "reverse"); VerifyTerminalAndHold(trajectory, EmBoundaryType.GearSwitchApproach, "reverse"); EmTrajectoryPoint moving = trajectory.Points[1]; Verification.True(moving.SignedLongitudinalVelocity < 0d, "reverse signed velocity is negative"); Verification.True(moving.VelocityX * Math.Cos(moving.Yaw) + moving.VelocityY * Math.Sin(moving.Yaw) < 0d, "reverse world velocity points opposite the vehicle yaw"); Verification.True(Math.Abs(Math.Abs(moving.Yaw) - Math.PI) < 0.1d, "reverse yaw interpolation unwraps across the pi boundary"); } private static void VerifiesPublishedListsAreImmutable() { EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble( CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(), CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop)); Verification.True(!(trajectory.Points is IList mutable) || mutable.IsReadOnly, "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"); 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"); 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()), CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(990f, 1010f, -10f, 10f) }), CreateValidationMap(new IMapObstacle[] { new AxisAlignedRectangleObstacle(490f, 510f, -10f, 10f) })); } private static PlanningGridMap CreateValidationMap(IReadOnlyList obstacles) { IMapObstacleSource[] sources = obstacles.Count == 0 ? Array.Empty() : 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(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; double previousTime = double.NegativeInfinity; double previousPathS = double.NegativeInfinity; for (int index = 0; index < trajectory.Points.Count; index++) { EmTrajectoryPoint point = trajectory.Points[index]; Verification.NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed, name + " speed field " + index); Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX, name + " velocity X field " + index); Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY, name + " velocity Y field " + index); Verification.NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate, name + " yaw-rate field " + index); if (index < 4) Verification.NearlyEqual(directionSign * CreateLongitudinalResult().Candidate.U[index], point.SignedLongitudinalVelocity, name + " signed speed field " + index); Verification.True(point.TimeFromStart > previousTime, name + " time strictly increases " + index); Verification.True(point.PathS >= previousPathS, name + " PathS never decreases " + index); previousTime = point.TimeFromStart; previousPathS = point.PathS; } } private static void VerifyTerminalAndHold(EmTrajectory trajectory, EmBoundaryType terminalBoundary, string name) { const int terminalIndex = 3; EmTrajectoryPoint terminal = trajectory.Points[terminalIndex]; Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type"); Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed"); Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate"); Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time"); Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples"); for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++) { EmTrajectoryPoint hold = trajectory.Points[index]; Verification.NearlyEqual(terminal.TimeFromStart + (index - terminalIndex) * 0.05d, hold.TimeFromStart, name + " hold timing " + index); Verification.NearlyEqual(terminal.X, hold.X, name + " hold X " + index); Verification.NearlyEqual(terminal.Y, hold.Y, name + " hold Y " + index); Verification.NearlyEqual(terminal.Yaw, hold.Yaw, name + " hold yaw " + index); Verification.NearlyEqual(0d, hold.SignedLongitudinalVelocity, name + " hold signed speed " + index); Verification.NearlyEqual(0d, hold.YawRate, name + " hold yaw rate " + index); } } private static LateralPath CreatePath(TravelDirection direction, double firstYaw, double lastYaw) { return new LateralPath(new[] { new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, firstYaw, 0d, 0.5d, 0d), new LateralPathPoint(1d, 0.12d, 0d, 0d, 0d, 0d, 0.12d, 0d, lastYaw, 0d, 0.5d, 0d), }, true); } private static LongitudinalPlanningResult CreateLongitudinalResult() { var candidate = new LongitudinalCandidate( new[] { 0d, 0.05d, 0.10d, 0.137d }, new[] { 0d, 0.04d, 0.08d, 0.12d }, new[] { 0.8d, 0.8d, 0.8d, 0d }, new[] { 0d, 0d, 0d, 0d }, new[] { 0d, 0d, 0d }); return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty); } private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType) { return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L, "reference", 4L, string.Empty, 2, direction, terminalType); } }