First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: separate rolling horizons from stop boundaries

Browse Source
This commit is contained in:
梁薄云
2026-08-05 17:13:09 +08:00
parent e56220fc29
commit 21b20d09d4
9 changed files with 246 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs
+11
View File
@@ -106,6 +106,17 @@ internal static class FoundationChecks
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");
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
+5 -4
View File
@@ -170,7 +170,7 @@ internal static class LongitudinalIntegrationChecks
}, true);
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string speedFailure);
Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
@@ -212,7 +212,7 @@ internal static class LongitudinalIntegrationChecks
}, true);
var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
var solver = new FakeQpSolver(new[]
{
Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
@@ -335,7 +335,8 @@ internal static class LongitudinalIntegrationChecks
Point(2d, terminalPathS, 0d),
};
return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>()));
initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
configuration, Array.Empty<double>(), Array.Empty<double>()));
}
private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
@@ -392,7 +393,7 @@ internal static class LongitudinalIntegrationChecks
Point(2d, terminalPathS, 0d),
}, true);
return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
configuration, Array.Empty<double>(), Array.Empty<double>());
EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
}
private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+49 -21
View File
@@ -16,7 +16,7 @@ internal static class LongitudinalModelChecks
VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
VerifiesStoppingPrecheckBeforeQpAssembly();
VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
}
@@ -64,7 +64,8 @@ internal static class LongitudinalModelChecks
new PathFixture(1d, 1d, 0d, 0d),
});
var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty<double>(), Array.Empty<double>());
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, EmPlannerConfiguration.CreateDefault(),
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
out PathSpeedLimit directionEnvelope, out string directionFailureReason);
Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
@@ -83,7 +84,8 @@ internal static class LongitudinalModelChecks
new PathFixture(21d, 5d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
@@ -124,7 +126,8 @@ internal static class LongitudinalModelChecks
new PathFixture(100d, 2d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
@@ -145,7 +148,8 @@ internal static class LongitudinalModelChecks
new PathFixture(100d, 2d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
@@ -173,7 +177,8 @@ internal static class LongitudinalModelChecks
new PathFixture(100d, 0.01d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
EmTerminalType.RollingSafetyStop, configuration, Array.Empty<double>(), Array.Empty<double>());
EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
Array.Empty<double>(), Array.Empty<double>());
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string failureReason);
@@ -183,24 +188,46 @@ internal static class LongitudinalModelChecks
Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
}
private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal);
EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration,
out PlanningHorizonSelection goalSelection, out string goalFailure);
configuration.Scheduling.DistanceHorizonMeters = 1d;
configuration.Scheduling.TimeHorizonSeconds = 2d;
Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure);
Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type");
Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S");
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
EmPlanningStatus status = new PlanningHorizonSelector().Select(
longSegment, 0d, 0d, 0d, configuration,
out PlanningHorizonSelection rolling, out string failure);
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
"distance horizon defines the LS window");
Verification.Equal(EmLongitudinalMode.RollingContinuation,
rolling.LongitudinalMode, "far boundary rolls");
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach);
status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration,
out PlanningHorizonSelection rollingSelection, out string rollingFailure);
Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure);
Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type");
Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d,
"rolling horizon remains within the current segment");
DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
status = new PlanningHorizonSelector().Select(
visibleButFar, 0d, 0d, 0d, configuration,
out PlanningHorizonSelection approach, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
approach.LongitudinalMode, "visible unreachable boundary approaches");
DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
status = new PlanningHorizonSelector().Select(
reachableGoal, 0d, 0d, 0d, configuration,
out PlanningHorizonSelection exact, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
exact.LongitudinalMode, "reachable goal stops exactly");
Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
"exact stop preserves Goal identity");
IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
"short final interval moves the stop anchor earlier");
}
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
@@ -240,7 +267,8 @@ internal static class LongitudinalModelChecks
new PathFixture(2d, 2d, 0d, 0d),
});
var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
out string speedFailure);