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/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs
+3 -2
View File
@@ -55,7 +55,8 @@ public sealed class EmPlanningService : IEmPlanningService
double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
var horizonSelector = new PlanningHorizonSelector();
EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
out string horizonReason);
if (horizonStatus != EmPlanningStatus.Success)
return Failure(horizonStatus, request, horizonReason);
ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
@@ -212,6 +213,6 @@ public sealed class EmPlanningService : IEmPlanningService
";reference=" + (request.ReferencePathId ?? string.Empty) +
";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
";segment=" + request.SegmentIndex;
";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
}
}
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs
+18 -2
View File
@@ -40,7 +40,8 @@ public sealed class PlanningHorizonSelector
public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
EmPlanningScope planningScope, EmPlannerConfiguration configuration, out PlanningHorizonSelection selection,
out string failureReason)
{
selection = null;
failureReason = string.Empty;
@@ -48,7 +49,8 @@ public sealed class PlanningHorizonSelector
!IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
!IsFinite(initialAccelerationMetersPerSecondSquared))
!IsFinite(initialAccelerationMetersPerSecondSquared) ||
!Enum.IsDefined(typeof(EmPlanningScope), planningScope))
{
failureReason = "Planning horizon inputs are invalid.";
return EmPlanningStatus.InvalidInput;
@@ -90,6 +92,20 @@ public sealed class PlanningHorizonSelector
return EmPlanningStatus.StoppingDistanceInsufficient;
}
if (planningScope == EmPlanningScope.FullDirectionSegment)
{
EmBoundaryType boundary = segment.EndBoundary.BoundaryType;
if (!IsStopBoundary(boundary))
{
failureReason = "A full direction segment must end at Goal or GearSwitch.";
return EmPlanningStatus.InvalidReferencePath;
}
selection = new PlanningHorizonSelection(segment.LengthMeters, boundary,
ToTerminalType(boundary), EmLongitudinalMode.ExactStopAtBoundary,
segment.LengthMeters, true);
return EmPlanningStatus.Success;
}
double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
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)
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);
ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs
+1 -1
View File
@@ -135,7 +135,7 @@ internal static class TrajectoryChecks
EmTrajectoryValidationFailure.WorldVelocityMismatch, 1, "world velocity");
AssertRejected(context, CorruptDouble(valid, 1, "YawRate", valid.Points[1].YawRate + 0.01d),
EmTrajectoryValidationFailure.YawRateMismatch, 1, "yaw rate");
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 0.30d)),
AssertRejected(context, Replace(valid, 1, Clone(valid.Points[1], signedSpeed: 1.10d)),
EmTrajectoryValidationFailure.SpeedLimitExceeded, 1, "speed limit");
AssertRejected(context, Replace(valid, 2, Clone(valid.Points[2], signedSpeed: 0.19d)),
EmTrajectoryValidationFailure.AccelerationLimitExceeded, 2, "acceleration limit");