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/EmPlanningServiceChecks.cs
+20 -2
View File
@@ -16,6 +16,7 @@ internal static class EmPlanningServiceChecks
VerifiesPreviousTrajectoryIsALongitudinalSoftReference();
VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
VerifiesServicePublishesRollingApproachAndExactStopModes();
VerifiesFullScopePublishesItsRequestScope();
VerifiesRequestAndStateFailuresPublishNoTrajectory();
VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
VerifiesTimeoutFallbackAndCancellationSemantics();
@@ -102,6 +103,22 @@ internal static class EmPlanningServiceChecks
}
}
private static void VerifiesFullScopePublishesItsRequestScope()
{
EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0.05d, false, false,
CreateReferencePath(TravelDirection.Forward, false, 0.0075d), null,
EmPlanningScope.FullDirectionSegment);
ConfigureExactStopServiceScenario(request.Configuration);
EmPlanningResult result = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(
request, CancellationToken.None);
VerifySuccess(result, request, EmTerminalType.Goal, "full scope publication");
Verification.Equal(EmPlanningScope.FullDirectionSegment, result.Trajectory.Metadata.PlanningScope,
"service metadata preserves full scope");
Verification.NearlyEqual(0d, result.Trajectory.Points[0].PathS, "full publication starts at projection");
Verification.NearlyEqual(0.0075d, result.Trajectory.Points[result.Trajectory.Points.Count - 1].PathS,
"full publication reaches the real segment boundary");
}
private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration)
{
configuration.Scheduling.DistanceHorizonMeters = 5d;
@@ -314,7 +331,8 @@ internal static class EmPlanningServiceChecks
}
private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null)
bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null,
EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
{
DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
@@ -336,7 +354,7 @@ internal static class EmPlanningServiceChecks
},
new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.RollingHorizon);
EmMotionModel.NonholonomicForwardReverse, planningScope);
}
private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)