From e844fe00a8c5c9870deeadbb614d1095902d23a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 08:01:18 +0800 Subject: [PATCH] feat: validate EM terminal world pose --- .../EMPlanner/Facade/EmPlanningService.cs | 31 ++++++++- .../Trajectory/EmTrajectoryAssembler.cs | 32 +++++++-- .../Trajectory/TrajectorySampleSchedule.cs | 50 ++++++++++++++ .../Validation/EmTrajectoryValidator.cs | 67 ++++++++++++++++++- .../EmPlanningServiceChecks.cs | 15 +++++ .../TrajectoryChecks.cs | 65 +++++++++++++++++- 6 files changed, 249 insertions(+), 11 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs index e47fb13..3527637 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs @@ -124,15 +124,22 @@ public sealed class EmPlanningService : IEmPlanningService request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId, segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode, request.PlanningScope); - EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata); + EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path, + longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure); + if (assemblyStatus != EmPlanningStatus.Success) + return Failure(assemblyStatus, request, assemblyFailure); EmitDebug(request, "trajectory assembly succeeded"); + EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType; + Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary) + ? TerminalPose(lateral.Path) + : null; EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map, request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS, - slice.TerminalBoundary.BoundaryType); + terminalPose, terminalBoundary); if (!publication.IsValid) { - return Failure(EmPlanningStatus.ValidationFailed, request, + return Failure(MapPublicationFailure(publication.Failure), request, publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message); } EmitDebug(request, "world-space publication validation succeeded"); @@ -225,6 +232,24 @@ public sealed class EmPlanningService : IEmPlanningService return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback; } + internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure) + { + return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch + ? EmPlanningStatus.TerminalPoseMismatch + : EmPlanningStatus.ValidationFailed; + } + + private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType) + { + return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach; + } + + private static Pose2D TerminalPose(LateralPath path) + { + LateralPathPoint terminal = path.Points[path.Points.Count - 1]; + return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw); + } + private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason) { return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty)); diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs index 42c47a6..8793677 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs @@ -9,6 +9,7 @@ public sealed class EmTrajectoryAssembler { private readonly double outputTimeStepSeconds; private readonly double zeroSpeedHoldSeconds; + private readonly int maximumPublishedSampleCount; public EmTrajectoryAssembler() : this(EmPlannerConfiguration.CreateDefault()) @@ -21,8 +22,9 @@ public sealed class EmTrajectoryAssembler throw new ArgumentNullException(nameof(configuration)); outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds; zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds; + maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount; if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) || - zeroSpeedHoldSeconds < 0d) + zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2) { throw new ArgumentOutOfRangeException(nameof(configuration)); } @@ -30,6 +32,18 @@ public sealed class EmTrajectoryAssembler public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata) { + EmPlanningStatus status = TryAssemble(path, longitudinal, metadata, out EmTrajectory trajectory, + out string failureReason); + if (status != EmPlanningStatus.Success) + throw new ArgumentException(failureReason, nameof(longitudinal)); + return trajectory; + } + + public EmPlanningStatus TryAssemble(LateralPath path, LongitudinalPlanningResult longitudinal, + EmTrajectoryMetadata metadata, out EmTrajectory trajectory, out string failureReason) + { + trajectory = null; + failureReason = string.Empty; if (longitudinal == null || longitudinal.Candidate == null || (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback)) { @@ -40,8 +54,17 @@ public sealed class EmTrajectoryAssembler var interpolator = new LateralPathInterpolator(path); bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment; - var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, - isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment); + double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds; + if (TrajectorySampleSchedule.ExceedsMaximumSampleCount(longitudinal.Candidate, outputTimeStepSeconds, + holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, maximumPublishedSampleCount, + out int requiredSampleCount)) + { + failureReason = "FullSegmentResourceLimitExceeded: MaximumPublishedSampleCount=" + + maximumPublishedSampleCount + ";required-at-least=" + requiredSampleCount; + return EmPlanningStatus.FullSegmentResourceLimitExceeded; + } + var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds, + metadata.LongitudinalMode, isFullDirectionSegment); double terminalPathS = path.Points[path.Points.Count - 1].PathS; var points = new List(schedule.Samples.Count); double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d; @@ -62,7 +85,8 @@ public sealed class EmTrajectoryAssembler boundaryType, sample.Acceleration, sample.Jerk)); } - return new EmTrajectory(metadata, points); + trajectory = new EmTrajectory(metadata, points); + return EmPlanningStatus.Success; } private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs index 3b5b9c4..6b9c654 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs @@ -86,6 +86,50 @@ internal sealed class TrajectorySampleSchedule public IReadOnlyList Samples { get; } public int TerminalAnchorSampleIndex { get; } + internal static bool ExceedsMaximumSampleCount(LongitudinalCandidate candidate, double outputTimeStepSeconds, + double holdDurationSeconds, EmLongitudinalMode mode, bool resampleMotion, int maximumSampleCount, + out int requiredSampleCount) + { + requiredSampleCount = 0; + if (candidate == null) + throw new ArgumentNullException(nameof(candidate)); + if (maximumSampleCount < 1) + throw new ArgumentOutOfRangeException(nameof(maximumSampleCount)); + + if (resampleMotion) + { + double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1]; + for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance; + sampleTime += outputTimeStepSeconds) + { + if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount)) + return true; + } + if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount)) + return true; + } + else + { + for (int index = 0; index < candidate.KnotTimes.Count; index++) + { + if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount)) + return true; + } + } + + if (mode != EmLongitudinalMode.ExactStopAtBoundary) + return false; + + double holdElapsed = 0d; + while (holdElapsed < holdDurationSeconds - ZeroTolerance) + { + holdElapsed = Math.Min(holdDurationSeconds, holdElapsed + outputTimeStepSeconds); + if (IncrementAndExceedsMaximum(ref requiredSampleCount, maximumSampleCount)) + return true; + } + return false; + } + private static void AddSample(TrajectorySample sample, ICollection samples, ref double previousPathS, LongitudinalCandidate candidate) { @@ -97,6 +141,12 @@ internal sealed class TrajectorySampleSchedule previousPathS = sample.PathS; } + private static bool IncrementAndExceedsMaximum(ref int sampleCount, int maximumSampleCount) + { + checked { sampleCount++; } + return sampleCount > maximumSampleCount; + } + private static TrajectorySample Interpolate(LongitudinalCandidate candidate, double sampleTime, ref int sourceInterval) { int lastKnot = candidate.KnotTimes.Count - 1; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs index 061a1a3..7e281e9 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs @@ -18,6 +18,7 @@ public enum EmTrajectoryValidationFailure TerminalSpeedNotZero, TerminalAccelerationNotZero, TerminalYawRateNotZero, + TerminalPoseMismatch, DirectionMismatch, DirectionSignMismatch, RedundantSpeedMismatch, @@ -79,6 +80,22 @@ public sealed class EmTrajectoryValidator public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle, EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, EmBoundaryType terminalBoundary) + { + return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, null, + terminalBoundary, false); + } + + public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle, + EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose, + EmBoundaryType terminalBoundary) + { + return ValidateCore(trajectory, map, vehicle, configuration, segmentIndex, pathUpperBoundS, terminalPose, + terminalBoundary, true); + } + + private EmTrajectoryValidationResult ValidateCore(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle, + EmPlannerConfiguration configuration, int segmentIndex, double pathUpperBoundS, Pose2D terminalPose, + EmBoundaryType terminalBoundary, bool requiresTerminalPose) { if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null || configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 || @@ -151,6 +168,31 @@ public sealed class EmTrajectoryValidator return Reject(EmTrajectoryValidationFailure.TerminalAccelerationNotZero, index, "Exact-stop tail stored acceleration must remain zero."); } + if (IsRealTerminalBoundary(terminalBoundary)) + { + if (requiresTerminalPose && terminalPose == null) + { + return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex, + "A real terminal anchor requires its expected world pose."); + } + if (terminalPose != null) + { + if (!IsFinite(terminalPose.X) || !IsFinite(terminalPose.Y) || !IsFinite(terminalPose.Heading)) + { + return Reject(EmTrajectoryValidationFailure.InvalidInput, terminalIndex, + "The expected terminal world pose is non-finite."); + } + double dx = terminal.X - terminalPose.X; + double dy = terminal.Y - terminalPose.Y; + double positionError = Math.Sqrt(dx * dx + dy * dy); + double yawError = Math.Abs(NormalizeAngle(terminal.Yaw - terminalPose.Heading)); + if (positionError > limits.TerminalPositionTolerance || yawError > limits.TerminalYawTolerance) + { + return Reject(EmTrajectoryValidationFailure.TerminalPoseMismatch, terminalIndex, + "TerminalPoseMismatch: position=" + Format(positionError) + ";yaw=" + Format(yawError)); + } + } + } } double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d; @@ -268,6 +310,18 @@ public sealed class EmTrajectoryValidator return -1; } + private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType) + { + return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach; + } + + private static double NormalizeAngle(double angle) + { + while (angle > Math.PI) angle -= 2d * Math.PI; + while (angle < -Math.PI) angle += 2d * Math.PI; + return angle; + } + private static int FindFirstTerminalPathIndex(EmTrajectory trajectory, double terminalPathS, double spatialTolerance) { for (int index = 0; index < trajectory.Points.Count; index++) @@ -315,6 +369,10 @@ public sealed class EmTrajectoryValidator configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond <= 0d || !IsFinite(configuration.Validation.SpatialToleranceMeters) || configuration.Validation.SpatialToleranceMeters < 0d || !IsFinite(configuration.Validation.KinematicTolerance) || configuration.Validation.KinematicTolerance < 0d || + !IsFinite(configuration.Validation.TerminalPositionToleranceMeters) || + configuration.Validation.TerminalPositionToleranceMeters < 0d || + !IsFinite(configuration.Validation.TerminalYawToleranceRadians) || + configuration.Validation.TerminalYawToleranceRadians < 0d || !IsFinite(configuration.Corridor.MaximumCollisionCheckStepMeters) || configuration.Corridor.MaximumCollisionCheckStepMeters <= 0d) { @@ -327,6 +385,8 @@ public sealed class EmTrajectoryValidator configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, configuration.Validation.SpatialToleranceMeters, configuration.Validation.KinematicTolerance, + configuration.Validation.TerminalPositionToleranceMeters, + configuration.Validation.TerminalYawToleranceRadians, configuration.Corridor.MaximumCollisionCheckStepMeters); return true; } @@ -356,7 +416,8 @@ public sealed class EmTrajectoryValidator { public ValidationLimits(double maximumSpeed, double maximumCurvature, double maximumAcceleration, double maximumDeceleration, double maximumJerk, double maximumCurvatureRate, double spatialTolerance, - double kinematicTolerance, double configuredCollisionStepMeters) + double kinematicTolerance, double terminalPositionTolerance, double terminalYawTolerance, + double configuredCollisionStepMeters) { MaximumSpeed = maximumSpeed; MaximumCurvature = maximumCurvature; @@ -366,6 +427,8 @@ public sealed class EmTrajectoryValidator MaximumCurvatureRate = maximumCurvatureRate; SpatialTolerance = spatialTolerance; KinematicTolerance = kinematicTolerance; + TerminalPositionTolerance = terminalPositionTolerance; + TerminalYawTolerance = terminalYawTolerance; ConfiguredCollisionStepMeters = configuredCollisionStepMeters; } @@ -377,6 +440,8 @@ public sealed class EmTrajectoryValidator public double MaximumCurvatureRate { get; } public double SpatialTolerance { get; } public double KinematicTolerance { get; } + public double TerminalPositionTolerance { get; } + public double TerminalYawTolerance { get; } public double ConfiguredCollisionStepMeters { get; } } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs index fd845ea..acfec3d 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs @@ -22,6 +22,7 @@ internal static class EmPlanningServiceChecks VerifiesNoProgressPublishesNoTrajectory(); VerifiesTimeoutFallbackAndCancellationSemantics(); VerifiesPublicationFailureAndDebugIsolation(); + VerifiesPublicationGateStatusMappings(); } private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic() @@ -427,6 +428,20 @@ internal static class EmPlanningServiceChecks VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation"); } + private static void VerifiesPublicationGateStatusMappings() + { + Verification.Equal(EmPlanningStatus.TerminalPoseMismatch, + EmPlanningService.MapPublicationFailure(EmTrajectoryValidationFailure.TerminalPoseMismatch), + "terminal-pose validator failure has its precise service status"); + + EmPlanningRequest sampleLimited = CreateRequest(TravelDirection.Forward, 0d, false, false); + sampleLimited.Configuration.Scheduling.MaximumPublishedSampleCount = 2; + EmPlanningResult sampleLimitResult = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan( + sampleLimited, CancellationToken.None); + VerifyFailure(sampleLimitResult, EmPlanningStatus.FullSegmentResourceLimitExceeded, + "sample-limit publication failure"); + } + private static void VerifiesPreviousTrajectoryIsALongitudinalSoftReference() { EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs index 2baf240..555de2f 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs @@ -17,6 +17,7 @@ internal static class TrajectoryChecks VerifiesRollingTrajectoryHasNoSyntheticStopTail(); VerifiesExactStopHoldHasNoJerkDiscontinuity(); VerifiesWorldSpacePublicationMutationsAreRejected(); + VerifiesTerminalPoseAndSampleLimitPublicationGates(); VerifiesReverseSpeedLimitUsesReverseConfiguration(); } @@ -184,6 +185,64 @@ internal static class TrajectoryChecks Verification.Equal(4, beyondSegment.PointIndex, "segment-boundary first point"); } + private static void VerifiesTerminalPoseAndSampleLimitPublicationGates() + { + ValidationContext context = CreateValidationContext(); + EmTrajectory valid = CreateValidationTrajectory(TravelDirection.Forward); + int terminalIndex = LongitudinalTerminalSchedule.GetStabilizationStartIndex( + CreateValidationCandidate().KnotTimes, context.Configuration.Scheduling.OutputTimeStepSeconds); + EmTrajectoryPoint terminal = valid.Points[terminalIndex]; + var validator = new EmTrajectoryValidator(); + + EmTrajectoryValidationResult insidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.029d, terminal.Y, terminal.Yaw), + EmBoundaryType.Goal); + Verification.True(insidePosition.IsValid, "2.9-centimetre terminal position error is accepted"); + EmTrajectoryValidationResult outsidePosition = validator.Validate(valid, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X + 0.031d, terminal.Y, terminal.Yaw), + EmBoundaryType.Goal); + Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsidePosition.Failure, + "3.1-centimetre terminal position error is rejected"); + + double degrees = Math.PI / 180d; + EmTrajectory wrappedYaw = Replace(valid, terminalIndex, Clone(terminal, yaw: 179d * degrees)); + EmTrajectoryValidationResult wrapped = validator.Validate(wrappedYaw, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -179d * degrees), + EmBoundaryType.Goal); + Verification.True(wrapped.IsValid, "179 and -179 degrees use normalized yaw error"); + EmTrajectoryValidationResult insideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -4.9d * degrees), + EmBoundaryType.Goal); + Verification.True(insideYaw.IsValid, "4.9-degree terminal yaw error is accepted"); + EmTrajectoryValidationResult outsideYaw = validator.Validate(valid, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.0055d, new Pose2D(terminal.X, terminal.Y, -5.1d * degrees), + EmBoundaryType.Goal); + Verification.Equal(EmTrajectoryValidationFailure.TerminalPoseMismatch, outsideYaw.Failure, + "5.1-degree terminal yaw error is rejected"); + + EmTrajectory rolling = new EmTrajectoryAssembler().Assemble( + CreatePath(TravelDirection.Forward, 0d, 0d), CreateRollingLongitudinalResult(), + CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop, + EmLongitudinalMode.RollingContinuation)); + EmTrajectoryValidationResult rollingResult = validator.Validate(rolling, context.EmptyMap, context.Vehicle, + context.Configuration, 2, 0.08d, new Pose2D(99d, 99d, Math.PI), EmBoundaryType.RollingSafetyStop); + Verification.True(rollingResult.IsValid, "rolling-safety window end does not use the terminal-pose gate"); + + EmPlannerConfiguration limitedConfiguration = EmPlannerConfiguration.CreateDefault(); + limitedConfiguration.Scheduling.MaximumPublishedSampleCount = 8; + EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(limitedConfiguration).TryAssemble( + CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(), + CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal), out EmTrajectory limitedTrajectory, + out string assemblyFailure); + Verification.Equal(EmPlanningStatus.FullSegmentResourceLimitExceeded, assemblyStatus, + "sample schedule one above the limit is rejected"); + Verification.True(limitedTrajectory == null, "sample-limit rejection does not assemble a partial trajectory"); + Verification.True(assemblyFailure.IndexOf("MaximumPublishedSampleCount", StringComparison.Ordinal) >= 0, + "sample-limit rejection preserves a diagnostic"); + Verification.True(assemblyFailure.IndexOf("required-at-least=9", StringComparison.Ordinal) >= 0, + "sample-limit fixture exceeds the cap by exactly one sample"); + } + private static void VerifiesReverseSpeedLimitUsesReverseConfiguration() { ValidationContext context = CreateValidationContext(); @@ -284,11 +343,11 @@ internal static class TrajectoryChecks return Replace(trajectory, index, replacement); } - private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? timeFromStart = null, - double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null, + private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? y = null, double? yaw = null, + double? timeFromStart = null, double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = null, EmBoundaryType? boundaryType = null) { - return new EmTrajectoryPoint(x ?? point.X, point.Y, point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity, + return new EmTrajectoryPoint(x ?? point.X, y ?? point.Y, yaw ?? point.Yaw, signedSpeed ?? point.SignedLongitudinalVelocity, timeFromStart ?? point.TimeFromStart, vehicleCurvature ?? point.VehicleCurvature, point.SegmentIndex, pathS ?? point.SegmentLocalS, pathS ?? point.PathS, point.Direction, boundaryType ?? point.BoundaryType, 0d, 0d);