feat: visualize EM observation diagnostics

2026-08-04 16:21:17 +08:00
parent 3a23fc2fe3
commit 08f6603cad
2 changed files with 299 additions and 0 deletions
@@ -0,0 +1,267 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Globalization;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using MDCSToolBox.Clumsy.Pilot;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+public static class TrajectoryObservationPresentationText
+{
+    public static string Create(TrajectoryObservationObservation observation, TrajectoryObservationCharts charts)
+    {
+        string projectionFailures = charts == null
+            ? "unavailable"
+            : charts.FailedProjectionCount.ToString(CultureInfo.InvariantCulture);
+        if (observation == null || observation.SelectedPoint == null)
+        {
+            return "OBSERVE_ONLY: no chassis command is sent.\n" +
+                "selected trajectory point unavailable\n" +
+                "LS projection failures=" + projectionFailures;
+        }
+
+        EmTrajectoryPoint point = observation.SelectedPoint;
+        return "OBSERVE_ONLY: no chassis command is sent.\n" +
+            "selected t=" + point.TimeFromStart.ToString("F2", CultureInfo.InvariantCulture) +
+            " s, path-S=" + point.PathS.ToString("F2", CultureInfo.InvariantCulture) + " m\n" +
+            "signed speed=" + point.SignedLongitudinalVelocity.ToString("F2", CultureInfo.InvariantCulture) +
+            " m/s, yaw rate=" + point.YawRate.ToString("F2", CultureInfo.InvariantCulture) + " rad/s\n" +
+            "LS projection failures=" + projectionFailures;
+    }
+}
+
+public sealed class TrajectoryObservationPresentation
+{
+    private const float MillimetersPerMeter = 1000f;
+    private const int MaximumVisibleGridLines = 80;
+    private const float VelocitySeriesBaseMillimeters = -2500f;
+
+    private readonly Painter worldPainter = UI.GetPainter("TrajectoryObserver.World", true);
+    private readonly Painter lsPainter = UI.GetPainter("TrajectoryObserver.LS", true);
+    private readonly Painter stPainter = UI.GetPainter("TrajectoryObserver.ST", true);
+
+    public void DrawWorld(TrajectoryObservationBootstrapResult bootstrap,
+        TrajectoryObservationObservation observation)
+    {
+        worldPainter.Clear();
+        if (bootstrap == null)
+        {
+            worldPainter.DrawText(Color.LightYellow, "Trajectory bootstrap unavailable.", 0f, 0f);
+            return;
+        }
+
+        DrawMap(bootstrap.Map);
+        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, Color.LimeGreen, "start");
+        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, Color.Orange, "goal");
+        DrawCoarsePath(bootstrap.CoarseResult == null ? null : bootstrap.CoarseResult.PlanningResult.Path);
+        DrawLocalG2Path(bootstrap.SmoothedPath == null ? null : bootstrap.SmoothedPath.Path);
+        DrawPose(observation == null || observation.VehicleState == null ? null : observation.VehicleState.Pose,
+            Color.DeepSkyBlue, "real pose");
+
+        EmTrajectory trajectory = observation == null ? null : observation.PublishedTrajectory;
+        if (trajectory == null || trajectory.Points == null || trajectory.Points.Count == 0)
+        {
+            worldPainter.DrawText(Color.LightYellow, "No published trajectory available.", 0f, 0f);
+            return;
+        }
+        DrawEmPath(trajectory.Points);
+    }
+
+    public void DrawLs(TrajectoryObservationCharts charts)
+    {
+        lsPainter.Clear();
+        if (charts == null || charts.LsSamples == null || charts.LsSamples.Count == 0)
+        {
+            lsPainter.DrawText(Color.LightYellow, "No published trajectory available for L-S chart.", 0f, 0f);
+            return;
+        }
+
+        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++)
+        {
+            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);
+        }
+    }
+
+    public void DrawSt(TrajectoryObservationCharts charts)
+    {
+        stPainter.Clear();
+        if (charts == null || charts.StSamples == null || charts.StSamples.Count == 0)
+        {
+            stPainter.DrawText(Color.LightYellow, "No published trajectory available for T-S/T-V charts.", 0f, 0f);
+            return;
+        }
+
+        float maxTime = MaximumTime(charts.StSamples, charts.SpeedSamples);
+        float maxS = MaximumPathS(charts.StSamples);
+        stPainter.DrawLine(Color.Gainsboro, 0f, 0f, maxTime, 0f, width: 2);
+        stPainter.DrawLine(Color.Gainsboro, 0f, 0f, 0f, maxS, width: 2);
+        stPainter.DrawText(Color.White, "T-S", 0f, maxS + 150f);
+        DrawStSeries(charts.StSamples);
+
+        stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters,
+            maxTime, VelocitySeriesBaseMillimeters, width: 2);
+        stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters, 0f,
+            VelocitySeriesBaseMillimeters + MillimetersPerMeter, width: 2);
+        stPainter.DrawText(Color.White, "T-V", 0f, VelocitySeriesBaseMillimeters - 200f);
+        DrawSpeedSeries(charts.SpeedSamples);
+        DrawStLegend(maxTime, maxS);
+    }
+
+    public void ClearAll()
+    {
+        worldPainter.Clear();
+        lsPainter.Clear();
+        stPainter.Clear();
+    }
+
+    private void DrawMap(PlanningGridMap map)
+    {
+        if (map == null) return;
+        float xMin = map.Bounds.XMin;
+        float xMax = map.Bounds.XMax;
+        float yMin = map.Bounds.YMin;
+        float yMax = map.Bounds.YMax;
+        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) /
+            (double)MaximumVisibleGridLines));
+        for (int column = 0; column <= map.Cols; column += gridStride)
+        {
+            float x = Math.Min(xMax, xMin + column * map.ResolutionMm);
+            worldPainter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
+        }
+        for (int row = 0; row <= map.Rows; row += gridStride)
+        {
+            float y = Math.Min(yMax, yMin + row * map.ResolutionMm);
+            worldPainter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
+        }
+        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
+        for (int row = 0; row < map.Rows; row++)
+        {
+            for (int column = 0; column < map.Cols; column++)
+            {
+                if (!map.IsOccupied(row, column)) continue;
+                float x = xMin + column * map.ResolutionMm + map.ResolutionMm / 2f;
+                float y = yMin + row * map.ResolutionMm;
+                worldPainter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + map.ResolutionMm,
+                    width: Math.Max(1, (int)Math.Round(map.ResolutionMm)));
+            }
+        }
+    }
+
+    private void DrawPose(Pose2D pose, Color color, string label)
+    {
+        if (pose == null) return;
+        float x = ToMillimeters(pose.X);
+        float y = ToMillimeters(pose.Y);
+        worldPainter.DrawCircle(color, x, y, 80f);
+        worldPainter.DrawLine(color, x, y, x + (float)Math.Cos(pose.Heading) * 250f,
+            y + (float)Math.Sin(pose.Heading) * 250f, endArrow: true, width: 3);
+        worldPainter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private void DrawCoarsePath(IReadOnlyList<CoarsePathPoint> path)
+    {
+        if (path == null || path.Count == 0) return;
+        for (int index = 1; index < path.Count; index++)
+            worldPainter.DrawLine(Color.LimeGreen, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 4);
+    }
+
+    private void DrawLocalG2Path(IReadOnlyList<SmoothedPathPoint> path)
+    {
+        if (path == null || path.Count == 0) return;
+        for (int index = 1; index < path.Count; index++)
+            worldPainter.DrawLine(Color.Gold, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 3);
+    }
+
+    private void DrawEmPath(IReadOnlyList<EmTrajectoryPoint> path)
+    {
+        for (int index = 1; index < path.Count; index++)
+            worldPainter.DrawLine(Color.DeepPink, ToMillimeters(path[index - 1].X), ToMillimeters(path[index - 1].Y),
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 4);
+    }
+
+    private void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
+    {
+        worldPainter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
+        worldPainter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
+        worldPainter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
+        worldPainter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
+    }
+
+    private void DrawStSeries(IReadOnlyList<TrajectoryObservationStSample> samples)
+    {
+        for (int index = 1; index < samples.Count; index++)
+            stPainter.DrawLine(Color.Cyan, ToMillimeters(samples[index - 1].TimeFromStart),
+                ToMillimeters(samples[index - 1].PathS), ToMillimeters(samples[index].TimeFromStart),
+                ToMillimeters(samples[index].PathS), width: 3);
+    }
+
+    private void DrawSpeedSeries(IReadOnlyList<TrajectoryObservationSpeedSample> samples)
+    {
+        if (samples == null) return;
+        for (int index = 1; index < samples.Count; index++)
+            stPainter.DrawLine(Color.Orange, ToMillimeters(samples[index - 1].TimeFromStart),
+                VelocitySeriesBaseMillimeters + ToMillimeters(samples[index - 1].SignedLongitudinalVelocity),
+                ToMillimeters(samples[index].TimeFromStart),
+                VelocitySeriesBaseMillimeters + ToMillimeters(samples[index].SignedLongitudinalVelocity), width: 3);
+    }
+
+    private void DrawStLegend(float maxTime, float maxS)
+    {
+        float x = maxTime + 250f;
+        stPainter.DrawLine(Color.Cyan, x, maxS, x + 200f, maxS, width: 4);
+        stPainter.DrawText(Color.Cyan, "t-s", x + 250f, maxS - 50f);
+        stPainter.DrawLine(Color.Orange, x, VelocitySeriesBaseMillimeters, x + 200f,
+            VelocitySeriesBaseMillimeters, width: 4);
+        stPainter.DrawText(Color.Orange, "t-v", x + 250f, VelocitySeriesBaseMillimeters - 50f);
+    }
+
+    private static float MaximumPathS(IReadOnlyList<TrajectoryObservationLsSample> samples)
+    {
+        double maximum = 1d;
+        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
+        return ToMillimeters(maximum);
+    }
+
+    private static float MaximumPathS(IReadOnlyList<TrajectoryObservationStSample> samples)
+    {
+        double maximum = 1d;
+        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, samples[index].PathS);
+        return ToMillimeters(maximum);
+    }
+
+    private static float MaximumLateralOffset(IReadOnlyList<TrajectoryObservationLsSample> samples)
+    {
+        double maximum = 1d;
+        for (int index = 0; index < samples.Count; index++) maximum = Math.Max(maximum, Math.Abs(samples[index].LateralOffset));
+        return ToMillimeters(maximum);
+    }
+
+    private static float MaximumTime(IReadOnlyList<TrajectoryObservationStSample> stSamples,
+        IReadOnlyList<TrajectoryObservationSpeedSample> speedSamples)
+    {
+        double maximum = 1d;
+        for (int index = 0; index < stSamples.Count; index++) maximum = Math.Max(maximum, stSamples[index].TimeFromStart);
+        if (speedSamples != null)
+            for (int index = 0; index < speedSamples.Count; index++)
+                maximum = Math.Max(maximum, speedSamples[index].TimeFromStart);
+        return ToMillimeters(maximum);
+    }
+
+    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
+}
@@ -17,6 +17,7 @@ internal static class TrajectoryObservationChecks
        VerifiesStartGoalBoundsUseOnlyConfiguredPadding();
        RejectsObstacleOutsideConfiguredBounds();
        VerifiesLsAndStUsePublishedTrajectoryData();
+        VerifiesPresentationTextDescribesObservationWithoutSendingCommand();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
        FreezesBootstrapVehicleForRollingRequests();
    }
@@ -123,6 +124,37 @@ internal static class TrajectoryObservationChecks
            "observer rolling request trajectory object and ID use one publication snapshot");
    }

+    private static void VerifiesPresentationTextDescribesObservationWithoutSendingCommand()
+    {
+        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 0, 0, 0, TimeSpan.Zero);
+        var settings = new TrajectoryObservationSettings();
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings,
+            Array.Empty<TrajectoryObservationObstacle>(), 0L);
+        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper()
+            .Bootstrap(job, CancellationToken.None);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedTrajectoryPlanningService(CreatePublishedTrajectory(effectiveAt)), "presentation-check");
+        var state = new VehicleMotionState(new Pose2D(0.25d, 0.10d, 0d), 0.20d, null, effectiveAt, 4L);
+        controller.StartCycle(effectiveAt, state, CancellationToken.None).GetAwaiter().GetResult();
+        TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state);
+        TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
+            observation.PublishedTrajectory, CreateStraightSegment(), 0.5d);
+
+        string text = TrajectoryObservationPresentationText.Create(observation, charts);
+
+        Verification.True(text.Contains("OBSERVE_ONLY: no chassis command is sent."),
+            "observer presentation observe-only notice");
+        Verification.True(text.Contains("selected t=0.50 s, path-S=4.50 m"),
+            "observer presentation selected point time and path-S");
+        Verification.True(text.Contains("signed speed=0.30 m/s"),
+            "observer presentation signed speed");
+        Verification.True(text.Contains("yaw rate=0.00 rad/s"),
+            "observer presentation yaw rate");
+        Verification.True(text.Contains("LS projection failures=0"),
+            "observer presentation LS projection failures");
+    }
+
    private static void FreezesBootstrapVehicleForRollingRequests()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);