diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs new file mode 100644 index 0000000..a214cd6 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs @@ -0,0 +1,342 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; +using MultiWheelC.TrajectoryPlanning.Mapping; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmTrajectoryValidationFailure +{ + None, + InvalidInput, + NonFinite, + TimeNotStrictlyIncreasing, + PathSDecreased, + SegmentBoundaryExceeded, + MissingTerminalAnchor, + TerminalSpeedNotZero, + TerminalYawRateNotZero, + DirectionMismatch, + DirectionSignMismatch, + RedundantSpeedMismatch, + WorldVelocityMismatch, + YawRateMismatch, + SpeedLimitExceeded, + AccelerationLimitExceeded, + JerkLimitExceeded, + CurvatureLimitExceeded, + CurvatureRateLimitExceeded, + PoseCollision, + SweptCollision, +} + +public sealed class EmTrajectoryValidationResult +{ + private EmTrajectoryValidationResult(EmTrajectoryValidationFailure failure, int pointIndex, string message) + { + Failure = failure; + PointIndex = pointIndex; + Message = message ?? string.Empty; + } + + public bool IsValid { get { return Failure == EmTrajectoryValidationFailure.None; } } + + public EmTrajectoryValidationFailure Failure { get; } + + public int PointIndex { get; } + + public string Message { get; } + + internal static EmTrajectoryValidationResult Success() + { + return new EmTrajectoryValidationResult(EmTrajectoryValidationFailure.None, -1, string.Empty); + } + + internal static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int pointIndex, + string message) + { + return new EmTrajectoryValidationResult(failure, pointIndex, message); + } +} + +/// Independently checks the public world-space trajectory before it can be published. +public sealed class EmTrajectoryValidator +{ + private const double MaximumSweptCollisionStepMeters = 0.025d; + private readonly FootprintCollisionChecker collisionChecker; + + public EmTrajectoryValidator() + : this(new FootprintCollisionChecker()) + { + } + + public EmTrajectoryValidator(FootprintCollisionChecker collisionChecker) + { + this.collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker)); + } + + public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle, + EmPlannerConfiguration configuration, int segmentIndex, double terminalPathS, EmBoundaryType terminalBoundary) + { + if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null || + configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 || + !IsFinite(terminalPathS) || terminalPathS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) || + !TryReadLimits(configuration, vehicle, trajectory.Metadata.Direction, out ValidationLimits limits)) + { + return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1, + "Trajectory publication inputs or validation limits are invalid."); + } + + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (point == null || !HasOnlyFiniteValues(point)) + return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point."); + if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > terminalPathS + limits.SpatialTolerance || + point.PathS > terminalPathS + limits.SpatialTolerance) + { + return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index, + "Trajectory point lies outside the current direction segment."); + } + if (index == 0) + continue; + + EmTrajectoryPoint previous = trajectory.Points[index - 1]; + if (point.TimeFromStart <= previous.TimeFromStart) + return Reject(EmTrajectoryValidationFailure.TimeNotStrictlyIncreasing, index, + "Trajectory time must be strictly increasing."); + if (point.PathS + limits.SpatialTolerance < previous.PathS || + point.SegmentLocalS + limits.SpatialTolerance < previous.SegmentLocalS) + { + return Reject(EmTrajectoryValidationFailure.PathSDecreased, index, + "Trajectory PathS must not decrease."); + } + } + + int terminalIndex = FindTerminalAnchor(trajectory, terminalPathS, terminalBoundary, limits.SpatialTolerance); + if (terminalIndex < 0) + { + return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor, + FindFirstTerminalPathIndex(trajectory, terminalPathS, limits.SpatialTolerance), + "Trajectory does not contain the exact terminal boundary anchor."); + } + + EmTrajectoryPoint terminal = trajectory.Points[terminalIndex]; + if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance) + return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex, + "Terminal signed speed must be zero."); + if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance) + return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex, + "Terminal yaw rate must be zero."); + + double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d; + double previousAcceleration = 0d; + bool hasPreviousAcceleration = false; + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (point.Direction != trajectory.Metadata.Direction) + return Reject(EmTrajectoryValidationFailure.DirectionMismatch, index, + "Trajectory point direction differs from trajectory metadata."); + if (Math.Abs(point.SignedLongitudinalVelocity) > limits.KinematicTolerance && + point.SignedLongitudinalVelocity * directionSign < 0d) + { + return Reject(EmTrajectoryValidationFailure.DirectionSignMismatch, index, + "Trajectory signed speed has the wrong direction sign."); + } + if (!NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed, limits.KinematicTolerance)) + return Reject(EmTrajectoryValidationFailure.RedundantSpeedMismatch, index, + "Trajectory Speed is inconsistent with signed speed."); + if (!NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX, + limits.KinematicTolerance) || + !NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY, + limits.KinematicTolerance)) + { + return Reject(EmTrajectoryValidationFailure.WorldVelocityMismatch, index, + "Trajectory world velocity is inconsistent with signed speed and yaw."); + } + if (!NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate, + limits.KinematicTolerance)) + { + return Reject(EmTrajectoryValidationFailure.YawRateMismatch, index, + "Trajectory yaw rate is inconsistent with signed speed and curvature."); + } + if (point.Speed > limits.MaximumSpeed + limits.KinematicTolerance) + return Reject(EmTrajectoryValidationFailure.SpeedLimitExceeded, index, + "Trajectory speed exceeds its direction limit."); + if (Math.Abs(point.VehicleCurvature) > limits.MaximumCurvature + limits.KinematicTolerance) + return Reject(EmTrajectoryValidationFailure.CurvatureLimitExceeded, index, + "Trajectory vehicle curvature exceeds the vehicle limit."); + + if (index == 0) + continue; + + EmTrajectoryPoint previous = trajectory.Points[index - 1]; + double dt = point.TimeFromStart - previous.TimeFromStart; + double previousProgressSpeed = directionSign * previous.SignedLongitudinalVelocity; + double progressSpeed = directionSign * point.SignedLongitudinalVelocity; + double acceleration = (progressSpeed - previousProgressSpeed) / dt; + if (acceleration > limits.MaximumAcceleration + limits.KinematicTolerance || + -acceleration > limits.MaximumDeceleration + limits.KinematicTolerance) + { + return Reject(EmTrajectoryValidationFailure.AccelerationLimitExceeded, index, + "Trajectory finite-difference acceleration exceeds its limit."); + } + if (hasPreviousAcceleration && Math.Abs((acceleration - previousAcceleration) / dt) > limits.MaximumJerk + + limits.KinematicTolerance) + { + return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index, + "Trajectory finite-difference jerk exceeds its limit."); + } + if (Math.Abs(point.VehicleCurvature - previous.VehicleCurvature) / dt > limits.MaximumCurvatureRate + + limits.KinematicTolerance) + { + return Reject(EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, index, + "Trajectory finite-difference curvature rate exceeds its limit."); + } + previousAcceleration = acceleration; + hasPreviousAcceleration = true; + } + + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (!collisionChecker.IsPoseCollisionFree(new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, 0d, out _)) + return Reject(EmTrajectoryValidationFailure.PoseCollision, index, + "Trajectory point fails the full-body world-space collision check."); + } + double sweptStepMeters = Math.Min(MaximumSweptCollisionStepMeters, limits.ConfiguredCollisionStepMeters); + for (int index = 1; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint previous = trajectory.Points[index - 1]; + EmTrajectoryPoint point = trajectory.Points[index]; + if (!collisionChecker.IsSweptMotionCollisionFree(new Pose2D(previous.X, previous.Y, previous.Yaw), + new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, sweptStepMeters, out _)) + { + return Reject(EmTrajectoryValidationFailure.SweptCollision, index, + "Trajectory segment fails the full-body swept world-space collision check."); + } + } + + return EmTrajectoryValidationResult.Success(); + } + + private static int FindTerminalAnchor(EmTrajectory trajectory, double terminalPathS, EmBoundaryType terminalBoundary, + double spatialTolerance) + { + for (int index = 0; index < trajectory.Points.Count; index++) + { + EmTrajectoryPoint point = trajectory.Points[index]; + if (point.BoundaryType == terminalBoundary && Math.Abs(point.PathS - terminalPathS) <= spatialTolerance) + return index; + } + return -1; + } + + private static int FindFirstTerminalPathIndex(EmTrajectory trajectory, double terminalPathS, double spatialTolerance) + { + for (int index = 0; index < trajectory.Points.Count; index++) + { + if (Math.Abs(trajectory.Points[index].PathS - terminalPathS) <= spatialTolerance) + return index; + } + return trajectory.Points.Count - 1; + } + + private static bool HasOnlyFiniteValues(EmTrajectoryPoint point) + { + return IsFinite(point.X) && IsFinite(point.Y) && IsFinite(point.Yaw) && + IsFinite(point.SignedLongitudinalVelocity) && IsFinite(point.Speed) && IsFinite(point.VelocityX) && + IsFinite(point.VelocityY) && IsFinite(point.YawRate) && IsFinite(point.TimeFromStart) && + IsFinite(point.VehicleCurvature) && IsFinite(point.SegmentLocalS) && IsFinite(point.PathS) && + IsFinite(point.LongitudinalAcceleration) && IsFinite(point.LongitudinalJerk); + } + + private static bool TryReadLimits(EmPlannerConfiguration configuration, VehicleParameters vehicle, + TravelDirection direction, out ValidationLimits limits) + { + limits = default; + double maximumCurvature; + if (vehicle.MaximumCurvaturePerMeter.HasValue) + maximumCurvature = vehicle.MaximumCurvaturePerMeter.Value; + else if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d) + maximumCurvature = 1d / vehicle.MinimumTurningRadiusMeters.Value; + else + return false; + + if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse) + return false; + double maximumSpeed = direction == TravelDirection.Forward + ? configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond + : configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond; + if (!IsFinite(maximumSpeed) || maximumSpeed <= 0d || !IsFinite(maximumCurvature) || maximumCurvature <= 0d || + !IsFinite(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared) || + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared <= 0d || + !IsFinite(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared) || + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared <= 0d || + !IsFinite(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed) || + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed <= 0d || + !IsFinite(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond) || + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond <= 0d || + !IsFinite(configuration.Validation.SpatialToleranceMeters) || configuration.Validation.SpatialToleranceMeters < 0d || + !IsFinite(configuration.Validation.KinematicTolerance) || configuration.Validation.KinematicTolerance < 0d || + !IsFinite(configuration.Corridor.MaximumCollisionCheckStepMeters) || + configuration.Corridor.MaximumCollisionCheckStepMeters <= 0d) + { + return false; + } + + limits = new ValidationLimits(maximumSpeed, maximumCurvature, + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, + configuration.Validation.SpatialToleranceMeters, configuration.Validation.KinematicTolerance, + configuration.Corridor.MaximumCollisionCheckStepMeters); + return true; + } + + private static bool NearlyEqual(double expected, double actual, double tolerance) + { + double scale = Math.Max(1d, Math.Max(Math.Abs(expected), Math.Abs(actual))); + return Math.Abs(expected - actual) <= tolerance + tolerance * scale; + } + + private static bool IsFinite(double value) + { + return !double.IsNaN(value) && !double.IsInfinity(value); + } + + private static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int index, string message) + { + return EmTrajectoryValidationResult.Reject(failure, index, message); + } + + private readonly struct ValidationLimits + { + public ValidationLimits(double maximumSpeed, double maximumCurvature, double maximumAcceleration, + double maximumDeceleration, double maximumJerk, double maximumCurvatureRate, double spatialTolerance, + double kinematicTolerance, double configuredCollisionStepMeters) + { + MaximumSpeed = maximumSpeed; + MaximumCurvature = maximumCurvature; + MaximumAcceleration = maximumAcceleration; + MaximumDeceleration = maximumDeceleration; + MaximumJerk = maximumJerk; + MaximumCurvatureRate = maximumCurvatureRate; + SpatialTolerance = spatialTolerance; + KinematicTolerance = kinematicTolerance; + ConfiguredCollisionStepMeters = configuredCollisionStepMeters; + } + + public double MaximumSpeed { get; } + public double MaximumCurvature { get; } + public double MaximumAcceleration { get; } + public double MaximumDeceleration { get; } + public double MaximumJerk { get; } + public double MaximumCurvatureRate { get; } + public double SpatialTolerance { get; } + public double KinematicTolerance { get; } + public double ConfiguredCollisionStepMeters { get; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs index b3e2d8b..b063b26 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -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()), + 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;