469 lines
28 KiB
C#
469 lines
28 KiB
C#
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();
|
|
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
|
|
VerifiesExactStopHoldHasNoJerkDiscontinuity();
|
|
VerifiesWorldSpacePublicationMutationsAreRejected();
|
|
VerifiesTerminalPoseAndSampleLimitPublicationGates();
|
|
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,
|
|
EmLongitudinalMode.RollingContinuation));
|
|
Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
|
|
"trajectory public point list is immutable");
|
|
}
|
|
|
|
private static void VerifiesRollingTrajectoryHasNoSyntheticStopTail()
|
|
{
|
|
EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
|
|
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
|
|
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
|
EmLongitudinalMode.RollingContinuation));
|
|
|
|
Verification.Equal(21, trajectory.Points.Count, "rolling trajectory keeps only ST knots");
|
|
EmTrajectoryPoint terminal = trajectory.Points[trajectory.Points.Count - 1];
|
|
Verification.Equal(EmBoundaryType.None, terminal.BoundaryType,
|
|
"rolling horizon end is not a boundary anchor");
|
|
Verification.True(terminal.SignedLongitudinalVelocity > 0d,
|
|
"rolling terminal speed stays nonzero");
|
|
Verification.Equal(EmLongitudinalMode.RollingContinuation, trajectory.Metadata.LongitudinalMode,
|
|
"rolling longitudinal mode is published");
|
|
}
|
|
|
|
private static void VerifiesExactStopHoldHasNoJerkDiscontinuity()
|
|
{
|
|
ValidationContext context = CreateValidationContext();
|
|
EmTrajectory trajectory = CreateValidationTrajectory(TravelDirection.Forward);
|
|
EmTrajectoryValidationResult result = new EmTrajectoryValidator().Validate(trajectory, context.EmptyMap,
|
|
context.Vehicle, context.Configuration, 2, 0.0055d, EmBoundaryType.Goal);
|
|
Verification.True(result.IsValid, "exact stop with internal tail and external hold is publishable: " + result.Message);
|
|
|
|
int anchorIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
EmTrajectoryPoint previousMoving = trajectory.Points[anchorIndex - 1];
|
|
EmTrajectoryPoint anchor = trajectory.Points[anchorIndex];
|
|
EmTrajectoryPoint stabilization = trajectory.Points[anchorIndex + 1];
|
|
EmTrajectoryPoint firstExternalHold = trajectory.Points[anchorIndex + 2];
|
|
Verification.Equal(EmBoundaryType.Goal, anchor.BoundaryType, "exact stop marks only the real boundary anchor");
|
|
Verification.Equal(EmBoundaryType.None, stabilization.BoundaryType,
|
|
"QP stabilization point is not a duplicate boundary anchor");
|
|
Verification.Equal(EmBoundaryType.None, firstExternalHold.BoundaryType,
|
|
"external hold is not a duplicate boundary anchor");
|
|
|
|
double terminalFiniteDifferenceAcceleration =
|
|
(anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
|
|
(anchor.TimeFromStart - previousMoving.TimeFromStart);
|
|
double stabilizationAcceleration =
|
|
(stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
|
|
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
|
double externalHoldAcceleration =
|
|
(firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
|
|
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
|
double jerkIntoStabilization = (stabilizationAcceleration - terminalFiniteDifferenceAcceleration) /
|
|
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
|
double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
|
|
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
|
Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
|
|
"jerk into QP stabilization stays within the limit");
|
|
Verification.NearlyEqual(0d, jerkIntoExternalHold, "jerk into the external hold is zero");
|
|
}
|
|
|
|
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: 1.10d)),
|
|
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");
|
|
|
|
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
AssertRejected(context, Replace(valid, terminalIndex, Clone(valid.Points[terminalIndex], boundaryType: EmBoundaryType.None)),
|
|
EmTrajectoryValidationFailure.MissingTerminalAnchor, terminalIndex - 1, "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, "LongitudinalAcceleration", 0.01d),
|
|
EmTrajectoryValidationFailure.TerminalAccelerationNotZero, terminalIndex, "terminal acceleration");
|
|
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 VerifiesTerminalPoseAndSampleLimitPublicationGates()
|
|
{
|
|
ValidationContext context = CreateValidationContext();
|
|
EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward);
|
|
int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
|
CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds);
|
|
EmTrajectoryPoint terminal = valid.Points[terminalIndex];
|
|
var validator = new EmTrajectoryValidator();
|
|
|
|
EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw),
|
|
EmBoundaryType.Goal);
|
|
Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted");
|
|
EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw),
|
|
EmBoundaryType.Goal);
|
|
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure,
|
|
"3.1-centimetre terminal position error is rejected");
|
|
|
|
double degrees = Math.PI / 180d;
|
|
EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees));
|
|
EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees),
|
|
EmBoundaryType.Goal);
|
|
Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error");
|
|
EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees),
|
|
EmBoundaryType.Goal);
|
|
Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted");
|
|
EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees),
|
|
EmBoundaryType.Goal);
|
|
Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure,
|
|
"5.1-degree terminal yaw error is rejected");
|
|
|
|
EmTrajectory rolling = new EmTrajectoryAssembler().Assemble(
|
|
CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(),
|
|
CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop,
|
|
EmLongitudinalMode.RollingContinuation));
|
|
EmTrajectoryValidationResult rollingResult = validator.Validate(rolling, context.EmptyMap, context.Vehicle,
|
|
context.Configuration, 2, 0.08d, new Pose2D(99d, 99d, Math.PI), EmBoundaryType.RollingSafetyStop);
|
|
Verification.True(rollingResult.IsValid, "rolling-safety window end does not use the terminal-pose gate");
|
|
|
|
EmPlannerConfiguration limitedConfiguration = EmPlannerConfiguration.CreateDefault();
|
|
limitedConfiguration.Scheduling.MaximumPublishedSampleCount = 8;
|
|
EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(limitedConfiguration).TryAssemble(
|
|
CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
|
|
CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal), out EmTrajectory limitedTrajectory,
|
|
out string assemblyFailure);
|
|
Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, assemblyStatus,
|
|
"sample schedule one above the limit is rejected");
|
|
Verification.True(limitedTrajectory == null, "sample-limit rejection does not assemble a partial trajectory");
|
|
Verification.True(assemblyFailure.IndexOf("MaximumPublishedSampleCount", StringComparison.Ordinal) >= 0,
|
|
"sample-limit rejection preserves a diagnostic");
|
|
Verification.True(assemblyFailure.IndexOf("required-at-least=9", StringComparison.Ordinal) >= 0,
|
|
"sample-limit fixture exceeds the cap by exactly one sample");
|
|
}
|
|
|
|
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)
|
|
{
|
|
LongitudinalCandidate candidate = CreateValidationCandidate();
|
|
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 LongitudinalCandidate CreateValidationCandidate()
|
|
{
|
|
double[] times = { 0d, 0.05d, 0.10d, 0.15d, 0.20d, 0.25d, 0.30d, 0.35d, 0.40d, 0.45d, 0.50d };
|
|
double[] pathS = { 0d, 0.0014375d, 0.00271875d, 0.00378125d, 0.0045625d, 0.0050625d, 0.00534375d,
|
|
0.00546875d, 0.0055d, 0.0055d, 0.0055d };
|
|
double[] speed = { 0.03d, 0.0275d, 0.02375d, 0.01875d, 0.0125d, 0.0075d, 0.00375d, 0.00125d, 0d, 0d, 0d };
|
|
double[] acceleration = { -0.05d, -0.075d, -0.10d, -0.125d, -0.10d, -0.075d, -0.05d, -0.025d, 0d, 0d, 0d };
|
|
double[] jerk = { -0.5d, -0.5d, -0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0.5d, 0d, 0d };
|
|
return new LongitudinalCandidate(times, pathS, speed, acceleration, jerk);
|
|
}
|
|
|
|
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? y = null, double? yaw = null,
|
|
double? timeFromStart = null, double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null,
|
|
EmBoundaryType? boundaryType = null)
|
|
{
|
|
return new EmTrajectoryPoint(x ?? point.X, y ?? point.Y, yaw ?? 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.15d, terminal.TimeFromStart, name + " exact terminal stabilization time");
|
|
Verification.Equal(terminalIndex + 6, 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.15d, 0.20d },
|
|
new[] { 0d, 0.04d, 0.08d, 0.12d, 0.12d },
|
|
new[] { 0.8d, 0.8d, 0.2d, 0d, 0d },
|
|
new[] { 0d, 0d, 0d, 0d, 0d },
|
|
new[] { 0d, 0d, 0d, 0d });
|
|
return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
|
|
}
|
|
|
|
private static LongitudinalPlanningResult CreateRollingLongitudinalResult()
|
|
{
|
|
var times = new double[21];
|
|
var pathS = new double[21];
|
|
var speed = new double[21];
|
|
var acceleration = new double[21];
|
|
var jerk = new double[20];
|
|
for (int index = 0; index < times.Length; index++)
|
|
{
|
|
times[index] = index * 0.1d;
|
|
pathS[index] = index * 0.004d;
|
|
speed[index] = 0.04d;
|
|
}
|
|
return new LongitudinalPlanningResult(EmPlanningStatus.Success,
|
|
new LongitudinalCandidate(times, pathS, speed, acceleration, jerk), string.Empty);
|
|
}
|
|
|
|
private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType,
|
|
EmLongitudinalMode longitudinalMode = EmLongitudinalMode.ExactStopAtBoundary)
|
|
{
|
|
return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
|
|
"reference", 4L, string.Empty, 2, direction, terminalType, longitudinalMode,
|
|
EmPlanningScope.RollingHorizon);
|
|
}
|
|
}
|