feat: build static EM lateral corridors
This commit is contained in:
梁薄云
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Connected free lateral interval at one exact reference-S station.</summary>
|
||||
public sealed class LateralInterval
|
||||
{
|
||||
public LateralInterval(double referenceS, double minimumL, double maximumL, double seedL)
|
||||
{
|
||||
if (!IsFinite(referenceS) || !IsFinite(minimumL) || !IsFinite(maximumL) || !IsFinite(seedL) ||
|
||||
minimumL > maximumL || seedL < minimumL - 1e-12d || seedL > maximumL + 1e-12d)
|
||||
throw new ArgumentOutOfRangeException(nameof(referenceS));
|
||||
|
||||
ReferenceS = referenceS;
|
||||
MinimumL = minimumL;
|
||||
MaximumL = maximumL;
|
||||
SeedL = seedL;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double MinimumL { get; }
|
||||
public double MaximumL { get; }
|
||||
public double SeedL { get; }
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable lateral hard bounds for one already-selected topological corridor.</summary>
|
||||
public sealed class StaticCorridor
|
||||
{
|
||||
public StaticCorridor(IReadOnlyList<LateralInterval> stations)
|
||||
{
|
||||
if (stations == null || stations.Count == 0)
|
||||
throw new ArgumentException("A static corridor requires stations.", nameof(stations));
|
||||
|
||||
var copy = new List<LateralInterval>(stations.Count);
|
||||
double previousS = double.NegativeInfinity;
|
||||
for (int index = 0; index < stations.Count; index++)
|
||||
{
|
||||
LateralInterval station = stations[index];
|
||||
if (station == null || station.ReferenceS < previousS)
|
||||
throw new ArgumentException("Static corridor stations must be non-null and sorted.", nameof(stations));
|
||||
copy.Add(station);
|
||||
previousS = station.ReferenceS;
|
||||
}
|
||||
Stations = new ReadOnlyCollection<LateralInterval>(copy);
|
||||
}
|
||||
|
||||
public IReadOnlyList<LateralInterval> Stations { get; }
|
||||
}
|
||||
@@ -0,0 +1,289 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Builds only the seed-connected static free-space corridor of a direction segment.</summary>
|
||||
public sealed class StaticCorridorBuilder
|
||||
{
|
||||
private const double Epsilon = 1e-12d;
|
||||
private const double ReconstructionDenominator = 1e-12d;
|
||||
|
||||
private readonly FootprintCollisionChecker _collisionChecker;
|
||||
|
||||
public StaticCorridorBuilder()
|
||||
: this(new FootprintCollisionChecker())
|
||||
{
|
||||
}
|
||||
|
||||
public StaticCorridorBuilder(FootprintCollisionChecker collisionChecker)
|
||||
{
|
||||
_collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
||||
}
|
||||
|
||||
public bool TryBuild(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
|
||||
IReadOnlyList<FrenetProjection> seed, PlanningGridMap map, VehicleParameters vehicle,
|
||||
CorridorConfiguration configuration, out StaticCorridor corridor, out string failureReason)
|
||||
{
|
||||
corridor = null;
|
||||
failureReason = string.Empty;
|
||||
if (!TryValidateInput(segment, startReferenceS, endReferenceS, map, vehicle, configuration, out failureReason))
|
||||
return false;
|
||||
if (!TryReadSeeds(seed, segment, out List<SeedSample> seeds, out failureReason))
|
||||
return false;
|
||||
|
||||
var stations = new List<LateralInterval>();
|
||||
LateralInterval previous = null;
|
||||
foreach (double referenceS in CreateStations(startReferenceS, endReferenceS,
|
||||
configuration.LongitudinalSampleSpacingMeters))
|
||||
{
|
||||
FrenetReferencePoint reference;
|
||||
try
|
||||
{
|
||||
reference = ReferencePathInterpolator.Interpolate(segment, referenceS);
|
||||
}
|
||||
catch (ArgumentException)
|
||||
{
|
||||
failureReason = "Reference interpolation failed at S=" + referenceS + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
double seedL = GetSeedL(seeds, referenceS);
|
||||
if (seedL < -configuration.MaximumLateralOffsetMeters - Epsilon ||
|
||||
seedL > configuration.MaximumLateralOffsetMeters + Epsilon)
|
||||
{
|
||||
failureReason = "Seed lateral offset is outside configured bounds at S=" + referenceS + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TrySelectSeedConnectedInterval(reference, seedL, map, vehicle, configuration,
|
||||
out double minimumL, out double maximumL))
|
||||
{
|
||||
failureReason = "Seed-connected free interval disappeared at S=" + referenceS + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
var selected = new LateralInterval(referenceS, minimumL, maximumL, seedL);
|
||||
if (previous != null && Math.Max(previous.MinimumL, selected.MinimumL) >
|
||||
Math.Min(previous.MaximumL, selected.MaximumL) + Epsilon)
|
||||
{
|
||||
failureReason = "Seed-connected corridor loses overlap at S=" + referenceS + ".";
|
||||
return false;
|
||||
}
|
||||
|
||||
stations.Add(selected);
|
||||
previous = selected;
|
||||
}
|
||||
|
||||
corridor = new StaticCorridor(stations);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryValidateInput(DirectionSegmentView segment, double startReferenceS, double endReferenceS,
|
||||
PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration, out string failureReason)
|
||||
{
|
||||
failureReason = string.Empty;
|
||||
if (segment == null || map == null || vehicle == null || configuration == null || !map.PlanningReady)
|
||||
{
|
||||
failureReason = "A planning-ready map, vehicle, segment, and corridor configuration are required.";
|
||||
return false;
|
||||
}
|
||||
if (!IsFinite(startReferenceS) || !IsFinite(endReferenceS) || startReferenceS < 0d ||
|
||||
endReferenceS < startReferenceS || endReferenceS > segment.LengthMeters + Epsilon)
|
||||
{
|
||||
failureReason = "Requested corridor S anchors are invalid.";
|
||||
return false;
|
||||
}
|
||||
if (!IsPositiveFinite(configuration.LongitudinalSampleSpacingMeters) ||
|
||||
!IsPositiveFinite(configuration.LateralSampleSpacingMeters) ||
|
||||
!IsFinite(configuration.MaximumLateralOffsetMeters) || configuration.MaximumLateralOffsetMeters < 0d ||
|
||||
!IsFinite(configuration.AdditionalClearanceReserveMeters) || configuration.AdditionalClearanceReserveMeters < 0d ||
|
||||
!IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters))
|
||||
{
|
||||
failureReason = "Corridor configuration is invalid.";
|
||||
return false;
|
||||
}
|
||||
if (!IsPositiveFinite(vehicle.LengthMeters) || !IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d)
|
||||
{
|
||||
failureReason = "Vehicle footprint dimensions are invalid.";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryReadSeeds(IReadOnlyList<FrenetProjection> input, DirectionSegmentView segment,
|
||||
out List<SeedSample> seeds, out string failureReason)
|
||||
{
|
||||
seeds = new List<SeedSample>();
|
||||
failureReason = string.Empty;
|
||||
if (input == null)
|
||||
return true;
|
||||
|
||||
for (int index = 0; index < input.Count; index++)
|
||||
{
|
||||
FrenetProjection projection = input[index];
|
||||
if (projection == null || projection.ReferencePoint == null ||
|
||||
projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon ||
|
||||
!IsFinite(projection.LateralOffset))
|
||||
{
|
||||
failureReason = "Corridor seeds must belong to the selected direction segment.";
|
||||
return false;
|
||||
}
|
||||
seeds.Add(new SeedSample(projection.ReferenceS, projection.LateralOffset));
|
||||
}
|
||||
seeds.Sort((left, right) => left.ReferenceS.CompareTo(right.ReferenceS));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IEnumerable<double> CreateStations(double startReferenceS, double endReferenceS, double spacing)
|
||||
{
|
||||
yield return startReferenceS;
|
||||
for (double candidate = startReferenceS + spacing; candidate < endReferenceS - Epsilon; candidate += spacing)
|
||||
yield return candidate;
|
||||
if (endReferenceS > startReferenceS + Epsilon)
|
||||
yield return endReferenceS;
|
||||
}
|
||||
|
||||
private bool TrySelectSeedConnectedInterval(FrenetReferencePoint reference, double seedL, PlanningGridMap map,
|
||||
VehicleParameters vehicle, CorridorConfiguration configuration, out double minimumL, out double maximumL)
|
||||
{
|
||||
minimumL = 0d;
|
||||
maximumL = 0d;
|
||||
List<double> samples = CreateLateralSamples(seedL, configuration.MaximumLateralOffsetMeters,
|
||||
configuration.LateralSampleSpacingMeters);
|
||||
int index = 0;
|
||||
while (index < samples.Count)
|
||||
{
|
||||
if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
|
||||
{
|
||||
index++;
|
||||
continue;
|
||||
}
|
||||
|
||||
double groupMinimum = samples[index];
|
||||
double groupMaximum = samples[index];
|
||||
bool containsSeed = Math.Abs(samples[index] - seedL) <= Epsilon;
|
||||
index++;
|
||||
while (index < samples.Count && samples[index] - groupMaximum <= configuration.LateralSampleSpacingMeters + Epsilon)
|
||||
{
|
||||
if (!IsCollisionFree(reference, samples[index], map, vehicle, configuration))
|
||||
{
|
||||
index++;
|
||||
break;
|
||||
}
|
||||
groupMaximum = samples[index];
|
||||
containsSeed |= Math.Abs(samples[index] - seedL) <= Epsilon;
|
||||
index++;
|
||||
}
|
||||
|
||||
if (containsSeed)
|
||||
{
|
||||
minimumL = groupMinimum;
|
||||
maximumL = groupMaximum;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static List<double> CreateLateralSamples(double seedL, double maximumOffset, double spacing)
|
||||
{
|
||||
var samples = new List<double>();
|
||||
for (double candidate = -maximumOffset; candidate < maximumOffset - Epsilon; candidate += spacing)
|
||||
AddSortedUnique(samples, candidate);
|
||||
AddSortedUnique(samples, maximumOffset);
|
||||
AddSortedUnique(samples, -maximumOffset);
|
||||
AddSortedUnique(samples, seedL);
|
||||
samples.Sort();
|
||||
return samples;
|
||||
}
|
||||
|
||||
private bool IsCollisionFree(FrenetReferencePoint reference, double lateralOffset, PlanningGridMap map,
|
||||
VehicleParameters vehicle, CorridorConfiguration configuration)
|
||||
{
|
||||
if (!FrenetTransform.TryReconstruct(reference, lateralOffset, 0d, ReconstructionDenominator, out Pose2D pose))
|
||||
return false;
|
||||
|
||||
if (IsObviouslyClear(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters))
|
||||
return true;
|
||||
return _collisionChecker.IsPoseCollisionFree(pose, map, vehicle,
|
||||
configuration.AdditionalClearanceReserveMeters, out _);
|
||||
}
|
||||
|
||||
private static bool IsObviouslyClear(Pose2D pose, PlanningGridMap map, VehicleParameters vehicle,
|
||||
double additionalClearanceReserveMeters)
|
||||
{
|
||||
double totalMargin = vehicle.SafetyMarginMeters + additionalClearanceReserveMeters;
|
||||
double halfLength = vehicle.LengthMeters / 2d + totalMargin;
|
||||
double halfWidth = vehicle.WidthMeters / 2d + totalMargin;
|
||||
double radius = Math.Sqrt(halfLength * halfLength + halfWidth * halfWidth);
|
||||
if (!IsFinite(radius) || map.GetConservativeObstacleDistanceMeters(pose.X, pose.Y) <= radius)
|
||||
return false;
|
||||
|
||||
double cosine = Math.Cos(pose.Heading);
|
||||
double sine = Math.Sin(pose.Heading);
|
||||
for (int longitudinalSign = -1; longitudinalSign <= 1; longitudinalSign += 2)
|
||||
for (int lateralSign = -1; lateralSign <= 1; lateralSign += 2)
|
||||
{
|
||||
double x = pose.X + longitudinalSign * halfLength * cosine - lateralSign * halfWidth * sine;
|
||||
double y = pose.Y + longitudinalSign * halfLength * sine + lateralSign * halfWidth * cosine;
|
||||
if (!map.TryWorldToGrid(x, y, out _, out _))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static double GetSeedL(IReadOnlyList<SeedSample> seeds, double referenceS)
|
||||
{
|
||||
if (seeds.Count == 0)
|
||||
return 0d;
|
||||
if (referenceS <= seeds[0].ReferenceS)
|
||||
return seeds[0].LateralOffset;
|
||||
for (int index = 1; index < seeds.Count; index++)
|
||||
{
|
||||
SeedSample upper = seeds[index];
|
||||
if (referenceS <= upper.ReferenceS)
|
||||
{
|
||||
SeedSample lower = seeds[index - 1];
|
||||
double span = upper.ReferenceS - lower.ReferenceS;
|
||||
return span <= Epsilon ? upper.LateralOffset : lower.LateralOffset +
|
||||
(upper.LateralOffset - lower.LateralOffset) * (referenceS - lower.ReferenceS) / span;
|
||||
}
|
||||
}
|
||||
return seeds[seeds.Count - 1].LateralOffset;
|
||||
}
|
||||
|
||||
private static void AddSortedUnique(List<double> samples, double value)
|
||||
{
|
||||
for (int index = 0; index < samples.Count; index++)
|
||||
if (Math.Abs(samples[index] - value) <= Epsilon)
|
||||
return;
|
||||
samples.Add(value);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private sealed class SeedSample
|
||||
{
|
||||
public SeedSample(double referenceS, double lateralOffset)
|
||||
{
|
||||
ReferenceS = referenceS;
|
||||
LateralOffset = lateralOffset;
|
||||
}
|
||||
|
||||
public double ReferenceS { get; }
|
||||
public double LateralOffset { get; }
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user