diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs index a9bc37e..f743f13 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs @@ -19,8 +19,8 @@ public sealed class PathSmoothingRequest CoarsePath = CopyReadOnly(coarsePath); Segments = CopyReadOnly(segments); Map = map; - Vehicle = vehicle; - Configuration = configuration; + Vehicle = CopyVehicle(vehicle); + Configuration = CopyConfiguration(configuration); } /// 原始粗路径的不可变快照。 @@ -48,4 +48,38 @@ public sealed class PathSmoothingRequest } return new ReadOnlyCollection(copy); } + + private static VehicleParameters CopyVehicle(VehicleParameters source) + { + if (source == null) return null; + return new VehicleParameters + { + LengthMeters = source.LengthMeters, + WidthMeters = source.WidthMeters, + SafetyMarginMeters = source.SafetyMarginMeters, + MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter, + MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters, + }; + } + + private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source) + { + if (source == null) return null; + var copy = new PathSmoothingConfiguration + { + Method = source.Method, + OutputSpacingMeters = source.OutputSpacingMeters, + MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters, + MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters, + SmoothingStrength = source.SmoothingStrength, + AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath, + }; + copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale; + copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians; + copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters; + copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio; + copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters; + copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters; + return copy; + } } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs index d63e094..1555b4f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs @@ -53,7 +53,7 @@ public sealed class PathSmoothingResult IReadOnlyList segments, PathSmoothingDiagnostics diagnostics) { - ValidatePublishedPath(path, segments); + ValidatePublishedResult(method, path, segments, diagnostics); return new PathSmoothingResult( PathSmoothingStatus.Success, method, @@ -69,7 +69,7 @@ public sealed class PathSmoothingResult IReadOnlyList segments, PathSmoothingDiagnostics diagnostics) { - ValidatePublishedPath(path, segments); + ValidatePublishedResult(attemptedMethod, path, segments, diagnostics); return new PathSmoothingResult( PathSmoothingStatus.FallbackToCoarsePath, attemptedMethod, @@ -88,14 +88,20 @@ public sealed class PathSmoothingResult return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics); } - private static void ValidatePublishedPath( + private static void ValidatePublishedResult( + SmoothingMethod method, IReadOnlyList path, - IReadOnlyList segments) + IReadOnlyList segments, + PathSmoothingDiagnostics diagnostics) { + if (!Enum.IsDefined(typeof(SmoothingMethod), method)) + throw new ArgumentOutOfRangeException(nameof(method)); 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)); + if (diagnostics == null || diagnostics.Metrics == null || !diagnostics.Metrics.IsFeasible) + throw new ArgumentException("Published smoothing results require feasible diagnostics.", nameof(diagnostics)); } private static IReadOnlyList CopyReadOnly(IReadOnlyList source) diff --git a/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 index affdb25..60e5cf4 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_contracts.ps1 @@ -60,11 +60,9 @@ $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.' +Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic' ([string]::Join(',', [Enum]::GetNames($methodType))) 'Smoothing method members and order must remain stable.' +Assert-Equal 'Success,FallbackToCoarsePath,InvalidInput,Infeasible,Failed,Cancelled' ([string]::Join(',', [Enum]::GetNames($statusType))) 'Smoothing status members and order must remain stable.' +Assert-Equal 'Anchor,Interpolated,GearSwitch,CoarsePathFallback' ([string]::Join(',', [Enum]::GetNames($sourceType))) 'Smoothed point source members and order must remain stable.' $configuration = [Activator]::CreateInstance($configurationType) Assert-Near 0.05 $configuration.OutputSpacingMeters 'Default output spacing must be 0.05 m.' @@ -74,7 +72,12 @@ Assert-Near 1.0 $configuration.SmoothingStrength 'Default smoothing strength mus 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.75 $configuration.RetryStrengthScales[1] 'Second retry scale must be 0.75.' +Assert-Near 0.50 $configuration.RetryStrengthScales[2] 'Third retry scale must be 0.50.' Assert-Near 0.25 $configuration.RetryStrengthScales[3] 'Last retry scale must be 0.25.' +for ($index = 1; $index -lt $configuration.RetryStrengthScales.Count; $index++) { + Assert-True ($configuration.RetryStrengthScales[$index] -lt $configuration.RetryStrengthScales[$index - 1]) 'Retry schedule must be strictly decreasing.' +} 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) @@ -121,6 +124,12 @@ $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.' +$feasibleMetrics = [Activator]::CreateInstance($metricsType, @( + $true, + [double]1.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0, + [double]0.5, [double]0.0, [double]0.0, [double]0.0, [double]0.0)) +$feasibleDiagnostics = [Activator]::CreateInstance($diagnosticsType, @( + $feasibleMetrics, [TimeSpan]::Zero, 0, [double]1.0, 'test feasible diagnostics')) $pointArray = [Array]::CreateInstance($pointType, 1) $pointArray.SetValue($point, 0) @@ -130,7 +139,9 @@ $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-Throws { $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $diagnostics)) } 'Success factory must reject diagnostics that are not feasible.' +Assert-Throws { $successMethod.Invoke($null, @([Enum]::ToObject($methodType, 99), $pointArray, $segmentArray, $feasibleDiagnostics)) } 'Success factory must reject undefined smoothing methods.' +$success = $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $feasibleDiagnostics)) 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.' @@ -148,7 +159,8 @@ $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-Throws { $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $diagnostics)) } 'Fallback factory must reject diagnostics that are not feasible.' +$fallback = $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $feasibleDiagnostics)) 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.' @@ -175,5 +187,46 @@ $requestConstructor = $requestType.GetConstructor(@( $vehicleType, $configurationType)) Assert-True ($null -ne $requestConstructor) 'PathSmoothingRequest must expose the public five-argument constructor.' +$boundsType = Get-RequiredType ($mappingRoot + 'MapBoundsMm') +$mapRequestType = Get-RequiredType ($mappingRoot + 'PlanningMapRequest') +$mapFactoryType = Get-RequiredType ($mappingRoot + 'PlanningMapFactory') +$mapRequest = [Activator]::CreateInstance($mapRequestType) +$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]1000, [single]0, [single]1000)) +$mapRequest.ResolutionMm = [single]50 +$mapRequest.AllowExplicitEmptyMap = $true +$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map +Assert-True ($null -ne $map) 'Contract test must create an explicit empty planning map.' +$requestCoarsePath = [Array]::CreateInstance($coarsePointType, 1) +$requestCoarsePath.SetValue([Activator]::CreateInstance($coarsePointType, @( + [double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0, + $forward, [double]0.0, [double]1.0, $false, + [Enum]::Parse((Get-RequiredType ($coarsePathRoot + 'CoarsePathPointSource')), 'Start'))), 0) +$requestSegments = [Array]::CreateInstance($coarseSegmentType, 1) +$requestSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 0, $false, $false)), 0) +$vehicle = [Activator]::CreateInstance($vehicleType) +$vehicle.LengthMeters = [double]0.80 +$vehicle.WidthMeters = [double]0.60 +$vehicle.SafetyMarginMeters = [double]0.05 +$vehicle.MaximumCurvaturePerMeter = [double]0.8333333333333334 +$requestConfiguration = [Activator]::CreateInstance($configurationType) +$request = $requestConstructor.Invoke(@($requestCoarsePath, $requestSegments, $map, $vehicle, $requestConfiguration)) +Assert-ReadOnlyCollection $request.CoarsePath 'Request coarse path must be immutable.' +Assert-ReadOnlyCollection $request.Segments 'Request segments must be immutable.' +$requestCoarsePath.SetValue($null, 0) +$requestSegments.SetValue($null, 0) +$vehicle.LengthMeters = [double]9.99 +$vehicle.MaximumCurvaturePerMeter = [double]0.1 +$requestConfiguration.OutputSpacingMeters = [double]0.99 +$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.99 +$requestConfiguration.LocalCubicBezier.HandleLengthRatio = [double]0.99 +$requestConfiguration.PiecewiseQuintic.KnotSpacingMeters = [double]0.99 +Assert-True ($null -ne $request.CoarsePath[0]) 'Request must copy the coarse-path collection.' +Assert-True ($null -ne $request.Segments[0]) 'Request must copy the segment collection.' +Assert-Near 0.80 $request.Vehicle.LengthMeters 'Request must snapshot vehicle parameters.' +Assert-Near (1.0 / 1.20) $request.Vehicle.MaximumCurvaturePerMeter 'Request must snapshot nullable vehicle curvature.' +Assert-Near 0.05 $request.Configuration.OutputSpacingMeters 'Request must snapshot common configuration.' +Assert-Near (1.0 / 3.0) $request.Configuration.CubicBSpline.EndpointTangentScale 'Request must snapshot B-spline options.' +Assert-Near (1.0 / 3.0) $request.Configuration.LocalCubicBezier.HandleLengthRatio 'Request must snapshot Bezier options.' +Assert-Near 0.50 $request.Configuration.PiecewiseQuintic.KnotSpacingMeters 'Request must snapshot quintic options.' Write-Output 'Path smoothing contract checks passed.'