feat: validate EM planner requests
This commit is contained in:
梁薄云
@@ -1,11 +1,20 @@
|
||||
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),
|
||||
@@ -48,4 +57,190 @@ internal static class FoundationChecks
|
||||
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);
|
||||
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user