258 lines
18 KiB
C#
258 lines
18 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
internal static class FoundationChecks
|
|
{
|
|
public static void Run()
|
|
{
|
|
VerifyContracts();
|
|
VerifyConfigurationAndRequestValidation();
|
|
}
|
|
|
|
private static void VerifyContracts()
|
|
{
|
|
var reverseState = new VehicleMotionState(
|
|
new Pose2D(1.5d, -2d, 0.25d),
|
|
-0.15d,
|
|
-0.03d,
|
|
new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero),
|
|
17L);
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond,
|
|
"reverse signed speed");
|
|
|
|
var point = new EmTrajectoryPoint(
|
|
1.25d,
|
|
-0.75d,
|
|
0.5d,
|
|
-0.12d,
|
|
0.4d,
|
|
-0.2d,
|
|
3,
|
|
0.6d,
|
|
0.8d,
|
|
TravelDirection.Reverse,
|
|
EmBoundaryType.GearSwitchApproach,
|
|
-0.04d,
|
|
0.03d);
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature");
|
|
EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s");
|
|
EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction");
|
|
EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
|
|
}
|
|
|
|
private static void VerifyConfigurationAndRequestValidation()
|
|
{
|
|
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(5d, configuration.Scheduling.DistanceHorizonMeters, "distance horizon");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.MaximumVehicleStateAgeSeconds, "vehicle state age");
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Corridor.LongitudinalSampleSpacingMeters, "longitudinal sample spacing");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.LateralSampleSpacingMeters, "lateral sample spacing");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Corridor.MaximumLateralOffsetMeters, "maximum lateral offset");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.02d, configuration.Corridor.AdditionalClearanceReserveMeters, "clearance reserve");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.MaximumCollisionCheckStepMeters, "collision check step");
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Frenet.MaximumProjectionDistanceMeters, "maximum projection distance");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Frenet.MinimumFrenetDenominator, "minimum Frenet denominator");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1e-8d, configuration.Frenet.BoundaryAnchorToleranceMeters, "boundary anchor tolerance");
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Lateral.MaximumLateralStepPerIterationMeters, "lateral trust region");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Lateral.MaximumLateralSlope, "maximum lateral slope");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative");
|
|
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared, "maximum lateral acceleration");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, "maximum curvature rate");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.01d, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, "stop speed tolerance");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.ZeroSpeedHoldSeconds, "zero speed hold");
|
|
|
|
EMPlannerVerificationHost.Verification.Equal(5, configuration.Solver.MaximumOuterIterations, "maximum outer iterations");
|
|
EMPlannerVerificationHost.Verification.Equal(4000, configuration.Solver.MaximumOsqpIterations, "maximum OSQP iterations");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
|
|
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
|
|
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
|
|
EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");
|
|
|
|
VerifyLateralWeights(configuration.Lateral.Weights);
|
|
VerifyLongitudinalWeights(configuration.Longitudinal.Weights);
|
|
|
|
EmPlannerConfiguration insufficientPreview = EmPlannerConfiguration.CreateDefault();
|
|
insufficientPreview.Scheduling.DistanceHorizonMeters = 0.01d;
|
|
EmPlanningRequestValidationResult insufficientPreviewValidation = EmPlanningRequestValidator.Validate(
|
|
CreateValidRequest(insufficientPreview));
|
|
AssertStatus(EmPlanningStatus.InvalidInput, insufficientPreviewValidation, "insufficient stopping preview");
|
|
EMPlannerVerificationHost.Verification.True(
|
|
insufficientPreviewValidation.FailureReason.Contains("DistanceHorizonMeters") &&
|
|
insufficientPreviewValidation.FailureReason.Contains("required") &&
|
|
insufficientPreviewValidation.FailureReason.Contains("configured"),
|
|
"insufficient stopping preview describes configured and required distance");
|
|
|
|
EmPlanningRequest valid = CreateValidRequest(configuration);
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, null!, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null map");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, null!, 0, EmMotionModel.NonholonomicForwardReverse)), "null configuration");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, CreateMap(false), valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "map not ready");
|
|
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(CreateFailedReferencePath(), valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "smoothing failure");
|
|
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)), "segment index");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(double.NaN, 9L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "non-finite speed");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, -1L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "negative sequence ID");
|
|
AssertStatus(EmPlanningStatus.StaleVehicleState, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, 9L, valid.RequestedAtUtc.AddSeconds(-1d)), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "stale state");
|
|
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d }, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "missing curvature limit");
|
|
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.CrabTranslation)), "crab motion");
|
|
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.InPlaceRotation)), "in-place rotation");
|
|
|
|
EmPlanningRequestValidationResult first = EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
|
EmPlanningRequestValidationResult second = EmPlanningRequestValidator.Validate(
|
|
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
|
EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message");
|
|
}
|
|
|
|
private static void VerifyLateralWeights(LateralWeights weights)
|
|
{
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.HeadingDeviation, "lateral heading weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.SecondDerivative, "lateral second derivative weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ThirdDerivative, "lateral third derivative weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.Curvature, "lateral curvature weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(20d, weights.CurvatureVariation, "lateral curvature variation weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "lateral previous weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.RollingTerminal, "lateral rolling terminal weight");
|
|
}
|
|
|
|
private static void VerifyLongitudinalWeights(LongitudinalWeights weights)
|
|
{
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceSpeed, "longitudinal speed weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.Acceleration, "longitudinal acceleration weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.Jerk, "longitudinal jerk weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "longitudinal previous weight");
|
|
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.TerminalAcceleration, "longitudinal terminal acceleration weight");
|
|
}
|
|
|
|
private static void AssertStatus(EmPlanningStatus expected, EmPlanningRequestValidationResult actual, string name)
|
|
{
|
|
EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name);
|
|
}
|
|
|
|
private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration)
|
|
{
|
|
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
|
return CreateRequest(
|
|
CreateValidReferencePath(),
|
|
CreateMap(true),
|
|
new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d, SafetyMarginMeters = 0.05d, MaximumCurvaturePerMeter = 0.5d },
|
|
CreateState(0d, 9L, requestedAtUtc),
|
|
configuration,
|
|
0,
|
|
EmMotionModel.NonholonomicForwardReverse);
|
|
}
|
|
|
|
private static EmPlanningRequest CreateRequest(
|
|
PathSmoothingResult referencePath,
|
|
PlanningGridMap map,
|
|
VehicleParameters vehicle,
|
|
VehicleMotionState vehicleState,
|
|
EmPlannerConfiguration configuration,
|
|
int segmentIndex,
|
|
EmMotionModel motionModel)
|
|
{
|
|
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
|
return new EmPlanningRequest(
|
|
referencePath,
|
|
map,
|
|
vehicle,
|
|
vehicleState,
|
|
configuration,
|
|
segmentIndex,
|
|
null!,
|
|
requestedAtUtc,
|
|
requestedAtUtc.AddSeconds(0.1d),
|
|
"trajectory-1",
|
|
"reference-1",
|
|
string.Empty,
|
|
motionModel);
|
|
}
|
|
|
|
private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc)
|
|
{
|
|
return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, null, capturedAtUtc, sequenceId);
|
|
}
|
|
|
|
private static PlanningGridMap CreateMap(bool planningReady)
|
|
{
|
|
var result = new PlanningMapFactory().Create(new PlanningMapRequest
|
|
{
|
|
Bounds = new MapBoundsMm(-2000f, 2000f, -2000f, 2000f),
|
|
ResolutionMm = 100f,
|
|
ObstacleSources = new List<IMapObstacleSource>(),
|
|
AllowExplicitEmptyMap = planningReady,
|
|
});
|
|
if (!result.Succeeded || result.Map == null)
|
|
throw new InvalidOperationException("Unable to create test planning map.");
|
|
return result.Map;
|
|
}
|
|
|
|
private static PathSmoothingResult CreateValidReferencePath()
|
|
{
|
|
var points = new List<SmoothedPathPoint>
|
|
{
|
|
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
new SmoothedPathPoint(1d, 0d, 0d, 0d, 1d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
|
};
|
|
var segments = new List<SmoothedPathSegment>
|
|
{
|
|
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 1, false, false),
|
|
};
|
|
var metrics = new PathQualityMetrics(true, 1d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
|
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
|
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
|
}
|
|
|
|
private static PathSmoothingResult CreateFailedReferencePath()
|
|
{
|
|
return PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics());
|
|
}
|
|
}
|