feat: validate EM terminal world pose

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));

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,
+        double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds;
-            isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds, metadata.LongitudinalMode, isFullDirectionSegment);
+        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<EmTrajectoryPoint>(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)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs

@@ -86,6 +86,50 @@ internal sealed class TrajectorySampleSchedule
     public IReadOnlyList<TrajectorySample> 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<TrajectorySample> 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;

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; }
     }
 }

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);

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,
+    private static EmTrajectoryPoint Clone(EmTrajectoryPoint point, double? x = null, double? y = null, double? yaw = null,
-        double? signedSpeed = null, double? pathS = null, double? vehicleCurvature = 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);