test: cover rolling-to-stop EM planning flow
This commit is contained in:
梁薄云
@@ -7,23 +7,26 @@ namespace EMPlannerVerificationHost;
|
||||
|
||||
internal static class EmFixtureFactory
|
||||
{
|
||||
public static PathSmoothingResult CreateGearPairReferencePath()
|
||||
public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d)
|
||||
{
|
||||
if (firstSegmentLength <= 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(firstSegmentLength));
|
||||
double midpoint = 0.5d * firstSegmentLength;
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
|
||||
Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor),
|
||||
Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch),
|
||||
Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segments = new List<SmoothedPathSegment>
|
||||
{
|
||||
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true),
|
||||
new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false),
|
||||
};
|
||||
var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
||||
var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 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>());
|
||||
}
|
||||
|
||||
@@ -15,6 +15,7 @@ internal static class EmPlanningServiceChecks
|
||||
{
|
||||
VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
|
||||
VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
|
||||
VerifiesServicePublishesRollingApproachAndExactStopModes();
|
||||
VerifiesRequestAndStateFailuresPublishNoTrajectory();
|
||||
VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
|
||||
VerifiesTimeoutFallbackAndCancellationSemantics();
|
||||
@@ -50,6 +51,100 @@ internal static class EmPlanningServiceChecks
|
||||
VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
|
||||
}
|
||||
|
||||
private static void VerifiesServicePublishesRollingApproachAndExactStopModes()
|
||||
{
|
||||
EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
|
||||
CreateReferencePath(TravelDirection.Forward, false, 10d), CreateMap(false, 12d));
|
||||
ConfigureFiveMeterWindowAndTwoSecondHorizon(rollingRequest.Configuration);
|
||||
EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
||||
rollingRequest, CancellationToken.None);
|
||||
VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "cycle 1 rolling");
|
||||
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
||||
rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
|
||||
Verification.Equal(21, rolling.Trajectory.Points.Count, "rolling publishes the two-second ST knot count");
|
||||
EmTrajectoryPoint rollingTerminal = rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1];
|
||||
Verification.True(rollingTerminal.PathS < 5d, "two-second ST output remains inside the five-metre LS window");
|
||||
Verification.True(rollingTerminal.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");
|
||||
|
||||
EmPlanningRequest approachRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false,
|
||||
CreateReferencePath(TravelDirection.Forward, false, 4d), CreateMap(false, 5d));
|
||||
EmPlanningResult approach = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
||||
approachRequest, CancellationToken.None);
|
||||
VerifySuccess(approach, approachRequest, EmTerminalType.Goal, "cycle 2 approach");
|
||||
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
||||
approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");
|
||||
Verification.True(approach.Trajectory.Points[approach.Trajectory.Points.Count - 1].SignedLongitudinalVelocity != 0d,
|
||||
"approach has no synthetic stop tail");
|
||||
|
||||
EmPlanningRequest exactRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
|
||||
CreateReferencePath(TravelDirection.Forward, false, 0.0075d));
|
||||
ConfigureExactStopServiceScenario(exactRequest.Configuration);
|
||||
EmPlanningResult exact = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
||||
exactRequest, CancellationToken.None);
|
||||
VerifySuccess(exact, exactRequest, EmTerminalType.Goal, "cycle 3 exact goal stop");
|
||||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
||||
exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
|
||||
AssertExactStopStabilization(exact.Trajectory, EmBoundaryType.Goal, "goal");
|
||||
|
||||
EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
|
||||
EmFixtureFactory.CreateGearPairReferencePath(0.0075d));
|
||||
ConfigureExactStopServiceScenario(gearRequest.Configuration);
|
||||
EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
|
||||
gearRequest, CancellationToken.None);
|
||||
VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear-switch exact stop");
|
||||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
||||
gear.Trajectory.Metadata.LongitudinalMode, "gear switch stops exactly");
|
||||
AssertExactStopStabilization(gear.Trajectory, EmBoundaryType.GearSwitchApproach, "gear switch");
|
||||
for (int index = 0; index < gear.Trajectory.Points.Count; index++)
|
||||
{
|
||||
Verification.Equal(TravelDirection.Forward, gear.Trajectory.Points[index].Direction,
|
||||
"gear-switch publication excludes the next direction point " + index);
|
||||
}
|
||||
}
|
||||
|
||||
private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
|
||||
{
|
||||
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
||||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
|
||||
}
|
||||
|
||||
private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration)
|
||||
{
|
||||
configuration.Scheduling.TimeHorizonSeconds = 0.40d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
|
||||
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d;
|
||||
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d;
|
||||
configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d;
|
||||
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d;
|
||||
}
|
||||
|
||||
private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name)
|
||||
{
|
||||
int exactAnchor = -1;
|
||||
for (int index = 0; index < trajectory.Points.Count; index++)
|
||||
{
|
||||
if (trajectory.Points[index].BoundaryType == boundaryType)
|
||||
{
|
||||
exactAnchor = index;
|
||||
break;
|
||||
}
|
||||
}
|
||||
Verification.True(exactAnchor >= 0, name + " has a real boundary anchor");
|
||||
Verification.NearlyEqual(0d, trajectory.Points[exactAnchor].SignedLongitudinalVelocity,
|
||||
name + " speed is zero");
|
||||
Verification.True(trajectory.Points.Count > exactAnchor + 1,
|
||||
name + " anchor is followed by a QP stabilization point");
|
||||
Verification.NearlyEqual(trajectory.Points[exactAnchor].PathS, trajectory.Points[exactAnchor + 1].PathS,
|
||||
name + " stabilization keeps the stop position");
|
||||
Verification.NearlyEqual(0d, trajectory.Points[exactAnchor + 1].SignedLongitudinalVelocity,
|
||||
name + " stabilization speed is zero");
|
||||
}
|
||||
|
||||
private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
|
||||
{
|
||||
EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
|
||||
@@ -300,7 +395,7 @@ internal static class EmPlanningServiceChecks
|
||||
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateMap(bool blockStart)
|
||||
private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d)
|
||||
{
|
||||
IMapObstacleSource[] sources = blockStart
|
||||
? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
|
||||
@@ -308,7 +403,7 @@ internal static class EmPlanningServiceChecks
|
||||
: Array.Empty<IMapObstacleSource>();
|
||||
PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f),
|
||||
Bounds = new MapBoundsMm((float)(-1000d * halfExtentMeters), (float)(1000d * halfExtentMeters), -1000f, 1000f),
|
||||
ResolutionMm = 20f,
|
||||
ObstacleSources = sources,
|
||||
AllowExplicitEmptyMap = !blockStart,
|
||||
|
||||
@@ -28,9 +28,11 @@ internal static class LongitudinalIntegrationChecks
|
||||
private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
||||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||||
configuration.Scheduling.OutputTimeStepSeconds = 0.10d;
|
||||
configuration.Scheduling.SolverTimeoutSeconds = 1d;
|
||||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d;
|
||||
LateralPath path = new LateralPath(new[]
|
||||
{
|
||||
Point(0d, 0d, 0d),
|
||||
@@ -46,9 +48,10 @@ internal static class LongitudinalIntegrationChecks
|
||||
CancellationToken.None);
|
||||
|
||||
Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status,
|
||||
"rolling seed remains a strict timeout fallback");
|
||||
"rolling seed remains a strict timeout fallback: " + result.FailureReason);
|
||||
LongitudinalCandidate candidate = result.Candidate ??
|
||||
throw new InvalidOperationException("Rolling timeout fallback candidate was missing.");
|
||||
Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots");
|
||||
Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS,
|
||||
"two-second ST does not consume the five-metre LS window");
|
||||
Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,
|
||||
|
||||
Reference in New Issue
Block a user