From 24af39de747818a8e59204f0e3c5487ef7fee23f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 31 Jul 2026 16:38:21 +0800 Subject: [PATCH] fix: harden Local G2 stabilization regressions --- .../LocalG2/LocalG2CandidateBuilder.cs | 145 ++++++++++++++++-- .../LocalG2/LocalG2SmoothingRegion.cs | 2 +- .../Processing/RawPathBaselineBuilder.cs | 24 +++ .../tests/verify_path_smoothing_geometry.ps1 | 68 +++++++- ...ify_path_smoothing_local_g2_candidates.ps1 | 10 +- 5 files changed, 234 insertions(+), 15 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs index edab501..fdf7756 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs @@ -562,6 +562,7 @@ internal sealed class LocalG2CandidateBuilder case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints(); case "GearBoundary": return BuildGearBoundary(); case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder(); + case "MultiSegmentWorkOrder": return BuildMultiSegmentWorkOrder(); default: throw new ArgumentOutOfRangeException(nameof(scenario)); } } @@ -576,7 +577,9 @@ internal sealed class LocalG2CandidateBuilder bool gearBoundaryMarkerPreserved = false, bool accepted = false, bool workOrderDescending = false, bool frontArcPreservedAfterBackReplacement = false, bool bothReplacementsRetained = false, bool forwardOrderRejected = false, - bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false) + bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false, + bool deterministicReplacementGeometry = false, bool segmentOrderAscending = false, + bool equalArcUsesReportOrder = false) { CandidateCount = candidateCount; StartPositionError = startPositionError; @@ -600,6 +603,9 @@ internal sealed class LocalG2CandidateBuilder ForwardOrderRejected = forwardOrderRejected; DeterministicWorkOrder = deterministicWorkOrder; InvalidWorkOrderRejected = invalidWorkOrderRejected; + DeterministicReplacementGeometry = deterministicReplacementGeometry; + SegmentOrderAscending = segmentOrderAscending; + EqualArcUsesReportOrder = equalArcUsesReportOrder; } public int CandidateCount { get; } public double StartPositionError { get; } @@ -623,6 +629,9 @@ internal sealed class LocalG2CandidateBuilder public bool ForwardOrderRejected { get; } public bool DeterministicWorkOrder { get; } public bool InvalidWorkOrderRejected { get; } + public bool DeterministicReplacementGeometry { get; } + public bool SegmentOrderAscending { get; } + public bool EqualArcUsesReportOrder { get; } } private static CandidateTestSnapshot BuildIsolated() @@ -951,15 +960,50 @@ internal sealed class LocalG2CandidateBuilder throw new InvalidOperationException(frontReason); } - int localG2PointCount = 0; + if (!splicer.TryReplace( + original, + backCandidate, + out PreparedPath repeatedAfterBack, + out string repeatedBackReason)) + { + throw new InvalidOperationException(repeatedBackReason); + } + if (!splicer.TryReplace( + repeatedAfterBack, + frontCandidate, + out PreparedPath repeatedAfterBoth, + out string repeatedFrontReason)) + { + throw new InvalidOperationException(repeatedFrontReason); + } + + bool frontInteriorRetained = false; + bool backInteriorRetained = false; + int frontInteriorCount = 0; + int backInteriorCount = 0; for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++) { - if (afterBoth.Segments[0].Points[index].Source == - SmoothedPathPointSource.LocalG2Transition) + SmoothingPoint2D point = afterBoth.Segments[0].Points[index]; + if (point.Source != SmoothedPathPointSource.LocalG2Transition) + continue; + if (point.X == 0.75d && point.Y == 0.20d) { - localG2PointCount++; + frontInteriorCount++; + frontInteriorRetained = true; + } + if (point.X == 2.25d && point.Y == 0.20d) + { + backInteriorCount++; + backInteriorRetained = true; } } + bool exactInteriorsRetained = + frontInteriorRetained && + backInteriorRetained && + frontInteriorCount == 1 && + backInteriorCount == 1; + bool deterministicReplacementGeometry = + HasSameReplacementGeometry(afterBoth, repeatedAfterBoth); if (!splicer.TryReplace( original, @@ -994,20 +1038,101 @@ internal sealed class LocalG2CandidateBuilder false, descending, frontArcPreserved, - localG2PointCount >= 2, + exactInteriorsRetained, forwardOrderRejected, deterministic, - invalidWorkOrderRejected); + invalidWorkOrderRejected, + deterministicReplacementGeometry); + } + + private static CandidateTestSnapshot BuildMultiSegmentWorkOrder() + { + LocalG2SmoothingRegion laterSegment = + CreateOrderedRegion(0.25d, 0d, 0.5d, 0, 1); + LocalG2SmoothingRegion firstTie = + CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0); + LocalG2SmoothingRegion secondTie = + CreateOrderedRegion(0.75d, 0.5d, 1.0d, 1); + var reportOrder = new[] { laterSegment, firstTie, secondTie }; + if (!new LocalG2RegionWorkOrder().TryCreate( + reportOrder, + out IReadOnlyList workOrder, + out string reason)) + { + throw new InvalidOperationException(reason); + } + + bool segmentOrderAscending = + workOrder.Count == 3 && + workOrder[0].SegmentIndex == 0 && + workOrder[1].SegmentIndex == 0 && + workOrder[2].SegmentIndex == 1; + bool equalArcUsesReportOrder = + ReferenceEquals(firstTie, workOrder[0]) && + ReferenceEquals(secondTie, workOrder[1]); + return new CandidateTestSnapshot( + 0, + 0d, + 0d, + 0d, + 0d, + false, + 0, + false, + string.Empty, + false, + false, + false, + segmentOrderAscending: segmentOrderAscending, + equalArcUsesReportOrder: equalArcUsesReportOrder); + } + + private static bool HasSameReplacementGeometry( + PreparedPath first, + PreparedPath second) + { + if (first == null || second == null || + first.Points.Count != second.Points.Count || + first.Segments.Count != second.Segments.Count) + { + return false; + } + + for (int segmentIndex = 0; segmentIndex < first.Segments.Count; segmentIndex++) + { + PreparedDirectionSegment firstSegment = first.Segments[segmentIndex]; + PreparedDirectionSegment secondSegment = second.Segments[segmentIndex]; + if (firstSegment.Direction != secondSegment.Direction || + firstSegment.Points.Count != secondSegment.Points.Count) + { + return false; + } + + for (int pointIndex = 0; pointIndex < firstSegment.Points.Count; pointIndex++) + { + SmoothingPoint2D firstPoint = firstSegment.Points[pointIndex]; + SmoothingPoint2D secondPoint = secondSegment.Points[pointIndex]; + if (firstPoint.X != secondPoint.X || + firstPoint.Y != secondPoint.Y || + firstPoint.Source != secondPoint.Source || + firstSegment.Direction != secondSegment.Direction) + { + return false; + } + } + } + return true; } private static LocalG2SmoothingRegion CreateOrderedRegion( double eventArc, double startArc, double endArc, - int index) + int index, + int segmentIndex = 0) { var transition = new CurvatureTransition( - 0, + segmentIndex, index, index + 1, eventArc, @@ -1017,7 +1142,7 @@ internal sealed class LocalG2CandidateBuilder 0d, 0.4d); return new LocalG2SmoothingRegion( - 0, + segmentIndex, new[] { transition }, startArc, endArc, diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs index 8a99f98..e2e102a 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs @@ -30,7 +30,7 @@ internal sealed class LocalG2WindowVariant internal double RightWindowLengthMeters { get; } } -/// 因最大合法窗口相交而合并的一组曲率事件。 +/// 至少存在一个联合合法生成窗口变体的一组曲率过渡。 internal sealed class LocalG2SmoothingRegion { internal LocalG2SmoothingRegion( diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs index decf6f3..b9d34f8 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation; namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing; @@ -45,6 +46,7 @@ internal static class RawPathBaselineBuilder } if (reason != "平滑路径包含非法数值或超限车辆曲率。" || + !HasTrustedVehicleCurvaturesWithinLimit(request) || !TryCreateTrustedRawAnalysis(request, out analysis, out reason) || !validator.TryValidate( analysis.Path, @@ -64,6 +66,28 @@ internal static class RawPathBaselineBuilder return true; } + private static bool HasTrustedVehicleCurvaturesWithinLimit(PathSmoothingRequest request) + { + if (request?.CoarsePath == null || request.CoarsePath.Count == 0 || + !VehicleKinematics.TryGetMaximumCurvaturePerMeter( + request.Vehicle, + out double maximumCurvaturePerMeter)) + { + return false; + } + + for (int index = 0; index < request.CoarsePath.Count; index++) + { + CoarsePathPoint point = request.CoarsePath[index]; + if (point == null || !IsFinite(point.VehicleCurvature) || + Math.Abs(point.VehicleCurvature) > maximumCurvaturePerMeter) + { + return false; + } + } + return true; + } + private static bool TryCreateTrustedRawAnalysis( PathSmoothingRequest request, out PathGeometryAnalysis analysis, diff --git a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 index 42bfbde..8aee66e 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 @@ -133,10 +133,12 @@ function New-CoarsePathPoint( [double]$ArcLength, $Direction, [bool]$IsGearSwitch = $false, - [string]$SourceName = 'MotionPrimitive') { + [string]$SourceName = 'MotionPrimitive', + [double]$Heading = 0.0, + [double]$VehicleCurvature = 0.0) { return [Activator]::CreateInstance($coarsePointType, @( - $X, $Y, [double]0.0, [double]0.0, $ArcLength, - $Direction, [double]0.0, [double]1.0, $IsGearSwitch, + $X, $Y, $Heading, $Heading, $ArcLength, + $Direction, $VehicleCurvature, [double]1.0, $IsGearSwitch, [Enum]::Parse($coarsePointSourceType, $SourceName))) } @@ -284,6 +286,7 @@ $boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBounds $mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest' $mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory' $validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator') +$rawBaselineBuilderType = Get-RequiredType ($processing + 'RawPathBaselineBuilder') Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.' Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.' @@ -299,6 +302,10 @@ $validator = [Activator]::CreateInstance($validatorType) $validateMethod = $validatorType.GetMethod('TryValidate') Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.' Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.' +$rawBaselineMethod = $rawBaselineBuilderType.GetMethod( + 'TryCreate', + [Reflection.BindingFlags]'Static,NonPublic') +Assert-True ($null -ne $rawBaselineMethod) 'RawPathBaselineBuilder must expose its internal TryCreate path.' $forward = [Enum]::Parse($directionType, 'Forward') $reverse = [Enum]::Parse($directionType, 'Reverse') @@ -331,6 +338,61 @@ Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $max $overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.' +# Trusted raw fallback must apply the exact vehicle-curvature gate before reconstruction. +$trustedLimit = 0.80 +$trustedOverLimit = $trustedLimit + 0.0000005 +$analyzedCurvature = $trustedLimit + 0.01 +$chordLength = 0.05 +$headingStep = 2.0 * [Math]::Asin($analyzedCurvature * $chordLength / 2.0) +$radius = 1.0 / $analyzedCurvature +$trustedPoints = [Array]::CreateInstance($coarsePointType, 21) +for ($index = 0; $index -le 20; $index++) { + $theta = $headingStep * $index + $trustedPoints.SetValue((New-CoarsePathPoint ` + (1.0 + $radius * [Math]::Sin($theta)) ` + (1.0 + $radius * (1.0 - [Math]::Cos($theta))) ` + ($index * $chordLength) ` + $forward ` + $false ` + $(if ($index -eq 0) { 'Start' } else { 'MotionPrimitive' }) ` + $theta ` + $trustedOverLimit), $index) +} +$trustedSegments = [Array]::CreateInstance($coarseSegmentType, 1) +$trustedSegments.SetValue( + [Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 20, $false, $false)), + 0) +$trustedVehicle = [Activator]::CreateInstance($vehicleType) +$trustedVehicle.LengthMeters = 0.20 +$trustedVehicle.WidthMeters = 0.20 +$trustedVehicle.SafetyMarginMeters = 0.0 +$trustedVehicle.MaximumCurvaturePerMeter = $trustedLimit +$trustedConfiguration = [Activator]::CreateInstance($smoothingConfigurationType) +$trustedRequest = [Activator]::CreateInstance($smoothingRequestType, @( + $trustedPoints, + $trustedSegments, + (New-EmptyGeometryMap), + $trustedVehicle, + $trustedConfiguration)) +$trustedPreprocessor = [Activator]::CreateInstance($preprocessorType) +$trustedPrepareMethod = $preprocessorType.GetMethod('TryPrepare') +$trustedPrepareArguments = [object[]]@($trustedRequest, $null, $null) +Assert-True $trustedPrepareMethod.Invoke($trustedPreprocessor, $trustedPrepareArguments) ` + ('Trusted raw fallback regression must prepare successfully. Reason=' + $trustedPrepareArguments[2]) +$trustedBaselineArguments = [object[]]@( + $trustedRequest, + $trustedPrepareArguments[1], + $analyzer, + $trustedConfiguration.OutputSpacingMeters, + $validator, + $trustedConfiguration.MaximumCollisionCheckStepMeters, + $null, + $null) +Assert-False $rawBaselineMethod.Invoke($null, $trustedBaselineArguments) ` + 'Trusted raw fallback must not publish a coarse curvature above the exact vehicle maximum.' +Assert-Equal $overLimitValidation.Reason $trustedBaselineArguments[7] ` + 'Rejecting trusted raw fallback must preserve the original analyzed-path validation failure.' + # The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator. foreach ($quinticCase in @( [pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 }, diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 index 2f7625b..8cbfe73 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 @@ -80,14 +80,22 @@ Assert-True $twoRegions.WorkOrderDescending ` Assert-True $twoRegions.FrontArcPreservedAfterBackReplacement ` 'Replacing the back region must preserve the front region original arc coordinates.' Assert-True $twoRegions.BothReplacementsRetained ` - 'Back-then-front replacement must retain both length-changing local replacements.' + 'Back-then-front replacement must retain the exact LocalG2Transition interiors at (0.75, 0.20) and (2.25, 0.20).' Assert-True $twoRegions.ForwardOrderRejected ` 'The regression fixture must prove that front-first invalidates the original back absolute arc.' Assert-True $twoRegions.DeterministicWorkOrder ` 'Work ordering must repeat exactly without mutating report order.' +Assert-True $twoRegions.DeterministicReplacementGeometry ` + 'Repeated back-then-front replacement must preserve count and every point X, Y, source, and direction.' Assert-True $twoRegions.InvalidWorkOrderRejected ` 'Work ordering must reject null regions and cross-segment event contents.' +$ordering = Invoke-Scenario 'MultiSegmentWorkOrder' +Assert-True $ordering.SegmentOrderAscending ` + 'Region work ordering must process segment indices in ascending order.' +Assert-True $ordering.EqualArcUsesReportOrder ` + 'Equal first-local-arc regions must retain their original report-index order.' + $evaluatorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateEvaluator' $evaluatorHooksType = $evaluatorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic') Assert-True ($null -ne $evaluatorHooksType) 'LocalG2CandidateEvaluator must expose narrowly scoped deterministic TestHooks.'