diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs index 64b9d3f..9107273 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs @@ -7,23 +7,26 @@ namespace EMPlannerVerificationHost; internal static class EmFixtureFactory { - public static PathSmoothingResult CreateGearPairReferencePath() + public static PathSmoothingResult CreateGearPairReferencePath(double firstSegmentLength = 2d) { + if (firstSegmentLength <= 0d) + throw new ArgumentOutOfRangeException(nameof(firstSegmentLength)); + double midpoint = 0.5d * firstSegmentLength; var points = new List { Point(0d, 0d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), - Point(1d, 1d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), - Point(2d, 2d, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), - Point(2d, 2d, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch), - Point(1d, 3d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), - Point(0d, 4d, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), + Point(midpoint, midpoint, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), + Point(firstSegmentLength, firstSegmentLength, TravelDirection.Forward, false, SmoothedPathPointSource.Anchor), + Point(firstSegmentLength, firstSegmentLength, TravelDirection.Reverse, true, SmoothedPathPointSource.GearSwitch), + Point(midpoint, firstSegmentLength + midpoint, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), + Point(0d, 2d * firstSegmentLength, TravelDirection.Reverse, false, SmoothedPathPointSource.Anchor), }; var segments = new List { new SmoothedPathSegment(0, TravelDirection.Forward, 0, 2, false, true), new SmoothedPathSegment(1, TravelDirection.Reverse, 3, 5, true, false), }; - var metrics = new PathQualityMetrics(true, 4d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); + var metrics = new PathQualityMetrics(true, 2d * firstSegmentLength, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d); return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments, new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List()); } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index 4fe53a9..d3c34d6 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -15,6 +15,7 @@ internal static class EmPlanningServiceChecks { VerifiesPreviousTrajectoryIsALongitudinalSoftReference(); VerifiesForwardReverseAndBoundarySuccessesAreDeterministic(); + VerifiesServicePublishesRollingApproachAndExactStopModes(); VerifiesRequestAndStateFailuresPublishNoTrajectory(); VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory(); VerifiesTimeoutFallbackAndCancellationSemantics(); @@ -50,6 +51,100 @@ internal static class EmPlanningServiceChecks VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop"); } + private static void VerifiesServicePublishesRollingApproachAndExactStopModes() + { + EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false, + CreateReferencePath(TravelDirection.Forward, false, 10d), CreateMap(false, 12d)); + ConfigureFiveMeterWindowAndTwoSecondHorizon(rollingRequest.Configuration); + EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + rollingRequest, CancellationToken.None); + VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "cycle 1 rolling"); + Verification.Equal(EmLongitudinalMode.RollingContinuation, + rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls"); + Verification.Equal(21, rolling.Trajectory.Points.Count, "rolling publishes the two-second ST knot count"); + EmTrajectoryPoint rollingTerminal = rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1]; + Verification.True(rollingTerminal.PathS < 5d, "two-second ST output remains inside the five-metre LS window"); + Verification.True(rollingTerminal.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed"); + + EmPlanningRequest approachRequest = CreateRequest(TravelDirection.Forward, 0.10d, false, false, + CreateReferencePath(TravelDirection.Forward, false, 4d), CreateMap(false, 5d)); + EmPlanningResult approach = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + approachRequest, CancellationToken.None); + VerifySuccess(approach, approachRequest, EmTerminalType.Goal, "cycle 2 approach"); + Verification.Equal(EmLongitudinalMode.ApproachStopBoundary, + approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches"); + Verification.True(approach.Trajectory.Points[approach.Trajectory.Points.Count - 1].SignedLongitudinalVelocity != 0d, + "approach has no synthetic stop tail"); + + EmPlanningRequest exactRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false, + CreateReferencePath(TravelDirection.Forward, false, 0.0075d)); + ConfigureExactStopServiceScenario(exactRequest.Configuration); + EmPlanningResult exact = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + exactRequest, CancellationToken.None); + VerifySuccess(exact, exactRequest, EmTerminalType.Goal, "cycle 3 exact goal stop"); + Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, + exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops"); + AssertExactStopStabilization(exact.Trajectory, EmBoundaryType.Goal, "goal"); + + EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0.05d, false, false, + EmFixtureFactory.CreateGearPairReferencePath(0.0075d)); + ConfigureExactStopServiceScenario(gearRequest.Configuration); + EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + gearRequest, CancellationToken.None); + VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear-switch exact stop"); + Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary, + gear.Trajectory.Metadata.LongitudinalMode, "gear switch stops exactly"); + AssertExactStopStabilization(gear.Trajectory, EmBoundaryType.GearSwitchApproach, "gear switch"); + for (int index = 0; index < gear.Trajectory.Points.Count; index++) + { + Verification.Equal(TravelDirection.Forward, gear.Trajectory.Points[index].Direction, + "gear-switch publication excludes the next direction point " + index); + } + } + + private static void ConfigureFiveMeterWindowAndTwoSecondHorizon(EmPlannerConfiguration configuration) + { + configuration.Scheduling.DistanceHorizonMeters = 5d; + configuration.Scheduling.TimeHorizonSeconds = 2d; + configuration.Scheduling.OutputTimeStepSeconds = 0.1d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d; + } + + private static void ConfigureExactStopServiceScenario(EmPlannerConfiguration configuration) + { + configuration.Scheduling.TimeHorizonSeconds = 0.40d; + configuration.Scheduling.OutputTimeStepSeconds = 0.10d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d; + configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared = 1e-6d; + configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumJerkMetersPerSecondCubed = 20d; + configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared = 1d; + configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1d; + } + + private static void AssertExactStopStabilization(EmTrajectory trajectory, EmBoundaryType boundaryType, string name) + { + int exactAnchor = -1; + for (int index = 0; index < trajectory.Points.Count; index++) + { + if (trajectory.Points[index].BoundaryType == boundaryType) + { + exactAnchor = index; + break; + } + } + Verification.True(exactAnchor >= 0, name + " has a real boundary anchor"); + Verification.NearlyEqual(0d, trajectory.Points[exactAnchor].SignedLongitudinalVelocity, + name + " speed is zero"); + Verification.True(trajectory.Points.Count > exactAnchor + 1, + name + " anchor is followed by a QP stabilization point"); + Verification.NearlyEqual(trajectory.Points[exactAnchor].PathS, trajectory.Points[exactAnchor + 1].PathS, + name + " stabilization keeps the stop position"); + Verification.NearlyEqual(0d, trajectory.Points[exactAnchor + 1].SignedLongitudinalVelocity, + name + " stabilization speed is zero"); + } + private static void VerifiesRequestAndStateFailuresPublishNoTrajectory() { EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false, @@ -300,7 +395,7 @@ internal static class EmPlanningServiceChecks new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List()); } - private static PlanningGridMap CreateMap(bool blockStart) + private static PlanningGridMap CreateMap(bool blockStart, double halfExtentMeters = 3d) { IMapObstacleSource[] sources = blockStart ? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true, @@ -308,7 +403,7 @@ internal static class EmPlanningServiceChecks : Array.Empty(); PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest { - Bounds = new MapBoundsMm(-3000f, 3000f, -1000f, 1000f), + Bounds = new MapBoundsMm((float)(-1000d * halfExtentMeters), (float)(1000d * halfExtentMeters), -1000f, 1000f), ResolutionMm = 20f, ObstacleSources = sources, AllowExplicitEmptyMap = !blockStart, diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs index 75d08bc..3202c3a 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs @@ -28,9 +28,11 @@ internal static class LongitudinalIntegrationChecks private static void VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed() { EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + configuration.Scheduling.DistanceHorizonMeters = 5d; configuration.Scheduling.TimeHorizonSeconds = 2d; configuration.Scheduling.OutputTimeStepSeconds = 0.10d; configuration.Scheduling.SolverTimeoutSeconds = 1d; + configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.20d; LateralPath path = new LateralPath(new[] { Point(0d, 0d, 0d), @@ -46,9 +48,10 @@ internal static class LongitudinalIntegrationChecks CancellationToken.None); Verification.Equal(EmPlanningStatus.SuccessWithFallback, result.Status, - "rolling seed remains a strict timeout fallback"); + "rolling seed remains a strict timeout fallback: " + result.FailureReason); LongitudinalCandidate candidate = result.Candidate ?? throw new InvalidOperationException("Rolling timeout fallback candidate was missing."); + Verification.Equal(21, candidate.KnotTimes.Count, "two-second ST emits twenty-one knots"); Verification.True(candidate.S[candidate.S.Count - 1] < input.PathUpperBoundS, "two-second ST does not consume the five-metre LS window"); Verification.True(candidate.U[candidate.U.Count - 1] > 0.01d,