diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonEntry.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonEntry.cs new file mode 100644 index 0000000..52e5dc9 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonEntry.cs @@ -0,0 +1,120 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 原始基线或一种平滑方法的不可变比较条目。 +public sealed class PathSmoothingComparisonEntry +{ + /// 为测试、离线分析和排序创建不携带路径几何的候选条目。 + public PathSmoothingComparisonEntry( + SmoothingMethod method, + PathSmoothingStatus status, + PathQualityMetrics metrics, + SmoothingTimingSummary timing, + string stableGeometryDigest, + string diagnostic) + : this(method, false, status, metrics, timing, stableGeometryDigest, diagnostic, Empty(), Empty()) + { + } + + private PathSmoothingComparisonEntry( + SmoothingMethod? method, + bool isRawPathBaseline, + PathSmoothingStatus status, + PathQualityMetrics metrics, + SmoothingTimingSummary timing, + string stableGeometryDigest, + string diagnostic, + IReadOnlyList path, + IReadOnlyList segments) + { + Method = method; + IsRawPathBaseline = isRawPathBaseline; + Status = status; + Metrics = metrics ?? new PathQualityMetrics(); + Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "未提供计时结果。"); + StableGeometryDigest = stableGeometryDigest ?? string.Empty; + Diagnostic = diagnostic ?? string.Empty; + Path = CopyReadOnly(path); + Segments = CopyReadOnly(segments); + } + + /// 候选所代表的方法；原始粗路径基线为空。 + public SmoothingMethod? Method { get; } + + /// 是否为单独分析的原始粗路径基线。 + public bool IsRawPathBaseline { get; } + + /// 本条目的最终状态。 + public PathSmoothingStatus Status { get; } + + /// 使用原始基线规范化后的质量指标。 + public PathQualityMetrics Metrics { get; } + + /// 方法的五次测量计时；基线不参与计时排名。 + public SmoothingTimingSummary Timing { get; } + + /// 由状态、分段元数据和完整路径 IEEE 754 位模式生成的 SHA-256 摘要。 + public string StableGeometryDigest { get; } + + /// 面向报告和诊断的稳定说明。 + public string Diagnostic { get; } + + /// 仅供比较与报告读取的正式路径；失败时为空。 + public IReadOnlyList Path { get; } + + /// 覆盖 的方向段；失败时为空。 + public IReadOnlyList Segments { get; } + + /// 条目能否参与方法推荐。 + public bool IsEligibleForRecommendation => + !IsRawPathBaseline && + Status == PathSmoothingStatus.Success && + Metrics.IsFeasible && + Timing.IsDeterministic; + + internal static PathSmoothingComparisonEntry CreateCandidate( + SmoothingMethod method, + PathSmoothingStatus status, + PathQualityMetrics metrics, + SmoothingTimingSummary timing, + string stableGeometryDigest, + string diagnostic, + IReadOnlyList path, + IReadOnlyList segments) + { + return new PathSmoothingComparisonEntry( + method, false, status, metrics, timing, stableGeometryDigest, diagnostic, path, segments); + } + + internal static PathSmoothingComparisonEntry CreateRawPathBaseline( + PathSmoothingStatus status, + PathQualityMetrics metrics, + string stableGeometryDigest, + string diagnostic, + IReadOnlyList path, + IReadOnlyList segments) + { + return new PathSmoothingComparisonEntry( + null, true, status, metrics, + new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, true, string.Empty), + stableGeometryDigest, diagnostic, path, segments); + } + + private static IReadOnlyList Empty() + { + return new ReadOnlyCollection(new List()); + } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonRequest.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonRequest.cs new file mode 100644 index 0000000..b7ee33e --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonRequest.cs @@ -0,0 +1,60 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 同一粗路径的离线平滑比较请求。 +public sealed class PathSmoothingComparisonRequest +{ + private static readonly SmoothingMethod[] DefaultMethods = + { + SmoothingMethod.CubicBSpline, + SmoothingMethod.LocalCubicBezier, + SmoothingMethod.PiecewiseQuintic, + }; + + /// 创建比较请求，并固定原始输入与方法顺序。 + public PathSmoothingComparisonRequest( + PathSmoothingRequest smoothingRequest, + IReadOnlyList methods = null) + { + SmoothingRequest = CopyRequest(smoothingRequest); + Methods = CopyMethods(methods ?? DefaultMethods); + } + + /// 所有方法共享的不可变粗路径、地图、车辆和配置快照。 + public PathSmoothingRequest SmoothingRequest { get; } + + /// 按调用方指定稳定顺序运行的方法集合。 + public IReadOnlyList Methods { get; } + + private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source) + { + if (source == null) return null; + return new PathSmoothingRequest( + source.CoarsePath, + source.Segments, + source.Map, + source.Vehicle, + source.Configuration); + } + + private static IReadOnlyList CopyMethods(IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) + { + SmoothingMethod method = source[index]; + if (!Enum.IsDefined(typeof(SmoothingMethod), method)) + throw new ArgumentOutOfRangeException(nameof(source), "比较方法无效。"); + if (copy.Contains(method)) + throw new ArgumentException("比较方法不能重复。", nameof(source)); + copy.Add(method); + } + } + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonResult.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonResult.cs new file mode 100644 index 0000000..94ed422 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonResult.cs @@ -0,0 +1,49 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 一次离线比较的不可变基线、方法条目和推荐结论。 +public sealed class PathSmoothingComparisonResult +{ + internal PathSmoothingComparisonResult( + PathSmoothingComparisonEntry rawPathBaseline, + IReadOnlyList entries, + SmoothingMethod? recommendedMethod, + bool isCancelled, + string diagnostic) + { + RawPathBaseline = rawPathBaseline ?? throw new ArgumentNullException(nameof(rawPathBaseline)); + Entries = CopyReadOnly(entries); + RecommendedMethod = recommendedMethod; + IsCancelled = isCancelled; + Diagnostic = diagnostic ?? string.Empty; + } + + /// 独立分析的原始粗路径；不属于任何候选方法。 + public PathSmoothingComparisonEntry RawPathBaseline { get; } + + /// 每个请求方法恰有一个条目；取消时可能只包含已完成的方法。 + public IReadOnlyList Entries { get; } + + /// 按公开字典序选择的方法；没有合格方法或取消时为空。 + public SmoothingMethod? RecommendedMethod { get; } + + /// 比较是否在启动后续方法前被取消。 + public bool IsCancelled { get; } + + /// 整个比较的稳定状态说明。 + public string Diagnostic { get; } + + private static IReadOnlyList CopyReadOnly( + IReadOnlyList source) + { + var copy = new List(source == null ? 0 : source.Count); + if (source != null) + { + for (int index = 0; index < source.Count; index++) copy.Add(source[index]); + } + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingMethodRanker.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingMethodRanker.cs new file mode 100644 index 0000000..9844037 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingMethodRanker.cs @@ -0,0 +1,60 @@ +using System; +using System.Collections.Generic; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 按公开字典序选择唯一的推荐平滑方法。 +public static class SmoothingMethodRanker +{ + /// 从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。 + public static SmoothingMethod? Rank(IReadOnlyList entries) + { + PathSmoothingComparisonEntry best = null; + if (entries == null) return null; + + for (int index = 0; index < entries.Count; index++) + { + PathSmoothingComparisonEntry candidate = entries[index]; + if (candidate == null || !candidate.IsEligibleForRecommendation) continue; + if (best == null || Compare(candidate, best) < 0) best = candidate; + } + return best == null ? (SmoothingMethod?)null : best.Method; + } + + private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right) + { + int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy); + if (comparison != 0) return comparison; + + comparison = CompareAscending( + left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter, + right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter); + if (comparison != 0) return comparison; + + comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters); + if (comparison != 0) return comparison; + + comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent); + if (comparison != 0) return comparison; + + comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds); + if (comparison != 0) return comparison; + + return ((int)left.Method.Value).CompareTo((int)right.Method.Value); + } + + private static int CompareAscending(double left, double right) + { + return Normalize(left).CompareTo(Normalize(right)); + } + + private static int CompareDescending(double left, double right) + { + return Normalize(right).CompareTo(Normalize(left)); + } + + private static double Normalize(double value) + { + return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingTimingSummary.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingTimingSummary.cs new file mode 100644 index 0000000..ae4a635 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingTimingSummary.cs @@ -0,0 +1,90 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 一个方法的固定五次计时样本和确定性结论。 +public sealed class SmoothingTimingSummary +{ + /// 创建一个只接受五个已测量样本的计时汇总。 + public SmoothingTimingSummary( + IReadOnlyList measuredElapsedMilliseconds, + bool isDeterministic, + string diagnostic) + { + if (measuredElapsedMilliseconds == null || measuredElapsedMilliseconds.Count != 5) + throw new ArgumentException("计时汇总必须包含五个已测量样本。", nameof(measuredElapsedMilliseconds)); + + var copy = new List(measuredElapsedMilliseconds.Count); + for (int index = 0; index < measuredElapsedMilliseconds.Count; index++) + { + double value = measuredElapsedMilliseconds[index]; + if (double.IsNaN(value) || double.IsInfinity(value) || value < 0d) + throw new ArgumentOutOfRangeException(nameof(measuredElapsedMilliseconds), "计时样本必须为有限非负数。"); + copy.Add(value); + } + + MeasuredElapsedMilliseconds = new ReadOnlyCollection(copy); + MedianElapsedMilliseconds = Median(copy); + IsDeterministic = isDeterministic; + Diagnostic = diagnostic ?? string.Empty; + } + + /// 不含预热执行的五个实测耗时，单位 ms。 + public IReadOnlyList MeasuredElapsedMilliseconds { get; } + + /// 五个实测耗时的稳定中位数，单位 ms。 + public double MedianElapsedMilliseconds { get; } + + /// 五次输出是否具有相同的状态和稳定几何摘要。 + public bool IsDeterministic { get; } + + /// 非确定性或测量失败的稳定诊断说明。 + public string Diagnostic { get; } + + /// 从五次测量结果中生成计时汇总，并拒绝状态或几何不稳定的输出。 + public static SmoothingTimingSummary FromMeasurements( + IReadOnlyList measuredElapsedMilliseconds, + IReadOnlyList measuredResults) + { + if (measuredResults == null || measuredResults.Count != 5) + throw new ArgumentException("确定性检查必须包含五个测量结果。", nameof(measuredResults)); + for (int index = 0; index < measuredResults.Count; index++) + { + if (measuredResults[index] == null) + throw new ArgumentException("确定性检查不能包含空测量结果。", nameof(measuredResults)); + } + + PathSmoothingResult canonical = measuredResults[0]; + string canonicalDigest = StableGeometryDigest.Compute(canonical); + for (int index = 1; index < measuredResults.Count; index++) + { + PathSmoothingResult measured = measuredResults[index]; + string digest = StableGeometryDigest.Compute(measured); + if (measured.Status != canonical.Status || + measured.Path.Count != canonical.Path.Count || + measured.Segments.Count != canonical.Segments.Count || + !string.Equals(digest, canonicalDigest, StringComparison.Ordinal)) + { + return new SmoothingTimingSummary( + measuredElapsedMilliseconds, + false, + "五次测量的状态、点数、分段数或稳定几何摘要不一致。"); + } + } + + return new SmoothingTimingSummary(measuredElapsedMilliseconds, true, string.Empty); + } + + internal static double Median(IReadOnlyList values) + { + var sorted = new double[values.Count]; + for (int index = 0; index < values.Count; index++) sorted[index] = values[index]; + Array.Sort(sorted); + int middle = sorted.Length / 2; + return sorted.Length % 2 == 1 + ? sorted[middle] + : (sorted[middle - 1] + sorted[middle]) / 2d; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/StableGeometryDigest.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/StableGeometryDigest.cs new file mode 100644 index 0000000..ee9d105 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/StableGeometryDigest.cs @@ -0,0 +1,111 @@ +using System; +using System.Collections.Generic; +using System.IO; +using System.Security.Cryptography; +using System.Text; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; + +/// 为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。 +public static class StableGeometryDigest +{ + /// 计算正式平滑结果的状态、方法、分段和路径位模式摘要。 + public static string Compute(PathSmoothingResult result) + { + if (result == null) throw new ArgumentNullException(nameof(result)); + return Compute(result.Status, result.Method, result.Path, result.Segments); + } + + /// 计算任意已分析路径的稳定摘要。 + public static string Compute( + PathSmoothingStatus status, + SmoothingMethod? method, + IReadOnlyList path, + IReadOnlyList segments) + { + using (var stream = new MemoryStream()) + { + WriteInt32(stream, 1); + WriteInt32(stream, (int)status); + WriteBoolean(stream, method.HasValue); + if (method.HasValue) WriteInt32(stream, (int)method.Value); + + WriteInt32(stream, path == null ? 0 : path.Count); + if (path != null) + { + for (int index = 0; index < path.Count; index++) WritePoint(stream, path[index]); + } + + WriteInt32(stream, segments == null ? 0 : segments.Count); + if (segments != null) + { + for (int index = 0; index < segments.Count; index++) WriteSegment(stream, segments[index]); + } + + using (SHA256 sha256 = SHA256.Create()) + { + byte[] hash = sha256.ComputeHash(stream.ToArray()); + var builder = new StringBuilder(hash.Length * 2); + for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2")); + return builder.ToString(); + } + } + } + + private static void WritePoint(Stream stream, SmoothedPathPoint point) + { + if (point == null) throw new ArgumentException("稳定路径摘要不能包含空点。", nameof(point)); + WriteDouble(stream, point.X); + WriteDouble(stream, point.Y); + WriteDouble(stream, point.Heading); + WriteDouble(stream, point.UnwrappedHeading); + WriteDouble(stream, point.ArcLength); + WriteInt32(stream, (int)point.Direction); + WriteDouble(stream, point.GeometricCurvature); + WriteDouble(stream, point.VehicleCurvature); + WriteDouble(stream, point.BodyClearance); + WriteBoolean(stream, point.IsGearSwitchPoint); + WriteInt32(stream, (int)point.Source); + } + + private static void WriteSegment(Stream stream, SmoothedPathSegment segment) + { + if (segment == null) throw new ArgumentException("稳定路径摘要不能包含空方向段。", nameof(segment)); + WriteInt32(stream, segment.SegmentIndex); + WriteInt32(stream, (int)segment.Direction); + WriteInt32(stream, segment.StartIndex); + WriteInt32(stream, segment.EndIndex); + WriteBoolean(stream, segment.StartsAtGearSwitch); + WriteBoolean(stream, segment.EndsAtGearSwitch); + } + + private static void WriteDouble(Stream stream, double value) + { + WriteInt64(stream, BitConverter.DoubleToInt64Bits(value)); + } + + private static void WriteBoolean(Stream stream, bool value) + { + stream.WriteByte(value ? (byte)1 : (byte)0); + } + + private static void WriteInt32(Stream stream, int value) + { + unchecked + { + stream.WriteByte((byte)value); + stream.WriteByte((byte)(value >> 8)); + stream.WriteByte((byte)(value >> 16)); + stream.WriteByte((byte)(value >> 24)); + } + } + + private static void WriteInt64(Stream stream, long value) + { + unchecked + { + ulong bits = (ulong)value; + for (int index = 0; index < 8; index++) stream.WriteByte((byte)(bits >> (index * 8))); + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs new file mode 100644 index 0000000..3c42601 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs @@ -0,0 +1,248 @@ +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Threading; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; +using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade; + +/// 以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。 +public sealed class PathSmoothingComparisonService +{ + private readonly PathSmoothingService _smoothingService = new PathSmoothingService(); + private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor(); + private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer(); + private readonly SmoothedPathValidator _validator = new SmoothedPathValidator(); + + /// 比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。 + public PathSmoothingComparisonResult Compare( + PathSmoothingComparisonRequest request, + CancellationToken cancellationToken = default) + { + PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason); + var entries = new List(); + + if (cancellationToken.IsCancellationRequested) + return Cancelled(baseline, entries); + + if (request == null || request.SmoothingRequest == null) + return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason); + + for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++) + { + if (cancellationToken.IsCancellationRequested) + return Cancelled(baseline, entries); + + SmoothingMethod method = request.Methods[methodIndex]; + if (!TryCompareMethod( + request.SmoothingRequest, + method, + baseline.Metrics, + cancellationToken, + out PathSmoothingComparisonEntry entry)) + return Cancelled(baseline, entries); + entries.Add(entry); + } + + return new PathSmoothingComparisonResult( + baseline, + entries, + SmoothingMethodRanker.Rank(entries), + false, + baselineReason); + } + + private bool TryCompareMethod( + PathSmoothingRequest sourceRequest, + SmoothingMethod method, + PathQualityMetrics rawMetrics, + CancellationToken cancellationToken, + out PathSmoothingComparisonEntry entry) + { + entry = null; + try + { + PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method); + cancellationToken.ThrowIfCancellationRequested(); + PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken); + if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false; + + var timings = new List(5); + var measuredResults = new List(5); + for (int sampleIndex = 0; sampleIndex < 5; sampleIndex++) + { + cancellationToken.ThrowIfCancellationRequested(); + Stopwatch stopwatch = Stopwatch.StartNew(); + PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken); + stopwatch.Stop(); + if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false; + timings.Add(stopwatch.Elapsed.TotalMilliseconds); + measuredResults.Add(result); + } + + PathSmoothingResult canonical = measuredResults[0]; + string digest = StableGeometryDigest.Compute(canonical); + SmoothingTimingSummary timing = SmoothingTimingSummary.FromMeasurements(timings, measuredResults); + string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic) + ? canonical.Diagnostics.TerminationReason + : timing.Diagnostic; + + PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success + ? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics) + : new PathQualityMetrics(); + entry = PathSmoothingComparisonEntry.CreateCandidate( + method, + canonical.Status, + metrics, + timing, + digest, + diagnostic, + canonical.Path, + canonical.Segments); + return true; + } + catch (OperationCanceledException) + { + return false; + } + catch (Exception exception) + { + entry = PathSmoothingComparisonEntry.CreateCandidate( + method, + PathSmoothingStatus.Failed, + new PathQualityMetrics(), + new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name), + string.Empty, + exception.Message, + null, + null); + return true; + } + } + + private PathSmoothingComparisonEntry CreateRawPathBaseline( + PathSmoothingComparisonRequest request, + out string reason) + { + reason = string.Empty; + if (request == null || request.SmoothingRequest == null) + return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason); + + PathSmoothingRequest smoothingRequest = request.SmoothingRequest; + PathSmoothingConfiguration configuration = smoothingRequest.Configuration; + if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null) + return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason); + + if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason)) + return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); + if (!_analyzer.TryAnalyze(preparedPath.Segments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analysis, out reason)) + return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); + if (!_validator.TryValidate( + analysis.Path, + analysis.Segments, + preparedPath, + smoothingRequest.Map, + smoothingRequest.Vehicle, + configuration.MaximumCollisionCheckStepMeters, + out IReadOnlyList safePath, + out double minimumClearanceMeters, + out reason)) + { + return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason); + } + + PathQualityMetrics metrics = CreateMetrics(analysis, minimumClearanceMeters); + string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, safePath, analysis.Segments); + return PathSmoothingComparisonEntry.CreateRawPathBaseline( + PathSmoothingStatus.Success, + metrics, + digest, + string.Empty, + safePath, + analysis.Segments); + } + + private static PathSmoothingComparisonEntry FailedBaseline( + PathSmoothingStatus status, + string failureReason, + out string reason) + { + reason = failureReason ?? string.Empty; + return PathSmoothingComparisonEntry.CreateRawPathBaseline( + status, + new PathQualityMetrics(), + StableGeometryDigest.Compute(status, null, null, null), + reason, + null, + null); + } + + private static PathSmoothingComparisonResult Cancelled( + PathSmoothingComparisonEntry baseline, + IReadOnlyList entries) + { + return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。"); + } + + private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method) + { + PathSmoothingConfiguration configuration = source.Configuration; + configuration.Method = method; + configuration.AllowFallbackToCoarsePath = false; + return new PathSmoothingRequest( + source.CoarsePath, + source.Segments, + source.Map, + source.Vehicle, + configuration); + } + + private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters) + { + return new PathQualityMetrics( + true, + analysis.PathLengthMeters, + analysis.MaximumAbsoluteVehicleCurvaturePerMeter, + analysis.RootMeanSquareVehicleCurvaturePerMeter, + analysis.TotalAbsoluteCurvatureVariationPerMeter, + analysis.CurvatureVariationEnergy, + minimumClearanceMeters, + 0d, + 0d, + 0d, + 0d); + } + + private static PathQualityMetrics NormalizeMetrics( + PathQualityMetrics candidate, + PathQualityMetrics raw) + { + if (candidate == null || raw == null || !candidate.IsFeasible) + return new PathQualityMetrics(); + + return new PathQualityMetrics( + true, + candidate.PathLengthMeters, + candidate.MaximumAbsoluteVehicleCurvaturePerMeter, + candidate.RootMeanSquareVehicleCurvaturePerMeter, + candidate.TotalAbsoluteCurvatureVariationPerMeter, + candidate.CurvatureVariationEnergy, + candidate.MinimumBodyClearanceMeters, + RelativePercentOrAbsoluteDelta(candidate.PathLengthMeters, raw.PathLengthMeters), + RelativePercentOrAbsoluteDelta( + candidate.MaximumAbsoluteVehicleCurvaturePerMeter, + raw.MaximumAbsoluteVehicleCurvaturePerMeter), + RelativePercentOrAbsoluteDelta( + candidate.TotalAbsoluteCurvatureVariationPerMeter, + raw.TotalAbsoluteCurvatureVariationPerMeter), + candidate.MinimumBodyClearanceMeters - raw.MinimumBodyClearanceMeters); + } + + private static double RelativePercentOrAbsoluteDelta(double candidate, double raw) + { + double delta = candidate - raw; + return Math.Abs(raw) < 1e-12d ? delta : delta / raw * 100d; + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_comparison.ps1 b/ClumsyPilot/tests/verify_path_smoothing_comparison.ps1 new file mode 100644 index 0000000..a331e62 --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_comparison.ps1 @@ -0,0 +1,230 @@ +param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll')) + +$ErrorActionPreference = 'Stop' +$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath)) + +function Assert-True($Actual, [string]$Message) { + if (-not $Actual) { throw $Message } +} + +function Assert-Equal($Expected, $Actual, [string]$Message) { + if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } +} + +function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) { + if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) { + throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance" + } +} + +function Get-RequiredType([string]$Name) { + return $assembly.GetType($Name, $true) +} + +function New-Map { + $mapRequest = [Activator]::CreateInstance($mapRequestType) + $mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000)) + $mapRequest.ResolutionMm = [single]50 + $mapRequest.AllowExplicitEmptyMap = $true + $map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map + Assert-True ($null -ne $map) 'Comparison test must create a planning map.' + return $map +} + +function New-Vehicle { + $vehicle = [Activator]::CreateInstance($vehicleType) + $vehicle.LengthMeters = [double]0.20 + $vehicle.WidthMeters = [double]0.20 + $vehicle.SafetyMarginMeters = [double]0.0 + $vehicle.MaximumCurvaturePerMeter = [double]100.0 + return $vehicle +} + +function New-CoarsePoint([double]$X, [double]$Y, [double]$ArcLength) { + return [Activator]::CreateInstance($coarsePointType, @( + $X, $Y, [double]0.0, [double]0.0, $ArcLength, $forward, + [double]0.0, [double]1.0, $false, $coarseAnchor)) +} + +function New-SmoothingRequest { + $points = [Array]::CreateInstance($coarsePointType, 2) + $points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0) + $points.SetValue((New-CoarsePoint 1.5 0.5 1.0), 1) + $segments = [Array]::CreateInstance($coarseSegmentType, 1) + $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $false)), 0) + $configuration = [Activator]::CreateInstance($configurationType) + return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration)) +} + +function New-CornerSmoothingRequest { + $points = [Array]::CreateInstance($coarsePointType, 4) + $points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0) + $points.SetValue((New-CoarsePoint 1.0 0.5 0.5), 1) + $points.SetValue((New-CoarsePoint 1.0 1.0 1.0), 2) + $points.SetValue((New-CoarsePoint 1.5 1.0 1.5), 3) + $segments = [Array]::CreateInstance($coarseSegmentType, 1) + $segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 3, $false, $false)), 0) + $configuration = [Activator]::CreateInstance($configurationType) + return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration)) +} + +function New-Metrics( + [double]$VariationEnergy, + [double]$PeakCurvature, + [double]$MinimumClearance, + [double]$LengthChangePercent) { + return [Activator]::CreateInstance($metricsType, @( + $true, [double]1.0, $PeakCurvature, [double]0.0, [double]0.0, $VariationEnergy, + $MinimumClearance, $LengthChangePercent, [double]0.0, [double]0.0, [double]0.0)) +} + +function New-Timing([double]$MedianMilliseconds) { + [double[]]$samples = @($MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds) + return [Activator]::CreateInstance($timingType, @($samples, $true, '')) +} + +function New-Entry( + $Method, + [double]$VariationEnergy, + [double]$PeakCurvature, + [double]$MinimumClearance, + [double]$LengthChangePercent, + [double]$MedianMilliseconds) { + [object[]]$arguments = @( + $Method, $successStatus, (New-Metrics $VariationEnergy $PeakCurvature $MinimumClearance $LengthChangePercent), + (New-Timing $MedianMilliseconds), ('synthetic-' + $Method.ToString()), '') + return [Activator]::CreateInstance($entryType, $arguments) +} + +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$comparison = $root + 'Comparison.' +$facade = $root + 'Facade.' +$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.' +$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.' + +$comparisonServiceType = Get-RequiredType ($facade + 'PathSmoothingComparisonService') +$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest') +$comparisonResultType = Get-RequiredType ($comparison + 'PathSmoothingComparisonResult') +$entryType = Get-RequiredType ($comparison + 'PathSmoothingComparisonEntry') +$timingType = Get-RequiredType ($comparison + 'SmoothingTimingSummary') +$rankerType = Get-RequiredType ($comparison + 'SmoothingMethodRanker') +$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest') +$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration') +$metricsType = Get-RequiredType ($root + 'PathQualityMetrics') +$methodType = Get-RequiredType ($root + 'SmoothingMethod') +$statusType = Get-RequiredType ($root + 'PathSmoothingStatus') +$smoothingResultType = Get-RequiredType ($root + 'PathSmoothingResult') +$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics') +$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint') +$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment') +$directionType = Get-RequiredType ($coarsePath + 'TravelDirection') +$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource') +$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters') +$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm') +$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest') +$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory') + +$forward = [Enum]::Parse($directionType, 'Forward') +$coarseAnchor = [Enum]::Parse($coarsePointSourceType, 'Start') +$cubicBSpline = [Enum]::Parse($methodType, 'CubicBSpline') +$localCubicBezier = [Enum]::Parse($methodType, 'LocalCubicBezier') +$piecewiseQuintic = [Enum]::Parse($methodType, 'PiecewiseQuintic') +$successStatus = [Enum]::Parse($statusType, 'Success') + +Assert-True $comparisonServiceType.IsPublic 'Comparison service must be public.' +$compareMethod = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken])) +Assert-True ($null -ne $compareMethod) 'Comparison service must expose Compare(PathSmoothingComparisonRequest, CancellationToken).' +Assert-Equal $comparisonResultType $compareMethod.ReturnType 'Compare must return PathSmoothingComparisonResult.' + +$methods = [Array]::CreateInstance($methodType, 3) +$methods.SetValue($cubicBSpline, 0) +$methods.SetValue($localCubicBezier, 1) +$methods.SetValue($piecewiseQuintic, 2) +$comparisonRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-SmoothingRequest), $methods)) +$comparisonService = [Activator]::CreateInstance($comparisonServiceType) +$result = $compareMethod.Invoke($comparisonService, @($comparisonRequest, [Threading.CancellationToken]::None)) + +Assert-True ($null -ne $result.RawPathBaseline) 'Comparison must publish a separately analyzed raw-path baseline.' +Assert-True $result.RawPathBaseline.IsRawPathBaseline 'Raw baseline must be explicitly marked and excluded from candidates.' +Assert-Equal 3 $result.Entries.Count 'Comparison must contain exactly one entry for every requested method.' +Assert-True ($null -ne $result.RecommendedMethod) 'A comparison with feasible methods must select a recommendation.' +foreach ($entry in $result.Entries) { + Assert-True (-not $entry.IsRawPathBaseline) 'Candidate entries must not be marked as the raw baseline.' + Assert-Equal 5 $entry.Timing.MeasuredElapsedMilliseconds.Count 'Warm-up must be excluded and exactly five measurements retained.' + Assert-True $entry.Timing.IsDeterministic 'Repeated deterministic smoothing geometry must remain eligible for recommendation.' + Assert-True (-not [string]::IsNullOrWhiteSpace($entry.StableGeometryDigest)) 'Every measured candidate must expose a stable geometry digest.' +} + +$fromMeasurementsMethod = $timingType.GetMethod('FromMeasurements') +Assert-True ($null -ne $fromMeasurementsMethod) 'Timing summary must analyze the five measured outputs for deterministic geometry.' +$failureFactory = $smoothingResultType.GetMethod('Failure') +$invalidInputStatus = [Enum]::Parse($statusType, 'InvalidInput') +$infeasibleStatus = [Enum]::Parse($statusType, 'Infeasible') +$inconsistentResults = [Array]::CreateInstance($smoothingResultType, 5) +for ($index = 0; $index -lt 5; $index++) { + $status = if ($index -eq 4) { $infeasibleStatus } else { $invalidInputStatus } + [object[]]$failureArguments = New-Object object[] 2 + $failureArguments[0] = $status + $failureArguments[1] = [Activator]::CreateInstance($diagnosticsType) + $inconsistentResults.SetValue($failureFactory.Invoke($null, $failureArguments), $index) +} +[object[]]$timingArguments = New-Object object[] 2 +$timingArguments[0] = [double[]]@(1.0, 2.0, 3.0, 4.0, 5.0) +$timingArguments[1] = $inconsistentResults +$nonDeterministicTiming = $fromMeasurementsMethod.Invoke($null, $timingArguments) +Assert-True (-not $nonDeterministicTiming.IsDeterministic) 'A status, point-count, segment-count, or digest mismatch must be non-deterministic.' +Assert-True (-not [string]::IsNullOrWhiteSpace($nonDeterministicTiming.Diagnostic)) 'Non-deterministic measurements must publish a stable diagnostic.' + +# All published comparison metrics must be normalized against the separately analyzed raw baseline. +$cornerMethods = [Array]::CreateInstance($methodType, 1) +$cornerMethods.SetValue($cubicBSpline, 0) +$cornerRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-CornerSmoothingRequest), $cornerMethods)) +$cornerResult = $compareMethod.Invoke($comparisonService, @($cornerRequest, [Threading.CancellationToken]::None)) +$cornerEntry = $cornerResult.Entries[0] +Assert-Equal 'Success' $cornerEntry.Status.ToString() 'The unconstrained empty-map corner fixture must produce a B-spline comparison candidate.' +$expectedLengthChange = (($cornerEntry.Path[$cornerEntry.Path.Count - 1].ArcLength - $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) / + $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) * 100.0 +Assert-Near $expectedLengthChange $cornerEntry.Metrics.LengthChangePercent 0.000001 'Candidate length change must be normalized relative to the raw baseline.' + +# The ranker must apply every public tie-break in order. Each pair ties all prior criteria. +$entryListType = [Collections.Generic.IReadOnlyList``1].MakeGenericType(@($entryType)) +$rankMethod = $rankerType.GetMethod('Rank', [Type[]]@($entryListType)) +Assert-True ($null -ne $rankMethod) 'SmoothingMethodRanker must expose Rank(IReadOnlyList).' + +function Assert-Rank($ExpectedMethod, [object[]]$Entries, [string]$Message) { + $typedEntries = [Array]::CreateInstance($entryType, $Entries.Count) + for ($index = 0; $index -lt $Entries.Count; $index++) { $typedEntries.SetValue($Entries[$index], $index) } + [object[]]$invokeArguments = New-Object object[] 1 + $invokeArguments[0] = $typedEntries + $actual = $rankMethod.Invoke($null, $invokeArguments) + Assert-Equal $ExpectedMethod.ToString() $actual.ToString() $Message +} + +Assert-Rank $cubicBSpline @( + (New-Entry $cubicBSpline 1.0 0.5 0.8 5.0 10.0), + (New-Entry $localCubicBezier 2.0 0.1 1.0 1.0 1.0)) 'Variation-energy tie-break must take priority over later criteria.' +Assert-Rank $cubicBSpline @( + (New-Entry $cubicBSpline 1.0 0.2 0.8 5.0 10.0), + (New-Entry $localCubicBezier 1.0 0.3 1.0 1.0 1.0)) 'Peak-curvature tie-break must follow variation energy.' +Assert-Rank $cubicBSpline @( + (New-Entry $cubicBSpline 1.0 0.2 0.9 5.0 10.0), + (New-Entry $localCubicBezier 1.0 0.2 0.8 1.0 1.0)) 'Clearance-loss tie-break must follow peak curvature.' +Assert-Rank $cubicBSpline @( + (New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 10.0), + (New-Entry $localCubicBezier 1.0 0.2 0.9 3.0 1.0)) 'Length-change tie-break must follow clearance loss.' +Assert-Rank $cubicBSpline @( + (New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 5.0), + (New-Entry $localCubicBezier 1.0 0.2 0.9 2.0 6.0)) 'Median elapsed tie-break must be last.' + +$cancelSource = [Threading.CancellationTokenSource]::new() +try { + $cancelSource.Cancel() + $cancelled = $compareMethod.Invoke($comparisonService, @($comparisonRequest, $cancelSource.Token)) + Assert-True $cancelled.IsCancelled 'Cancellation must stop comparison before subsequent methods start.' + Assert-Equal $null $cancelled.RecommendedMethod 'Cancelled comparison must not make a recommendation.' +} +finally { + $cancelSource.Dispose() +} + +Write-Output 'Path smoothing comparison checks passed.'