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

feat: select complete EM direction segments

Browse Source
This commit is contained in:
梁薄云
2026-08-06 20:55:47 +08:00
parent f4e89b4b4f
commit 048b4f618e
5 changed files with 74 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+32 -4
View File
@@ -18,6 +18,7 @@ internal static class LongitudinalModelChecks
VerifiesStoppingEnvelopeUsesJerkLimitedStoppingMath();
VerifiesStoppingPrecheckOnlyAppliesToRealStopBoundaries();
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
VerifiesFullDirectionScopeSelectsActualSegmentBoundary();
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
VerifiesModeSpecificSolutionValidation();
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
@@ -107,7 +108,7 @@ internal static class LongitudinalModelChecks
out PathSpeedLimit directionEnvelope, out string directionFailureReason);
Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
directionFailureReason);
Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
Verification.NearlyEqual(1d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
"default direction speed limit");
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -235,7 +236,7 @@ internal static class LongitudinalModelChecks
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
EmPlanningStatus status = new PlanningHorizonSelector().Select(
longSegment, 0d, 0d, 0d, configuration,
longSegment, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
out PlanningHorizonSelection rolling, out string failure);
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
@@ -245,7 +246,7 @@ internal static class LongitudinalModelChecks
DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
status = new PlanningHorizonSelector().Select(
visibleButFar, 0d, 0d, 0d, configuration,
visibleButFar, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
out PlanningHorizonSelection approach, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
@@ -253,7 +254,7 @@ internal static class LongitudinalModelChecks
DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
status = new PlanningHorizonSelector().Select(
reachableGoal, 0d, 0d, 0d, configuration,
reachableGoal, 0d, 0d, 0d, EmPlanningScope.RollingHorizon, configuration,
out PlanningHorizonSelection exact, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
@@ -269,6 +270,33 @@ internal static class LongitudinalModelChecks
"short final interval moves the stop anchor earlier");
}
private static void VerifiesFullDirectionScopeSelectsActualSegmentBoundary()
{
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
configuration.Scheduling.DistanceHorizonMeters = 1d;
DirectionSegmentView tenMeterGoal = CreateSegment(10d, EmBoundaryType.Goal);
var selector = new PlanningHorizonSelector();
EmPlanningStatus status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection full, out string failure);
Verification.Equal(EmPlanningStatus.Success, status, "full selection: " + failure);
Verification.NearlyEqual(10d, full.WindowEndReferenceS, "full selection reaches actual segment end");
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, full.LongitudinalMode,
"full selection stops at its real boundary");
Verification.True(full.HasStopBoundary, "full selection retains the real stop boundary");
status = selector.Select(tenMeterGoal, 3d, 0d, 0d,
EmPlanningScope.RollingHorizon, configuration, out PlanningHorizonSelection rolling, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
Verification.NearlyEqual(4d, rolling.WindowEndReferenceS, "rolling selection retains the one-metre window");
DirectionSegmentView gearSwitch = CreateSegment(10d, EmBoundaryType.GearSwitchApproach);
status = selector.Select(gearSwitch, 3d, 0d, 0d,
EmPlanningScope.FullDirectionSegment, configuration, out PlanningHorizonSelection gear, out failure);
Verification.Equal(EmPlanningStatus.Success, status, "gear full selection: " + failure);
Verification.Equal(EmTerminalType.GearSwitch, gear.TerminalType, "gear full selection preserves switch terminal");
Verification.NearlyEqual(10d, gear.WindowEndReferenceS, "gear full selection stops before the next segment");
}
private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
{
var layout = new LongitudinalVariableLayout(5);