diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/CubicBSplineOptions.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/CubicBSplineOptions.cs new file mode 100644 index 0000000..07b2615 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/CubicBSplineOptions.cs @@ -0,0 +1,8 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 三次 B 样条平滑参数。 +public sealed class CubicBSplineOptions +{ + /// 端点切向控制柄相对于相邻弦长的比例。 + public double EndpointTangentScale { get; set; } = 1d / 3d; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalCubicBezierOptions.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalCubicBezierOptions.cs new file mode 100644 index 0000000..bd99eec --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalCubicBezierOptions.cs @@ -0,0 +1,16 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 局部三次 Bézier 平滑参数。 +public sealed class LocalCubicBezierOptions +{ + /// 判定为明显转角的最小航向变化，单位 rad。 + public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d; + + /// 单个局部平滑窗口的最大弧长，单位 m。 + public double MaximumWindowLengthMeters { get; set; } = 0.60d; + + /// 控制柄相对于窗口局部弦长的比例。 + public double HandleLengthRatio { get; set; } = 1d / 3d; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs new file mode 100644 index 0000000..fdd088e --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs @@ -0,0 +1,71 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 一条原始或平滑路径的不可变质量指标。 +public sealed class PathQualityMetrics +{ + /// 创建全零、不可行的质量指标。 + public PathQualityMetrics() + : this(false, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d) + { + } + + /// 创建完整的质量指标快照。 + public PathQualityMetrics( + bool isFeasible, + double pathLengthMeters, + double maximumAbsoluteVehicleCurvaturePerMeter, + double rootMeanSquareVehicleCurvaturePerMeter, + double totalAbsoluteCurvatureVariationPerMeter, + double curvatureVariationEnergy, + double minimumBodyClearanceMeters, + double lengthChangePercent, + double peakCurvatureChangePercent, + double curvatureVariationChangePercent, + double minimumClearanceChangeMeters) + { + IsFeasible = isFeasible; + PathLengthMeters = pathLengthMeters; + MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter; + RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter; + TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter; + CurvatureVariationEnergy = curvatureVariationEnergy; + MinimumBodyClearanceMeters = minimumBodyClearanceMeters; + LengthChangePercent = lengthChangePercent; + PeakCurvatureChangePercent = peakCurvatureChangePercent; + CurvatureVariationChangePercent = curvatureVariationChangePercent; + MinimumClearanceChangeMeters = minimumClearanceChangeMeters; + } + + /// 该路径是否通过完整安全和运动学复核。 + public bool IsFeasible { get; } + + /// 路径总弧长，单位 m。 + public double PathLengthMeters { get; } + + /// 绝对车辆曲率峰值，单位 1/m。 + public double MaximumAbsoluteVehicleCurvaturePerMeter { get; } + + /// 车辆曲率均方根，单位 1/m。 + public double RootMeanSquareVehicleCurvaturePerMeter { get; } + + /// 逐方向段累加的绝对曲率变化，单位 1/m。 + public double TotalAbsoluteCurvatureVariationPerMeter { get; } + + /// 逐方向段计算的曲率变化能量。 + public double CurvatureVariationEnergy { get; } + + /// 完整扩大车体的最小保守净空，单位 m。 + public double MinimumBodyClearanceMeters { get; } + + /// 相对原始粗路径的长度变化百分比。 + public double LengthChangePercent { get; } + + /// 相对原始粗路径的峰值曲率变化百分比。 + public double PeakCurvatureChangePercent { get; } + + /// 相对原始粗路径的曲率变化百分比。 + public double CurvatureVariationChangePercent { get; } + + /// 相对原始粗路径的最小净空变化，单位 m。 + public double MinimumClearanceChangeMeters { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs new file mode 100644 index 0000000..ab1f2a1 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs @@ -0,0 +1,50 @@ +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 路径平滑的公共配置；所有距离使用 m。 +public sealed class PathSmoothingConfiguration +{ + /// 创建带有安全默认值的平滑配置。 + public PathSmoothingConfiguration() + { + OutputSpacingMeters = 0.05d; + MaximumCollisionCheckStepMeters = 0.025d; + MinimumClearanceReserveMeters = 0.02d; + SmoothingStrength = 1d; + AllowFallbackToCoarsePath = true; + RetryStrengthScales = new ReadOnlyCollection( + new List { 1d, 0.75d, 0.50d, 0.25d }); + } + + /// 正式单算法入口使用的方法。 + public SmoothingMethod Method { get; set; } + + /// 输出路径的目标弧长采样间距，单位 m。 + public double OutputSpacingMeters { get; set; } + + /// 扫掠碰撞检查的最大步长，单位 m。 + public double MaximumCollisionCheckStepMeters { get; set; } + + /// 平滑候选必须在最小净空之外保留的额外余量，单位 m。 + public double MinimumClearanceReserveMeters { get; set; } + + /// 算法初始平滑强度。 + public double SmoothingStrength { get; set; } + + /// 所有平滑尝试失败时是否允许发布经过复核的原粗路径。 + public bool AllowFallbackToCoarsePath { get; set; } + + /// 有限且严格递减的平滑强度重试比例。 + public IReadOnlyList RetryStrengthScales { get; } + + /// 三次 B 样条专用参数。 + public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions(); + + /// 局部三次 Bézier 专用参数。 + public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions(); + + /// 分段五次多项式专用参数。 + public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions(); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingDiagnostics.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingDiagnostics.cs new file mode 100644 index 0000000..7f49d76 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingDiagnostics.cs @@ -0,0 +1,80 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 一次平滑尝试的不可变诊断信息。 +public sealed class PathSmoothingDiagnostics +{ + /// 创建不含路径指标的默认诊断信息。 + public PathSmoothingDiagnostics() + : this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty) + { + } + + /// 创建完整的平滑诊断快照。 + public PathSmoothingDiagnostics( + PathQualityMetrics metrics, + TimeSpan elapsed, + int retryCount, + double acceptedStrength, + string terminationReason = null) + { + Metrics = metrics ?? new PathQualityMetrics(); + Elapsed = elapsed; + RetryCount = retryCount; + AcceptedStrength = acceptedStrength; + TerminationReason = terminationReason ?? string.Empty; + } + + /// 使用所有测量量创建平滑诊断快照。 + public PathSmoothingDiagnostics( + bool isFeasible, + double pathLengthMeters, + double maximumAbsoluteVehicleCurvaturePerMeter, + double rootMeanSquareVehicleCurvaturePerMeter, + double totalAbsoluteCurvatureVariationPerMeter, + double curvatureVariationEnergy, + double minimumBodyClearanceMeters, + double lengthChangePercent, + double peakCurvatureChangePercent, + double curvatureVariationChangePercent, + double minimumClearanceChangeMeters, + TimeSpan elapsed, + int retryCount, + double acceptedStrength, + string terminationReason = null) + : this( + new PathQualityMetrics( + isFeasible, + pathLengthMeters, + maximumAbsoluteVehicleCurvaturePerMeter, + rootMeanSquareVehicleCurvaturePerMeter, + totalAbsoluteCurvatureVariationPerMeter, + curvatureVariationEnergy, + minimumBodyClearanceMeters, + lengthChangePercent, + peakCurvatureChangePercent, + curvatureVariationChangePercent, + minimumClearanceChangeMeters), + elapsed, + retryCount, + acceptedStrength, + terminationReason) + { + } + + /// 路径质量指标；始终非空。 + public PathQualityMetrics Metrics { get; } + + /// 从算法入口到返回诊断的耗时。 + public TimeSpan Elapsed { get; } + + /// 已执行的安全强度重试次数。 + public int RetryCount { get; } + + /// 通过复核的平滑强度；未接受候选时为零。 + public double AcceptedStrength { get; } + + /// 面向调用方的稳定终止说明。 + public string TerminationReason { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs new file mode 100644 index 0000000..a9bc37e --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs @@ -0,0 +1,51 @@ +using System.Collections.Generic; +using System.Collections.ObjectModel; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Mapping; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 路径平滑所需的原始粗路径、复核上下文和配置。 +public sealed class PathSmoothingRequest +{ + /// 创建路径平滑请求，并复制粗路径和方向分段集合。 + public PathSmoothingRequest( + IReadOnlyList coarsePath, + IReadOnlyList segments, + PlanningGridMap map, + VehicleParameters vehicle, + PathSmoothingConfiguration configuration) + { + CoarsePath = CopyReadOnly(coarsePath); + Segments = CopyReadOnly(segments); + Map = map; + Vehicle = vehicle; + Configuration = configuration; + } + + /// 原始粗路径的不可变快照。 + public IReadOnlyList CoarsePath { get; } + + /// 原始粗路径方向分段的不可变快照。 + public IReadOnlyList Segments { get; } + + /// 用于平滑后完整车体复核的规划栅格地图。 + public PlanningGridMap Map { get; } + + /// 车辆几何与最大曲率约束。 + public VehicleParameters Vehicle { get; } + + /// 本次平滑的配置。 + public PathSmoothingConfiguration Configuration { 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/Contracts/PathSmoothingResult.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs new file mode 100644 index 0000000..d63e094 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs @@ -0,0 +1,108 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 路径平滑的最终不可变结果。 +public sealed class PathSmoothingResult +{ + private static readonly IReadOnlyList EmptyPath = + new ReadOnlyCollection(new List()); + private static readonly IReadOnlyList EmptySegments = + new ReadOnlyCollection(new List()); + + private PathSmoothingResult( + PathSmoothingStatus status, + SmoothingMethod? method, + IReadOnlyList path, + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics) + { + Status = status; + Method = method; + Path = path; + Segments = segments; + Diagnostics = diagnostics ?? new PathSmoothingDiagnostics( + new PathQualityMetrics(), + TimeSpan.Zero, + 0, + 0d, + "No smoothing diagnostics were supplied."); + } + + /// 最终发布状态。 + public PathSmoothingStatus Status { get; } + + /// 成功或回退时的实际（或尝试）平滑方法；失败时为空。 + public SmoothingMethod? Method { get; } + + /// 成功或经过复核的回退路径；其他状态始终为空且不可变。 + public IReadOnlyList Path { get; } + + /// 覆盖 的方向分段；其他状态始终为空且不可变。 + public IReadOnlyList Segments { get; } + + /// 本次平滑的质量和终止诊断；始终非空。 + public PathSmoothingDiagnostics Diagnostics { get; } + + /// 创建已通过所有复核的平滑结果。 + public static PathSmoothingResult Success( + SmoothingMethod method, + IReadOnlyList path, + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics) + { + ValidatePublishedPath(path, segments); + return new PathSmoothingResult( + PathSmoothingStatus.Success, + method, + CopyReadOnly(path), + CopyReadOnly(segments), + diagnostics); + } + + /// 创建经过完整复核的原始粗路径回退结果。 + public static PathSmoothingResult Fallback( + SmoothingMethod attemptedMethod, + IReadOnlyList path, + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics) + { + ValidatePublishedPath(path, segments); + return new PathSmoothingResult( + PathSmoothingStatus.FallbackToCoarsePath, + attemptedMethod, + CopyReadOnly(path), + CopyReadOnly(segments), + diagnostics); + } + + /// 创建不发布路径的失败、不可行、取消或输入无效结果。 + public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics) + { + if (status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath) + throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status)); + if (!Enum.IsDefined(typeof(PathSmoothingStatus), status)) + throw new ArgumentOutOfRangeException(nameof(status)); + return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics); + } + + private static void ValidatePublishedPath( + IReadOnlyList path, + IReadOnlyList segments) + { + if (path == null || path.Count == 0) + throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path)); + if (segments == null || segments.Count == 0) + throw new ArgumentException("Published smoothing results require non-empty segments.", nameof(segments)); + } + + private static IReadOnlyList CopyReadOnly(IReadOnlyList source) + { + var copy = new List(source.Count); + for (int index = 0; index < source.Count; index++) + copy.Add(source[index]); + return new ReadOnlyCollection(copy); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs new file mode 100644 index 0000000..1212230 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs @@ -0,0 +1,12 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 路径平滑的最终发布状态。 +public enum PathSmoothingStatus +{ + Success, + FallbackToCoarsePath, + InvalidInput, + Infeasible, + Failed, + Cancelled, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PiecewiseQuinticOptions.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PiecewiseQuinticOptions.cs new file mode 100644 index 0000000..4e45f06 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PiecewiseQuinticOptions.cs @@ -0,0 +1,11 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 分段五次 Hermite 平滑参数。 +public sealed class PiecewiseQuinticOptions +{ + /// 相邻内部结点的目标距离，单位 m。 + public double KnotSpacingMeters { get; set; } = 0.50d; + + /// 允许创建内部结点的最小间距，单位 m。 + public double MinimumKnotSpacingMeters { get; set; } = 0.10d; +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs new file mode 100644 index 0000000..5103a4a --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs @@ -0,0 +1,66 @@ +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 平滑空间路径上的不可变采样点；位置和长度单位为 m，航向为 rad，曲率为 1/m。 +public sealed class SmoothedPathPoint +{ + public SmoothedPathPoint( + double xMeters, + double yMeters, + double headingRadians, + double unwrappedHeadingRadians, + double arcLengthMeters, + TravelDirection direction, + double geometricCurvaturePerMeter, + double vehicleCurvaturePerMeter, + double bodyClearanceMeters, + bool isGearSwitchPoint, + SmoothedPathPointSource source) + { + X = xMeters; + Y = yMeters; + Heading = headingRadians; + UnwrappedHeading = unwrappedHeadingRadians; + ArcLength = arcLengthMeters; + Direction = direction; + GeometricCurvature = geometricCurvaturePerMeter; + VehicleCurvature = vehicleCurvaturePerMeter; + BodyClearance = bodyClearanceMeters; + IsGearSwitchPoint = isGearSwitchPoint; + Source = source; + } + + /// 世界 X 坐标，单位 m。 + public double X { get; } + + /// 世界 Y 坐标，单位 m。 + public double Y { get; } + + /// 归一化的车辆航向，单位 rad。 + public double Heading { get; } + + /// 连续展开的车辆航向，单位 rad。 + public double UnwrappedHeading { get; } + + /// 从完整路径起点累计的弧长，单位 m。 + public double ArcLength { get; } + + /// 该点所在连续方向段的行驶方向。 + public TravelDirection Direction { get; } + + /// 按几何弧长计算的有符号路径曲率，单位 1/m。 + public double GeometricCurvature { get; } + + /// 车辆模型使用的有符号曲率，单位 1/m。 + public double VehicleCurvature { get; } + + /// 扩大车体后的保守净空下界，单位 m。 + public double BodyClearance { get; } + + /// 该点是否为新方向段开始的换向点。 + public bool IsGearSwitchPoint { get; } + + /// 该点在平滑流程中的来源。 + public SmoothedPathPointSource Source { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs new file mode 100644 index 0000000..f3140e1 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs @@ -0,0 +1,10 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 平滑路径点的来源。 +public enum SmoothedPathPointSource +{ + Anchor, + Interpolated, + GearSwitch, + CoarsePathFallback, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathSegment.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathSegment.cs new file mode 100644 index 0000000..5ce1d95 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathSegment.cs @@ -0,0 +1,41 @@ +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 平滑路径中方向一致的连续点范围；起止索引均包含在内。 +public sealed class SmoothedPathSegment +{ + public SmoothedPathSegment( + int segmentIndex, + TravelDirection direction, + int startIndex, + int endIndex, + bool startsAtGearSwitch, + bool endsAtGearSwitch) + { + SegmentIndex = segmentIndex; + Direction = direction; + StartIndex = startIndex; + EndIndex = endIndex; + StartsAtGearSwitch = startsAtGearSwitch; + EndsAtGearSwitch = endsAtGearSwitch; + } + + /// 从零开始的分段序号。 + public int SegmentIndex { get; } + + /// 本段行驶方向。 + public TravelDirection Direction { get; } + + /// 本段在平滑路径中的起始包含式索引。 + public int StartIndex { get; } + + /// 本段在平滑路径中的结束包含式索引。 + public int EndIndex { get; } + + /// 本段首点是否为换向后保留的新方向点。 + public bool StartsAtGearSwitch { get; } + + /// 本段末点是否紧邻下一方向段的换向对。 + public bool EndsAtGearSwitch { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs new file mode 100644 index 0000000..41d57e6 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs @@ -0,0 +1,9 @@ +namespace MultiWheelC.TrajectoryPlanning.PathSmoothing; + +/// 支持的粗路径平滑方法。 +public enum SmoothingMethod +{ + CubicBSpline, + LocalCubicBezier, + PiecewiseQuintic, +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 new file mode 100644 index 0000000..affdb25 --- /dev/null +++ b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 @@ -0,0 +1,179 @@ +param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll')) +$ErrorActionPreference = 'Stop' + +$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath)) +$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.' +$coarsePathRoot = 'MultiWheelC.TrajectoryPlanning.CoarsePath.' +$mappingRoot = 'MultiWheelC.TrajectoryPlanning.Mapping.' + +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, [string]$Message) { + if ([Math]::Abs($Expected - $Actual) -gt 0.000001d) { + throw "$Message Expected=$Expected Actual=$Actual" + } +} + +function Assert-Throws([scriptblock]$Action, [string]$Message) { + $threw = $false + try { & $Action } + catch { $threw = $true } + if (-not $threw) { throw $Message } +} + +function Assert-ReadOnlyCollection($Collection, [string]$Message) { + $list = [System.Collections.IList]$Collection + Assert-True ($null -ne $list) "$Message The collection must implement IList." + Assert-True $list.IsReadOnly "$Message The collection must report IsReadOnly." + Assert-Throws { $list.Add($null) } "$Message The collection must reject Add." +} + +function Get-RequiredType([string]$Name) { + return $assembly.GetType($Name, $true) +} + +$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration') +$methodType = Get-RequiredType ($root + 'SmoothingMethod') +$statusType = Get-RequiredType ($root + 'PathSmoothingStatus') +$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource') +$pointType = Get-RequiredType ($root + 'SmoothedPathPoint') +$segmentType = Get-RequiredType ($root + 'SmoothedPathSegment') +$bsplineOptionsType = Get-RequiredType ($root + 'CubicBSplineOptions') +$bezierOptionsType = Get-RequiredType ($root + 'LocalCubicBezierOptions') +$quinticOptionsType = Get-RequiredType ($root + 'PiecewiseQuinticOptions') +$requestType = Get-RequiredType ($root + 'PathSmoothingRequest') +$metricsType = Get-RequiredType ($root + 'PathQualityMetrics') +$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics') +$resultType = Get-RequiredType ($root + 'PathSmoothingResult') +$directionType = Get-RequiredType ($coarsePathRoot + 'TravelDirection') +$coarsePointType = Get-RequiredType ($coarsePathRoot + 'CoarsePathPoint') +$coarseSegmentType = Get-RequiredType ($coarsePathRoot + 'PathSegment') +$mapType = Get-RequiredType ($mappingRoot + 'PlanningGridMap') +$vehicleType = Get-RequiredType ($coarsePathRoot + 'VehicleParameters') + +Assert-Equal $true $methodType.IsEnum 'SmoothingMethod must be a public enum.' +Assert-Equal $true $statusType.IsEnum 'PathSmoothingStatus must be a public enum.' +Assert-Equal $true $sourceType.IsEnum 'SmoothedPathPointSource must be a public enum.' +Assert-Equal 'CubicBSpline' ([Enum]::GetNames($methodType)[0]) 'Smoothing method order must remain stable.' +Assert-Equal 'LocalCubicBezier' ([Enum]::GetNames($methodType)[1]) 'Smoothing method order must remain stable.' +Assert-Equal 'PiecewiseQuintic' ([Enum]::GetNames($methodType)[2]) 'Smoothing method order must remain stable.' +Assert-Equal 'Success' ([Enum]::GetNames($statusType)[0]) 'Smoothing status order must remain stable.' +Assert-Equal 'FallbackToCoarsePath' ([Enum]::GetNames($statusType)[1]) 'Fallback status must be explicit.' + +$configuration = [Activator]::CreateInstance($configurationType) +Assert-Near 0.05 $configuration.OutputSpacingMeters 'Default output spacing must be 0.05 m.' +Assert-Near 0.025 $configuration.MaximumCollisionCheckStepMeters 'Default collision step must be 0.025 m.' +Assert-Near 0.02 $configuration.MinimumClearanceReserveMeters 'Default clearance reserve must be 0.02 m.' +Assert-Near 1.0 $configuration.SmoothingStrength 'Default smoothing strength must be 1.0.' +Assert-Equal $true $configuration.AllowFallbackToCoarsePath 'Fallback must be enabled by default.' +Assert-Equal 4 $configuration.RetryStrengthScales.Count 'Retry schedule must contain four entries.' +Assert-Near 1.0 $configuration.RetryStrengthScales[0] 'First retry scale must be 1.0.' +Assert-Near 0.25 $configuration.RetryStrengthScales[3] 'Last retry scale must be 0.25.' +Assert-ReadOnlyCollection $configuration.RetryStrengthScales 'Retry schedule must be immutable.' +Assert-Near (1.0 / 3.0) ([Activator]::CreateInstance($bsplineOptionsType)).EndpointTangentScale 'B-spline endpoint tangent default must be one third.' +$bezier = [Activator]::CreateInstance($bezierOptionsType) +Assert-Near ([Math]::PI / 18.0) $bezier.CornerHeadingThresholdRadians 'Bezier corner threshold must be 10 degrees.' +Assert-Near 0.60 $bezier.MaximumWindowLengthMeters 'Bezier window default must be 0.60 m.' +Assert-Near (1.0 / 3.0) $bezier.HandleLengthRatio 'Bezier handle default must be one third.' +$quintic = [Activator]::CreateInstance($quinticOptionsType) +Assert-Near 0.50 $quintic.KnotSpacingMeters 'Quintic knot spacing must be 0.50 m.' +Assert-Near 0.10 $quintic.MinimumKnotSpacingMeters 'Quintic minimum knot spacing must be 0.10 m.' + +$forward = [Enum]::Parse($directionType, 'Forward') +$anchor = [Enum]::Parse($sourceType, 'Anchor') +$point = [Activator]::CreateInstance($pointType, @( + [double]1.25, [double]-2.50, [double]0.30, [double]6.58, [double]4.75, + $forward, [double]0.12, [double]0.12, [double]0.44, $false, $anchor)) +Assert-Near 1.25 $point.X 'Smoothed point X must be stored in m.' +Assert-Near -2.50 $point.Y 'Smoothed point Y must be stored in m.' +Assert-Near 0.30 $point.Heading 'Smoothed point heading must be stored in rad.' +Assert-Near 6.58 $point.UnwrappedHeading 'Smoothed point unwrapped heading must be stored in rad.' +Assert-Near 4.75 $point.ArcLength 'Smoothed point arc length must be stored in m.' +Assert-Equal 'Forward' $point.Direction.ToString() 'Smoothed point direction must be preserved.' +Assert-Near 0.12 $point.GeometricCurvature 'Smoothed point geometric curvature must be stored in 1/m.' +Assert-Near 0.12 $point.VehicleCurvature 'Smoothed point vehicle curvature must be stored in 1/m.' +Assert-Near 0.44 $point.BodyClearance 'Smoothed point clearance must be stored in m.' +Assert-Equal $false $point.IsGearSwitchPoint 'Smoothed point gear-switch marker must be preserved.' +Assert-Equal 'Anchor' $point.Source.ToString() 'Smoothed point source must be preserved.' + +$segmentA = [Activator]::CreateInstance($segmentType, @(0, $forward, 0, 2, $false, $true)) +$reverse = [Enum]::Parse($directionType, 'Reverse') +$segmentB = [Activator]::CreateInstance($segmentType, @(1, $reverse, 3, 5, $true, $false)) +Assert-Equal 0 $segmentA.SegmentIndex 'First smoothing segment index must be retained.' +Assert-Equal 'Forward' $segmentA.Direction.ToString() 'First smoothing segment direction must be retained.' +Assert-Equal 2 $segmentA.EndIndex 'First smoothing segment end index must be retained.' +Assert-Equal $true $segmentA.EndsAtGearSwitch 'First smoothing segment switch flag must be retained.' +Assert-Equal 1 $segmentB.SegmentIndex 'Second smoothing segment index must be retained.' +Assert-Equal 'Reverse' $segmentB.Direction.ToString() 'Second smoothing segment direction must be retained.' +Assert-Equal $true $segmentB.StartsAtGearSwitch 'Second smoothing segment switch flag must be retained.' + +$metrics = [Activator]::CreateInstance($metricsType) +Assert-Equal $false $metrics.IsFeasible 'Default metrics must be infeasible until analysis accepts a candidate.' +Assert-Near 0.0 $metrics.PathLengthMeters 'Default metrics must be zero-valued.' +Assert-Near 0.0 $metrics.MinimumBodyClearanceMeters 'Default metrics must be zero-valued.' +$diagnostics = [Activator]::CreateInstance($diagnosticsType) +Assert-True ($diagnostics.Metrics -ne $null) 'Default diagnostics must provide quality metrics.' +Assert-Equal 0 $diagnostics.RetryCount 'Default diagnostics must have no retries.' +Assert-Near 0.0 $diagnostics.AcceptedStrength 'Default diagnostics must have zero accepted strength.' + +$pointArray = [Array]::CreateInstance($pointType, 1) +$pointArray.SetValue($point, 0) +$segmentArray = [Array]::CreateInstance($segmentType, 2) +$segmentArray.SetValue($segmentA, 0) +$segmentArray.SetValue($segmentB, 1) +$method = [Enum]::Parse($methodType, 'CubicBSpline') +$successMethod = $resultType.GetMethod('Success') +Assert-True ($null -ne $successMethod) 'PathSmoothingResult must expose Success.' +$success = $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $diagnostics)) +Assert-Equal 'Success' $success.Status.ToString() 'Success factory must publish Success status.' +Assert-Equal 'CubicBSpline' $success.Method.ToString() 'Success factory must retain the selected method.' +Assert-Equal 1 $success.Path.Count 'Success factory must publish the provided path.' +Assert-Equal 2 $success.Segments.Count 'Success factory must publish the provided segments.' +Assert-ReadOnlyCollection $success.Path 'Success path must be immutable.' +Assert-ReadOnlyCollection $success.Segments 'Success segments must be immutable.' +$pointArray.SetValue($null, 0) +$segmentArray.SetValue($null, 0) +Assert-True ($null -ne $success.Path[0]) 'Success factory must copy path collections.' +Assert-True ($null -ne $success.Segments[0]) 'Success factory must copy segment collections.' + +$fallbackMethod = $resultType.GetMethod('Fallback') +Assert-True ($null -ne $fallbackMethod) 'PathSmoothingResult must expose Fallback.' +$fallbackPath = [Array]::CreateInstance($pointType, 1) +$fallbackPath.SetValue($point, 0) +$fallbackSegments = [Array]::CreateInstance($segmentType, 1) +$fallbackSegments.SetValue($segmentA, 0) +$fallback = $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $diagnostics)) +Assert-Equal 'FallbackToCoarsePath' $fallback.Status.ToString() 'Fallback factory must publish an explicit fallback status.' +Assert-Equal 1 $fallback.Path.Count 'Fallback factory must publish a validated fallback path.' + +$failureMethod = $resultType.GetMethod('Failure') +Assert-True ($null -ne $failureMethod) 'PathSmoothingResult must expose Failure.' +$failed = $failureMethod.Invoke( + $null, + @([Enum]::Parse($statusType, 'InvalidInput'), + [Activator]::CreateInstance($diagnosticsType))) +Assert-Equal 'InvalidInput' $failed.Status.ToString() 'Failure factory must retain failure status.' +Assert-Equal 0 $failed.Path.Count 'Failure must publish no path.' +Assert-Equal 0 $failed.Segments.Count 'Failure must publish no segments.' +Assert-ReadOnlyCollection $failed.Path 'Failure path must be immutable.' +Assert-ReadOnlyCollection $failed.Segments 'Failure segments must be immutable.' +Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'Success'), $diagnostics)) } 'Failure factory must reject Success.' +Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'FallbackToCoarsePath'), $diagnostics)) } 'Failure factory must reject fallback status.' +Assert-Throws { $successMethod.Invoke($null, @($method, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $diagnostics)) } 'Success factory must reject an empty path.' +Assert-Throws { $successMethod.Invoke($null, @($method, $fallbackPath, [Array]::CreateInstance($segmentType, 0), $diagnostics)) } 'Success factory must reject empty segments.' + +$requestConstructor = $requestType.GetConstructor(@( + [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarsePointType), + [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarseSegmentType), + $mapType, + $vehicleType, + $configurationType)) +Assert-True ($null -ne $requestConstructor) 'PathSmoothingRequest must expose the public five-argument constructor.' + +Write-Output 'Path smoothing contract checks passed.'