First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix: harden Local G2 stabilization regressions

Browse Source
This commit is contained in:
梁薄云
2026-07-31 16:38:21 +08:00
parent 49c7e5b0c0
commit 24af39de74
5 changed files with 234 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
+135 -10
View File
@@ -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<LocalG2SmoothingRegion> 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,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs
+1 -1
View File
@@ -30,7 +30,7 @@ internal sealed class LocalG2WindowVariant
internal double RightWindowLengthMeters { get; }
}
/// <summary>因最大合法窗口相交而合并的一组曲率事件。</summary>
/// <summary>至少存在一个联合合法生成窗口变体的一组曲率过渡。</summary>
internal sealed class LocalG2SmoothingRegion
{
internal LocalG2SmoothingRegion(
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
+24
View File
@@ -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,
ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
+65 -3
View File
@@ -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 },
ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
+9 -1
View File
@@ -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.'