From 048b4f618e2237cd0cb3d257bf6ee04a1d13670b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 6 Aug 2026 20:54:22 +0800 Subject: [PATCH] feat: select complete EM direction segments --- .../EMPlanner/Facade/EmPlanningService.cs | 5 +-- .../Segmentation/PlanningHorizonSelector.cs | 20 +++++++++-- .../EmPlanningServiceChecks.cs | 22 ++++++++++-- .../LongitudinalModelChecks.cs | 36 ++++++++++++++++--- .../TrajectoryChecks.cs | 2 +- 5 files changed, 74 insertions(+), 11 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index b7653cc..6fb2696 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -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; } } diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs index ea127cc..8415661 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs @@ -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); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index c9a0268..167af5a 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -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) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs index ee386ac..82b7b28 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs @@ -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); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs index 28eea64..2baf240 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -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");