diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs new file mode 100644 index 0000000..47c798d --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/LateralInterval.cs @@ -0,0 +1,29 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Connected free lateral interval at one exact reference-S station. +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); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs new file mode 100644 index 0000000..eebf384 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridor.cs @@ -0,0 +1,29 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable lateral hard bounds for one already-selected topological corridor. +public sealed class StaticCorridor +{ + public StaticCorridor(IReadOnlyList stations) + { + if (stations == null || stations.Count == 0) + throw new ArgumentException("A static corridor requires stations.", nameof(stations)); + + var copy = new List(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(copy); + } + + public IReadOnlyList Stations { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs new file mode 100644 index 0000000..58aa867 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Corridor/StaticCorridorBuilder.cs @@ -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; + +/// Builds only the seed-connected static free-space corridor of a direction segment. +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 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 seeds, out failureReason)) + return false; + + var stations = new List(); + 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 input, DirectionSegmentView segment, + out List seeds, out string failureReason) + { + seeds = new List(); + 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 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 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 CreateLateralSamples(double seedL, double maximumOffset, double spacing) + { + var samples = new List(); + 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 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 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; } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs new file mode 100644 index 0000000..4d64dd7 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/CorridorChecks.cs @@ -0,0 +1,182 @@ +using System; +using System.Collections.Generic; +using EMPlannerVerificationHost; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle; +using MultiWheelC.TrajectoryPlanning.Mapping; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class CorridorChecks +{ + public static void Run() + { + VerifiesEmptyAndNarrowedCorridors(); + VerifiesSeedConnectedIntervalIsPreserved(); + VerifiesDisappearingSeedIntervalFails(); + } + + private static void VerifiesEmptyAndNarrowedCorridors() + { + DirectionSegmentView segment = CreateStraightSegment(); + CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor; + VehicleParameters vehicle = CreateVehicle(); + var builder = new StaticCorridorBuilder(); + + Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, Array.Empty(), + CreateMap(Array.Empty()), vehicle, configuration, out StaticCorridor empty, out string emptyReason), + "empty map corridor succeeds: " + emptyReason); + VerifyAnchors(empty, 0.2d, 1.8d); + for (int index = 1; index + 1 < empty.Stations.Count; index++) + { + Verification.NearlyEqual(-0.3d, empty.Stations[index].MinimumL, "empty map minimum l"); + Verification.NearlyEqual(0.3d, empty.Stations[index].MaximumL, "empty map maximum l"); + } + + IMapObstacle[] leftNarrowing = + { + new AxisAlignedRectangleObstacle(800f, 1200f, 150f, 400f), + }; + PlanningGridMap narrowedMap = CreateMap(leftNarrowing); + IReadOnlyList seed = CreateSeed(segment, -0.2d); + Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, narrowedMap, vehicle, configuration, + out StaticCorridor narrowed, out string narrowedReason), "narrowed corridor succeeds: " + narrowedReason); + LateralInterval narrowedStation = FindStation(narrowed, 1d); + Verification.True(narrowedStation.MaximumL < 0.3d, "left obstacle narrows positive-l side"); + VerifyAcceptedSamplesUseExactFootprints(segment, narrowed, narrowedMap, vehicle, configuration); + } + + private static void VerifiesSeedConnectedIntervalIsPreserved() + { + DirectionSegmentView segment = CreateStraightSegment(); + CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor; + VehicleParameters vehicle = CreateVehicle(); + IMapObstacle[] split = + { + new AxisAlignedRectangleObstacle(800f, 1200f, -50f, 50f), + }; + IReadOnlyList seed = CreateSeed(segment, -0.2d); + var builder = new StaticCorridorBuilder(); + + Verification.True(builder.TryBuild(segment, 0.2d, 1.8d, seed, CreateMap(split), vehicle, configuration, + out StaticCorridor corridor, out string reason), "split corridor succeeds for left seed: " + reason); + for (int index = 0; index < corridor.Stations.Count; index++) + { + LateralInterval station = corridor.Stations[index]; + Verification.True(station.MinimumL <= station.SeedL && station.SeedL <= station.MaximumL, + "chosen interval contains seed at station " + index); + } + Verification.True(FindStation(corridor, 1d).MaximumL < 0d, + "split station retains seed-connected left interval instead of right interval"); + } + + private static void VerifiesDisappearingSeedIntervalFails() + { + DirectionSegmentView segment = CreateStraightSegment(); + CorridorConfiguration configuration = EmPlannerConfiguration.CreateDefault().Corridor; + VehicleParameters vehicle = CreateVehicle(); + IMapObstacle[] removesLeft = + { + new AxisAlignedRectangleObstacle(800f, 1200f, -400f, -50f), + }; + var builder = new StaticCorridorBuilder(); + + Verification.True(!builder.TryBuild(segment, 0.2d, 1.8d, CreateSeed(segment, -0.2d), CreateMap(removesLeft), + vehicle, configuration, out _, out string failureReason), + "disappearing seed-connected interval does not switch sides"); + Verification.True(failureReason.IndexOf("S=", StringComparison.Ordinal) >= 0, + "failure identifies the first failed reference-s station: " + failureReason); + } + + private static void VerifyAnchors(StaticCorridor corridor, double startS, double endS) + { + Verification.NearlyEqual(startS, corridor.Stations[0].ReferenceS, "first station is exact requested start"); + Verification.NearlyEqual(endS, corridor.Stations[corridor.Stations.Count - 1].ReferenceS, + "last station is exact requested end"); + } + + private static void VerifyAcceptedSamplesUseExactFootprints(DirectionSegmentView segment, StaticCorridor corridor, + PlanningGridMap map, VehicleParameters vehicle, CorridorConfiguration configuration) + { + var checker = new FootprintCollisionChecker(); + for (int stationIndex = 0; stationIndex < corridor.Stations.Count; stationIndex++) + { + LateralInterval station = corridor.Stations[stationIndex]; + FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, station.ReferenceS); + for (double l = station.MinimumL; l <= station.MaximumL + 1e-12d; l += configuration.LateralSampleSpacingMeters) + { + Verification.True(FrenetTransform.TryReconstruct(reference, l, 0d, 0.2d, out Pose2D pose), + "accepted sample reconstructs"); + Verification.True(checker.IsPoseCollisionFree(pose, map, vehicle, configuration.AdditionalClearanceReserveMeters, + out _), "accepted sample passes exact rotated footprint"); + } + } + } + + private static DirectionSegmentView CreateStraightSegment() + { + var points = new List + { + Point(0d, 0d), + Point(1d, 1d), + Point(2d, 2d), + }; + return new DirectionSegmentView(0, TravelDirection.Forward, points, + new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d), + new ReferenceBoundary(0, 2d, EmBoundaryType.Goal, 2d), 0d); + } + + private static IReadOnlyList CreateSeed(DirectionSegmentView segment, double l) + { + return new FrenetProjection[] + { + new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 0.2d), l, 0d, 0d), + new FrenetProjection(ReferencePathInterpolator.Interpolate(segment, 1.8d), l, 0d, 0d), + }; + } + + private static SmoothedPathPoint Point(double x, double s) + { + return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, + false, SmoothedPathPointSource.Anchor); + } + + private static VehicleParameters CreateVehicle() + { + return new VehicleParameters + { + LengthMeters = 0.10d, + WidthMeters = 0.10d, + SafetyMarginMeters = 0d, + MaximumCurvaturePerMeter = 1d, + }; + } + + private static PlanningGridMap CreateMap(IReadOnlyList obstacles) + { + IMapObstacleSource[] sources = obstacles.Count == 0 + ? Array.Empty() + : new IMapObstacleSource[] { new ManualObstacleSource("corridor-test", 1L, true, obstacles) }; + var result = new PlanningMapFactory().Create(new PlanningMapRequest + { + Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f), + ResolutionMm = 20f, + ObstacleSources = sources, + AllowExplicitEmptyMap = obstacles.Count == 0, + }); + Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady, + "corridor test map builds: " + result.FailureReason); + return result.Map!; + } + + private static LateralInterval FindStation(StaticCorridor corridor, double referenceS) + { + for (int index = 0; index < corridor.Stations.Count; index++) + { + if (Math.Abs(corridor.Stations[index].ReferenceS - referenceS) <= 1e-12d) + return corridor.Stations[index]; + } + throw new InvalidOperationException("Requested corridor station was not sampled."); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 79aaf36..2d07268 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -6,29 +6,35 @@ internal static class Program { private static int Main(string[] args) { - if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet")) + if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" && + args[0] != "corridor" && args[0] != "all-foundation")) { - Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet"); + Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|all-foundation"); return 2; } try { - if (args[0] == "foundation") + if (args[0] == "foundation" || args[0] == "all-foundation") { MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run(); Console.WriteLine("PASS foundation"); } - else if (args[0] == "segmentation") + if (args[0] == "segmentation" || args[0] == "all-foundation") { MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run(); Console.WriteLine("PASS segmentation"); } - else + if (args[0] == "frenet" || args[0] == "all-foundation") { MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run(); Console.WriteLine("PASS frenet"); } + if (args[0] == "corridor" || args[0] == "all-foundation") + { + MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run(); + Console.WriteLine("PASS corridor"); + } return 0; } catch (Exception exception)