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

fix: cover Local G2 window targets before splits

Browse Source
This commit is contained in:
梁薄云
2026-08-01 00:20:15 +08:00
parent 144a0883b2
commit bd08a9bac6
2 changed files with 69 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs
+60 -10
View File
@@ -106,12 +106,25 @@ internal sealed class LocalG2WindowPlanner
double firstEvent = transitions[0].LocalArcLengthMeters;
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
double anchor = (firstEvent + lastEvent) / 2d;
foreach (double target in BuildTargets(options, segmentLength))
IReadOnlyList<double> targets = BuildTargets(options, segmentLength);
double[] ratios = { 0.5d, 0.4d, 0.6d };
for (int ratioIndex = 0; ratioIndex < ratios.Length; ratioIndex++)
{
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options);
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options);
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options);
for (int targetIndex = 0; targetIndex < targets.Count; targetIndex++)
{
if (variants.Count >= options.MaximumCandidatesPerRegion)
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
AddIfLegal(
variants,
targets[targetIndex],
ratios[ratioIndex],
anchor,
firstEvent,
lastEvent,
segmentLength,
ratioIndex == 0,
options);
}
}
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
}
@@ -160,10 +173,10 @@ internal sealed class LocalG2WindowPlanner
double[] requested =
{
options.PreferredWindowLengthMeters,
0.75d * options.PreferredWindowLengthMeters,
1.25d * options.PreferredWindowLengthMeters,
options.MinimumWindowLengthMeters,
options.MaximumWindowLengthMeters,
0.75d * options.PreferredWindowLengthMeters,
1.25d * options.PreferredWindowLengthMeters,
};
var targets = new List<double>(requested.Length);
for (int index = 0; index < requested.Length; index++)
@@ -262,6 +275,10 @@ internal sealed class LocalG2WindowPlanner
transitions = new[] { Transition(0.1d, 0) };
segmentLength = 1d;
break;
case "InteriorCoverage":
transitions = new[] { Transition(1d, 0) };
segmentLength = 2d;
break;
default:
throw new ArgumentOutOfRangeException(nameof(scenario));
}
@@ -305,7 +322,25 @@ internal sealed class LocalG2WindowPlanner
exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
}
LocalG2WindowVariant first = regions[0].WindowVariants[0];
IReadOnlyList<LocalG2WindowVariant> firstVariants = regions[0].WindowVariants;
bool representativeTargetsFirst =
firstVariants.Count >= 3 &&
Math.Abs(WindowLength(firstVariants[0]) - 0.50d) <= 1e-9d &&
Math.Abs(WindowLength(firstVariants[1]) - 0.20d) <= 1e-9d &&
Math.Abs(WindowLength(firstVariants[2]) - 0.80d) <= 1e-9d;
bool hasAsymmetricVariant = false;
for (int index = 0; index < firstVariants.Count; index++)
{
if (Math.Abs(
firstVariants[index].LeftWindowLengthMeters -
firstVariants[index].RightWindowLengthMeters) > 1e-9d)
{
hasAsymmetricVariant = true;
break;
}
}
LocalG2WindowVariant first = firstVariants[0];
return new WindowPlanningTestSnapshot(
regions.Count,
string.Join(",", counts),
@@ -313,7 +348,10 @@ internal sealed class LocalG2WindowPlanner
exactEnvelope,
first.LeftWindowLengthMeters,
first.RightWindowLengthMeters,
string.Join("|", signature));
string.Join("|", signature),
firstVariants.Count,
representativeTargetsFirst,
hasAsymmetricVariant);
}
public sealed class WindowPlanningTestSnapshot
@@ -325,7 +363,10 @@ internal sealed class LocalG2WindowPlanner
bool exactEnvelope,
double firstLeftLength,
double firstRightLength,
string signature)
string signature,
int firstRegionVariantCount,
bool representativeTargetsFirst,
bool hasAsymmetricVariant)
{
RegionCount = regionCount;
TransitionCounts = transitionCounts;
@@ -334,6 +375,9 @@ internal sealed class LocalG2WindowPlanner
FirstLeftLength = firstLeftLength;
FirstRightLength = firstRightLength;
Signature = signature;
FirstRegionVariantCount = firstRegionVariantCount;
RepresentativeTargetsFirst = representativeTargetsFirst;
HasAsymmetricVariant = hasAsymmetricVariant;
}
public int RegionCount { get; }
@@ -343,8 +387,14 @@ internal sealed class LocalG2WindowPlanner
public double FirstLeftLength { get; }
public double FirstRightLength { get; }
public string Signature { get; }
public int FirstRegionVariantCount { get; }
public bool RepresentativeTargetsFirst { get; }
public bool HasAsymmetricVariant { get; }
}
private static double WindowLength(LocalG2WindowVariant variant) =>
variant.EndArcLengthMeters - variant.StartArcLengthMeters;
private static CurvatureTransition Transition(double arcLength, int index)
{
return new CurvatureTransition(