From 06d9037645de78ebd0274205352bd4fb5d6b4063 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Mon, 10 Aug 2026 14:24:15 +0800 Subject: [PATCH] fix: normalize EM PathS resampling residuals --- .../Trajectory/EmTrajectoryAssembler.cs | 10 +++-- .../Trajectory/TrajectorySampleSchedule.cs | 44 +++++++++++++++---- 2 files changed, 43 insertions(+), 11 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs index ad63ba0..2adfd0d 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 double pathSTolerance; private readonly int maximumPublishedSampleCount; public EmTrajectoryAssembler() @@ -18,13 +19,16 @@ public sealed class EmTrajectoryAssembler public EmTrajectoryAssembler(EmPlannerConfiguration configuration) { - if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null) + if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null || + configuration.Validation == null) throw new ArgumentNullException(nameof(configuration)); outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds; zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds; + pathSTolerance = configuration.Validation.KinematicTolerance; maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount; if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) || - zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2) + zeroSpeedHoldSeconds < 0d || !IsFinite(pathSTolerance) || pathSTolerance < 0d || + maximumPublishedSampleCount < 2) { throw new ArgumentOutOfRangeException(nameof(configuration)); } @@ -64,7 +68,7 @@ public sealed class EmTrajectoryAssembler return EmPlanningStatus.FullSegmentResourceLimitExceeded; } var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds, - metadata.LongitudinalMode, isFullDirectionSegment); + metadata.LongitudinalMode, isFullDirectionSegment, pathSTolerance); double terminalPathS = path.Points[path.Points.Count - 1].PathS; var points = new List(schedule.Samples.Count); double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d; diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs index 6b9c654..7103d82 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; +using System.Globalization; namespace MultiWheelC.TrajectoryPlanning.EMPlanner; @@ -9,7 +10,7 @@ internal sealed class TrajectorySampleSchedule private const double ZeroTolerance = 1e-12d; public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds, - EmLongitudinalMode mode, bool resampleMotion) + EmLongitudinalMode mode, bool resampleMotion, double pathSTolerance) { if (candidate == null) throw new ArgumentNullException(nameof(candidate)); @@ -17,11 +18,14 @@ internal sealed class TrajectorySampleSchedule throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds)); if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d) throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds)); + if (!IsFinite(pathSTolerance) || pathSTolerance < 0d) + throw new ArgumentOutOfRangeException(nameof(pathSTolerance)); if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode)) throw new ArgumentOutOfRangeException(nameof(mode)); var samples = new List(candidate.KnotTimes.Count + 4); double previousPathS = double.NegativeInfinity; + double terminalPathS = candidate.S[candidate.S.Count - 1]; if (resampleMotion) { double finalTime = candidate.KnotTimes[candidate.KnotTimes.Count - 1]; @@ -29,9 +33,11 @@ internal sealed class TrajectorySampleSchedule for (double sampleTime = 0d; sampleTime < finalTime - ZeroTolerance; sampleTime += outputTimeStepSeconds) { - AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples, ref previousPathS, candidate); + AddSample(Interpolate(candidate, sampleTime, ref sourceInterval), samples.Count, samples, + ref previousPathS, terminalPathS, pathSTolerance, candidate); } - AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples, ref previousPathS, candidate); + AddSample(Interpolate(candidate, finalTime, ref sourceInterval), samples.Count, samples, + ref previousPathS, terminalPathS, pathSTolerance, candidate); } else { @@ -39,7 +45,7 @@ internal sealed class TrajectorySampleSchedule { AddSample(new TrajectorySample(candidate.KnotTimes[index], candidate.S[index], Math.Max(0d, candidate.U[index]), candidate.A[index], index < candidate.J.Count ? candidate.J[index] : 0d, - false), samples, ref previousPathS, candidate); + false), samples.Count, samples, ref previousPathS, terminalPathS, pathSTolerance, candidate); } } @@ -130,17 +136,39 @@ internal sealed class TrajectorySampleSchedule return false; } - private static void AddSample(TrajectorySample sample, ICollection samples, - ref double previousPathS, LongitudinalCandidate candidate) + private static void AddSample(TrajectorySample sample, int sampleIndex, ICollection samples, + ref double previousPathS, double terminalPathS, double pathSTolerance, LongitudinalCandidate candidate) { - if (sample.PathS < previousPathS) - throw new ArgumentException("Trajectory PathS cannot decrease.", nameof(candidate)); + double lowerDifference = previousPathS - sample.PathS; + double upperDifference = sample.PathS - terminalPathS; + if (lowerDifference > pathSTolerance || upperDifference > pathSTolerance) + { + throw PathSFailure(sampleIndex, previousPathS, sample.PathS, + Math.Max(lowerDifference, upperDifference), candidate); + } + + double normalizedPathS = Math.Min(terminalPathS, Math.Max(previousPathS, sample.PathS)); + if (normalizedPathS != sample.PathS) + { + sample = new TrajectorySample(sample.TimeFromStart, normalizedPathS, sample.ProgressSpeed, + sample.Acceleration, sample.Jerk, sample.IsHoldSample); + } if (sample.ProgressSpeed < -ZeroTolerance) throw new ArgumentException("Longitudinal progress speed cannot be negative.", nameof(candidate)); samples.Add(sample); previousPathS = sample.PathS; } + private static ArgumentException PathSFailure(int sampleIndex, double previousPathS, + double candidatePathS, double difference, LongitudinalCandidate candidate) + { + return new ArgumentException("Trajectory PathS violates monotonic publication bounds: sampleIndex=" + + sampleIndex.ToString(CultureInfo.InvariantCulture) + ";previousPathS=" + + previousPathS.ToString("R", CultureInfo.InvariantCulture) + ";candidatePathS=" + + candidatePathS.ToString("R", CultureInfo.InvariantCulture) + ";difference=" + + difference.ToString("R", CultureInfo.InvariantCulture) + ".", nameof(candidate)); + } + private static bool IncrementAndExceedsMaximum(ref int sampleCount, int maximumSampleCount) { checked { sampleCount++; }