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

fix: order Local G2 regions back to front

Browse Source
This commit is contained in:
梁薄云
2026-07-31 16:01:37 +08:00
parent 1ac3dbda8d
commit e8c4704d87
3 changed files with 366 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
+245 -1
View File
@@ -561,6 +561,7 @@ internal sealed class LocalG2CandidateBuilder
case "ConstantVelocityCurve": return BuildConstantVelocityCurve();
case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints();
case "GearBoundary": return BuildGearBoundary();
case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder();
default: throw new ArgumentOutOfRangeException(nameof(scenario));
}
}
@@ -572,7 +573,10 @@ internal sealed class LocalG2CandidateBuilder
int outputRegionCount, bool internalConnectionsAreG2, string direction,
bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints,
bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false,
bool gearBoundaryMarkerPreserved = false, bool accepted = false)
bool gearBoundaryMarkerPreserved = false, bool accepted = false,
bool workOrderDescending = false, bool frontArcPreservedAfterBackReplacement = false,
bool bothReplacementsRetained = false, bool forwardOrderRejected = false,
bool deterministicWorkOrder = false, bool invalidWorkOrderRejected = false)
{
CandidateCount = candidateCount;
StartPositionError = startPositionError;
@@ -590,6 +594,12 @@ internal sealed class LocalG2CandidateBuilder
EndpointsAreExact = endpointsAreExact;
GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved;
Accepted = accepted;
WorkOrderDescending = workOrderDescending;
FrontArcPreservedAfterBackReplacement = frontArcPreservedAfterBackReplacement;
BothReplacementsRetained = bothReplacementsRetained;
ForwardOrderRejected = forwardOrderRejected;
DeterministicWorkOrder = deterministicWorkOrder;
InvalidWorkOrderRejected = invalidWorkOrderRejected;
}
public int CandidateCount { get; }
public double StartPositionError { get; }
@@ -607,6 +617,12 @@ internal sealed class LocalG2CandidateBuilder
public bool EndpointsAreExact { get; }
public bool GearBoundaryMarkerPreserved { get; }
public bool Accepted { get; }
public bool WorkOrderDescending { get; }
public bool FrontArcPreservedAfterBackReplacement { get; }
public bool BothReplacementsRetained { get; }
public bool ForwardOrderRejected { get; }
public bool DeterministicWorkOrder { get; }
public bool InvalidWorkOrderRejected { get; }
}
private static CandidateTestSnapshot BuildIsolated()
@@ -815,6 +831,234 @@ internal sealed class LocalG2CandidateBuilder
false, false, false, false, false, preserved);
}
private static CandidateTestSnapshot BuildTwoRegionWorkOrder()
{
var points = new List<SmoothingPoint2D>();
for (int index = 0; index <= 12; index++)
{
double arc = index * 0.25d;
points.Add(new SmoothingPoint2D(
arc,
0d,
arc,
0d,
0d,
1d,
false,
SmoothedPathPointSource.Anchor));
}
var segment = new PreparedDirectionSegment(
0,
TravelDirection.Forward,
points,
false,
false);
var original = new PreparedPath(new[] { segment });
LocalG2SmoothingRegion frontRegion = CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
LocalG2SmoothingRegion backRegion = CreateOrderedRegion(2.25d, 2.0d, 2.5d, 1);
var reportOrder = new[] { frontRegion, backRegion };
var orderer = new LocalG2RegionWorkOrder();
if (!orderer.TryCreate(
reportOrder,
out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
out string orderReason))
{
throw new InvalidOperationException(orderReason);
}
if (!orderer.TryCreate(
reportOrder,
out IReadOnlyList<LocalG2SmoothingRegion> repeatedOrder,
out string repeatedReason))
{
throw new InvalidOperationException(repeatedReason);
}
bool descending =
ReferenceEquals(backRegion, workOrder[0]) &&
ReferenceEquals(frontRegion, workOrder[1]);
bool deterministic =
ReferenceEquals(workOrder[0], repeatedOrder[0]) &&
ReferenceEquals(workOrder[1], repeatedOrder[1]) &&
ReferenceEquals(frontRegion, reportOrder[0]) &&
ReferenceEquals(backRegion, reportOrder[1]);
var wrongSegmentTransition = new CurvatureTransition(
1,
0,
1,
0.75d,
0.75d,
0d,
0d,
0d,
0.4d);
var invalidRegion = new LocalG2SmoothingRegion(
0,
new[] { wrongSegmentTransition },
0.5d,
1.0d,
new[]
{
new LocalG2WindowVariant(0, 0.5d, 1.0d, 0.25d, 0.25d),
});
bool invalidWorkOrderRejected =
!orderer.TryCreate(
new[] { invalidRegion },
out _,
out _) &&
!orderer.TryCreate(
new LocalG2SmoothingRegion[] { null },
out _,
out _);
LocalG2CandidateGeometry frontCandidate =
CreateLengthChangingCandidate(0, 0.5d, 1.0d);
LocalG2CandidateGeometry backCandidate =
CreateLengthChangingCandidate(1, 2.0d, 2.5d);
var splicer = new LocalG2PathSplicer();
if (!splicer.TryReplace(
original,
backCandidate,
out PreparedPath afterBack,
out string backReason))
{
throw new InvalidOperationException(backReason);
}
bool frontArcPreserved =
PathReferenceInterpolator.TryInterpolateByArcLength(
afterBack.Segments[0].Points,
0.5d,
out SmoothingPoint2D frontStart,
out _) &&
PathReferenceInterpolator.TryInterpolateByArcLength(
afterBack.Segments[0].Points,
1.0d,
out SmoothingPoint2D frontEnd,
out _) &&
Math.Abs(frontStart.X - 0.5d) <= 1e-12d &&
Math.Abs(frontEnd.X - 1.0d) <= 1e-12d;
if (!splicer.TryReplace(
afterBack,
frontCandidate,
out PreparedPath afterBoth,
out string frontReason))
{
throw new InvalidOperationException(frontReason);
}
int localG2PointCount = 0;
for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++)
{
if (afterBoth.Segments[0].Points[index].Source ==
SmoothedPathPointSource.LocalG2Transition)
{
localG2PointCount++;
}
}
if (!splicer.TryReplace(
original,
frontCandidate,
out PreparedPath afterFront,
out string firstReason))
{
throw new InvalidOperationException(firstReason);
}
bool forwardOrderRejected = !splicer.TryReplace(
afterFront,
backCandidate,
out _,
out _);
return new CandidateTestSnapshot(
0,
0d,
0d,
0d,
0d,
false,
0,
false,
string.Empty,
false,
false,
false,
false,
false,
false,
false,
descending,
frontArcPreserved,
localG2PointCount >= 2,
forwardOrderRejected,
deterministic,
invalidWorkOrderRejected);
}
private static LocalG2SmoothingRegion CreateOrderedRegion(
double eventArc,
double startArc,
double endArc,
int index)
{
var transition = new CurvatureTransition(
0,
index,
index + 1,
eventArc,
eventArc,
0d,
0d,
0d,
0.4d);
return new LocalG2SmoothingRegion(
0,
new[] { transition },
startArc,
endArc,
new[]
{
new LocalG2WindowVariant(
0,
startArc,
endArc,
eventArc - startArc,
endArc - eventArc),
});
}
private static LocalG2CandidateGeometry CreateLengthChangingCandidate(
int candidateIndex,
double startArc,
double endArc)
{
double middleArc = 0.5d * (startArc + endArc);
var points = new[]
{
Point(startArc, 0d, startArc, false),
Point(middleArc, 0.20d, middleArc, false),
Point(endArc, 0d, endArc, false),
};
return new LocalG2CandidateGeometry(
candidateIndex,
0,
startArc,
endArc,
middleArc - startArc,
endArc - middleArc,
points,
0d,
0d,
0d,
0d,
true);
}
private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature)
{
double heading = direction == TravelDirection.Forward ? 0d : Math.PI;
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs
+107
View File
@@ -0,0 +1,107 @@
using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.Utils;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
internal sealed class LocalG2RegionWorkOrder
{
internal bool TryCreate(
IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
out string reason)
{
workOrder = Empty();
reason = string.Empty;
if (reportOrder == null)
{
reason = "局部 G2 区域工作顺序输入无效。";
return false;
}
var indexed = new List<IndexedRegion>(reportOrder.Count);
for (int index = 0; index < reportOrder.Count; index++)
{
LocalG2SmoothingRegion region = reportOrder[index];
if (!TryValidate(region, out double firstArc, out reason))
return false;
indexed.Add(new IndexedRegion(region, index, firstArc));
}
indexed.Sort(Compare);
var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
for (int index = 0; index < indexed.Count; index++)
ordered.Add(indexed[index].Region);
workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
return true;
}
private static bool TryValidate(
LocalG2SmoothingRegion region,
out double firstArc,
out string reason)
{
firstArc = 0d;
reason = string.Empty;
if (region == null || region.SegmentIndex < 0 ||
region.Transitions == null || region.Transitions.Count == 0 ||
region.WindowVariants == null || region.WindowVariants.Count == 0)
{
reason = "局部 G2 工作顺序包含空区域或空窗口集。";
return false;
}
double previousArc = -1d;
for (int index = 0; index < region.Transitions.Count; index++)
{
CurvatureTransition transition = region.Transitions[index];
if (transition == null ||
transition.SegmentIndex != region.SegmentIndex ||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
transition.LocalArcLengthMeters < previousArc)
{
reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
return false;
}
previousArc = transition.LocalArcLengthMeters;
}
firstArc = region.Transitions[0].LocalArcLengthMeters;
return true;
}
private static int Compare(IndexedRegion left, IndexedRegion right)
{
int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
if (segment != 0) return segment;
int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
return descendingArc != 0
? descendingArc
: left.ReportIndex.CompareTo(right.ReportIndex);
}
private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
{
return new ReadOnlyCollection<LocalG2SmoothingRegion>(
new List<LocalG2SmoothingRegion>());
}
private sealed class IndexedRegion
{
internal IndexedRegion(
LocalG2SmoothingRegion region,
int reportIndex,
double firstArc)
{
Region = region;
ReportIndex = reportIndex;
FirstArc = firstArc;
}
internal LocalG2SmoothingRegion Region { get; }
internal int ReportIndex { get; }
internal double FirstArc { get; }
}
}
ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
+14
View File
@@ -74,6 +74,20 @@ $gearBoundary = Invoke-Scenario 'GearBoundary'
Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
$twoRegions = Invoke-Scenario 'TwoRegionWorkOrder'
Assert-True $twoRegions.WorkOrderDescending `
'Same-segment regions must be processed from larger original local arc to smaller local arc.'
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.'
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.InvalidWorkOrderRejected `
'Work ordering must reject null regions and cross-segment event contents.'
$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.'