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;