diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs index 8a6c6f9..14e52d6 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Collections.ObjectModel; using System.Drawing; using System.Globalization; using ClumsyCore; @@ -36,6 +37,46 @@ public static class TrajectoryObservationPresentationText } } +public sealed class TrajectoryObservationLsPresentationPoint +{ + public TrajectoryObservationLsPresentationPoint(double horizontalPathS, double verticalLateralOffset) + { + HorizontalPathS = horizontalPathS; + VerticalLateralOffset = verticalLateralOffset; + } + + public double HorizontalPathS { get; } + + public double VerticalLateralOffset { get; } +} + +public sealed class TrajectoryObservationLsPresentationModel +{ + private TrajectoryObservationLsPresentationModel(IReadOnlyList samples) + { + Samples = new ReadOnlyCollection( + new List(samples)); + } + + public string HorizontalAxisLabel => "path-S (m)"; + + public string VerticalAxisLabel => "lateral offset (m)"; + + public IReadOnlyList Samples { get; } + + public static TrajectoryObservationLsPresentationModel Create(TrajectoryObservationCharts charts) + { + if (charts == null) throw new ArgumentNullException(nameof(charts)); + var samples = new List(charts.LsSamples.Count); + for (int index = 0; index < charts.LsSamples.Count; index++) + { + TrajectoryObservationLsSample sample = charts.LsSamples[index]; + samples.Add(new TrajectoryObservationLsPresentationPoint(sample.PathS, sample.LateralOffset)); + } + return new TrajectoryObservationLsPresentationModel(samples); + } +} + public sealed class TrajectoryObservationPresentation { private const float MillimetersPerMeter = 1000f; @@ -82,17 +123,21 @@ public sealed class TrajectoryObservationPresentation return; } + TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts); float maxS = MaximumPathS(charts.LsSamples); float maxL = MaximumLateralOffset(charts.LsSamples); - lsPainter.DrawLine(Color.Gainsboro, -maxL, 0f, maxL, 0f, width: 2); - lsPainter.DrawLine(Color.Gainsboro, 0f, 0f, 0f, maxS, width: 2); - lsPainter.DrawText(Color.White, "L-S (lateral offset / path-S)", -maxL, maxS + 150f); - for (int index = 1; index < charts.LsSamples.Count; index++) + lsPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxS, 0f, width: 2); + lsPainter.DrawLine(Color.Gainsboro, 0f, -maxL, 0f, maxL, width: 2); + lsPainter.DrawText(Color.White, "S-L", maxS + 100f, maxL + 150f); + lsPainter.DrawText(Color.White, model.HorizontalAxisLabel, maxS + 100f, -200f); + lsPainter.DrawText(Color.White, model.VerticalAxisLabel, 100f, maxL + 300f); + for (int index = 1; index < model.Samples.Count; index++) { - TrajectoryObservationLsSample previous = charts.LsSamples[index - 1]; - TrajectoryObservationLsSample current = charts.LsSamples[index]; - lsPainter.DrawLine(Color.MediumPurple, ToMillimeters(previous.LateralOffset), ToMillimeters(previous.PathS), - ToMillimeters(current.LateralOffset), ToMillimeters(current.PathS), width: 3); + TrajectoryObservationLsPresentationPoint previous = model.Samples[index - 1]; + TrajectoryObservationLsPresentationPoint current = model.Samples[index]; + lsPainter.DrawLine(Color.MediumPurple, ToMillimeters(previous.HorizontalPathS), + ToMillimeters(previous.VerticalLateralOffset), ToMillimeters(current.HorizontalPathS), + ToMillimeters(current.VerticalLateralOffset), width: 3); } } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 3c83ba5..3c57f92 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -18,6 +18,7 @@ internal static class TrajectoryObservationChecks RejectsObstacleOutsideConfiguredBounds(); VerifiesLsAndStUsePublishedTrajectoryData(); VerifiesPresentationTextDescribesObservationWithoutSendingCommand(); + VerifiesLsPresentationUsesPathSOnHorizontalAxis(); VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot(); FreezesBootstrapVehicleForRollingRequests(); } @@ -155,6 +156,22 @@ internal static class TrajectoryObservationChecks "observer presentation LS projection failures"); } + private static void VerifiesLsPresentationUsesPathSOnHorizontalAxis() + { + TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build( + CreatePublishedTrajectory(new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero)), + CreateStraightSegment(), 0.5d); + + TrajectoryObservationLsPresentationModel model = TrajectoryObservationLsPresentationModel.Create(charts); + + Verification.Equal("path-S (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label"); + Verification.Equal("lateral offset (m)", model.VerticalAxisLabel, "observer LS vertical axis label"); + Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS, + "observer LS path-S is horizontal"); + Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset, + "observer LS lateral offset is vertical"); + } + private static void FreezesBootstrapVehicleForRollingRequests() { DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);