fix: correct EM observation painter geometry

2026-08-07 10:58:18 +08:00
parent d019b08074
commit 37c27bdb1b
3 changed files with 149 additions and 15 deletions
@@ -58,9 +58,9 @@ public sealed class TrajectoryObservationLsPresentationModel
            new List<TrajectoryObservationLsPresentationPoint>(samples));
    }

-    public string HorizontalAxisLabel => "path-S (m)";
+    public string HorizontalAxisLabel => "ReferenceS (m)";

-    public string VerticalAxisLabel => "lateral offset (m)";
+    public string VerticalAxisLabel => "l (m)";

    public IReadOnlyList<TrajectoryObservationLsPresentationPoint> Samples { get; }

@@ -98,13 +98,14 @@ public sealed class TrajectoryObservationPresentation
            return;
        }

+        VehicleParameters vehicle = bootstrap.Vehicle;
        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");
+        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Start, vehicle, Color.LimeGreen, "计划起点");
+        DrawPose(bootstrap.Job == null ? null : bootstrap.Job.Goal, vehicle, Color.Orange, "终点");
        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");
+            vehicle, Color.DeepSkyBlue, "车辆");
        if (runtimeState != null && runtimeState.WorldNotice.Length > 0)
        {
            float noticeX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
@@ -117,11 +118,13 @@ public sealed class TrajectoryObservationPresentation
        {
            float diagnosticX = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.XMin + 100f;
            float diagnosticY = bootstrap.Map == null ? 0f : bootstrap.Map.Bounds.YMin + 300f;
+            DrawBoundaryMarkers(bootstrap.Segments);
            worldPainter.DrawText(Color.LightYellow,
                EmptyChartMessage("No published trajectory available.\n", diagnosticText), diagnosticX, diagnosticY);
            return;
        }
        DrawEmPath(trajectory.Points);
+        DrawBoundaryMarkers(bootstrap.Segments);
    }

    public void DrawLs(TrajectoryObservationCharts charts, string diagnosticText)
@@ -167,6 +170,8 @@ public sealed class TrajectoryObservationPresentation
        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);
+        stPainter.DrawText(Color.White, "t (s)", maxTime + 100f, -200f);
+        stPainter.DrawText(Color.White, "PathS (m)", 100f, maxS + 300f);
        DrawStSeries(charts.StSamples);

        stPainter.DrawLine(Color.Gainsboro, 0f, VelocitySeriesBaseMillimeters,
@@ -225,14 +230,83 @@ public sealed class TrajectoryObservationPresentation
        }
    }

-    private void DrawPose(Pose2D pose, Color color, string label)
+    private void DrawPose(Pose2D pose, VehicleParameters vehicle, Color color, string label)
    {
        if (pose == null) return;
+        double lengthMeters = vehicle == null || vehicle.LengthMeters <= 0d ? 0.8d : vehicle.LengthMeters;
+        double widthMeters = vehicle == null || vehicle.WidthMeters <= 0d ? 0.6d : vehicle.WidthMeters;
+        double heading = pose.Heading;
+        double cosHeading = Math.Cos(heading);
+        double sinHeading = Math.Sin(heading);
        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);
+        float halfLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter / 2d);
+        float halfWidthMillimeters = (float)(widthMeters * MillimetersPerMeter / 2d);
+        float frontLeftX = x + (float)(cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
+        float frontLeftY = y + (float)(sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
+        float frontRightX = x + (float)(cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
+        float frontRightY = y + (float)(sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
+        float rearRightX = x + (float)(-cosHeading * halfLengthMillimeters + sinHeading * halfWidthMillimeters);
+        float rearRightY = y + (float)(-sinHeading * halfLengthMillimeters - cosHeading * halfWidthMillimeters);
+        float rearLeftX = x + (float)(-cosHeading * halfLengthMillimeters - sinHeading * halfWidthMillimeters);
+        float rearLeftY = y + (float)(-sinHeading * halfLengthMillimeters + cosHeading * halfWidthMillimeters);
+        DrawVehicleOutline(frontLeftX, frontLeftY, frontRightX, frontRightY,
+            rearRightX, rearRightY, rearLeftX, rearLeftY, color);
+        float headingLengthMillimeters = (float)(lengthMeters * MillimetersPerMeter * 0.25d);
+        worldPainter.DrawLine(color, x, y,
+            x + (float)cosHeading * headingLengthMillimeters,
+            y + (float)sinHeading * headingLengthMillimeters, endArrow: false, width: 1);
+        worldPainter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private void DrawVehicleOutline(float frontLeftX, float frontLeftY, float frontRightX, float frontRightY,
+        float rearRightX, float rearRightY, float rearLeftX, float rearLeftY, Color color)
+    {
+        worldPainter.DrawLine(color, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 1);
+        worldPainter.DrawLine(color, frontRightX, frontRightY, rearRightX, rearRightY, width: 1);
+        worldPainter.DrawLine(color, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 1);
+        worldPainter.DrawLine(color, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 1);
+    }
+
+    private void DrawBoundaryMarkers(IReadOnlyList<DirectionSegmentView> segments)
+    {
+        if (segments == null) return;
+        for (int index = 0; index < segments.Count; index++)
+        {
+            DirectionSegmentView segment = segments[index];
+            if (segment.Points.Count == 0) continue;
+            var point = segment.Points[segment.Points.Count - 1];
+            if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
+            {
+                DrawDiamondMarker(point, Color.Orange,
+                    "换向点 " + (segment.SegmentIndex + 1).ToString(CultureInfo.InvariantCulture) + " / s_end");
+            }
+            else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
+            {
+                DrawCrossMarker(point, Color.OrangeRed, "终点 / s_end");
+            }
+        }
+    }
+
+    private void DrawDiamondMarker(SmoothedPathPoint point, Color color, string label)
+    {
+        float x = ToMillimeters(point.X);
+        float y = ToMillimeters(point.Y);
+        const float radius = 50f;
+        worldPainter.DrawLine(color, x, y + radius, x + radius, y, width: 1);
+        worldPainter.DrawLine(color, x + radius, y, x, y - radius, width: 1);
+        worldPainter.DrawLine(color, x, y - radius, x - radius, y, width: 1);
+        worldPainter.DrawLine(color, x - radius, y, x, y + radius, width: 1);
+        worldPainter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private void DrawCrossMarker(SmoothedPathPoint point, Color color, string label)
+    {
+        float x = ToMillimeters(point.X);
+        float y = ToMillimeters(point.Y);
+        const float radius = 50f;
+        worldPainter.DrawLine(color, x - radius, y - radius, x + radius, y + radius, width: 1);
+        worldPainter.DrawLine(color, x - radius, y + radius, x + radius, y - radius, width: 1);
        worldPainter.DrawText(color, label, x + 100f, y + 100f);
    }

@@ -241,7 +315,7 @@ public sealed class TrajectoryObservationPresentation
        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);
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
    }

    private void DrawLocalG2Path(IReadOnlyList<SmoothedPathPoint> path)
@@ -249,14 +323,14 @@ public sealed class TrajectoryObservationPresentation
        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);
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 1);
    }

    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);
+                ToMillimeters(path[index].X), ToMillimeters(path[index].Y), width: 2);
    }

    private void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
@@ -23,6 +23,7 @@ internal static class TrajectoryObservationChecks
    {
        TrajectoryObservationSettingsChecks.Run();
        TrajectoryObservationSegmentChecks.Run();
+        VerifiesPainterPoseGeometryReplacesLargeArrow();
        TrajectoryObservationVisualizationChecks.Run();
        VerifiesObservationSourceHasNoActuatorCalls();
        VerifiesObservationSourceUsesRequiredOperatorText();
@@ -48,6 +49,7 @@ internal static class TrajectoryObservationChecks
        VerifiesPublishedPlanningDiagnosticsIncludeTrajectorySummary();
        VerifiesEmptyChartsReceivePersistentPlanningDiagnostic();
        VerifiesLsPresentationUsesPathSOnHorizontalAxis();
+        VerifiesPainterStAxesUsePathSAndTimeUnits();
        VerifiesRollingRequestUsesOnePublishedTrajectorySnapshot();
        VerifiesActiveSegmentControllerDoesNotCrossSeedTrajectories();
        VerifiesLoopDefersSegmentAdvanceUntilPlanningIsConsumed();
@@ -982,14 +984,52 @@ internal static class TrajectoryObservationChecks

        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.Equal("ReferenceS (m)", model.HorizontalAxisLabel, "observer LS horizontal axis label");
+        Verification.Equal("l (m)", model.VerticalAxisLabel, "observer LS vertical axis label");
        Verification.NearlyEqual(10.25d, model.Samples[0].HorizontalPathS,
-            "observer LS path-S is horizontal");
+            "observer LS shared ReferenceS is horizontal");
        Verification.NearlyEqual(0.10d, model.Samples[0].VerticalLateralOffset,
            "observer LS lateral offset is vertical");
    }

+    private static void VerifiesPainterPoseGeometryReplacesLargeArrow()
+    {
+        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
+
+        Verification.True(!source.Contains("endArrow: true"),
+            "observer painter removes the large terminal arrow");
+        Verification.True(source.Contains("endArrow: false"),
+            "observer painter uses a thin heading ray without an arrow");
+        foreach (string corner in new[]
+        {
+            "frontLeftX", "frontLeftY", "frontRightX", "frontRightY",
+            "rearRightX", "rearRightY", "rearLeftX", "rearLeftY",
+        })
+        {
+            Verification.True(source.Contains(corner), "observer pose outline includes " + corner);
+        }
+        Verification.True(source.Contains("VehicleParameters vehicle"),
+            "observer pose outline is scaled from vehicle parameters");
+        Verification.True(source.Contains("MillimetersPerMeter = 1000f") &&
+            source.Contains("ToMillimeters(path[index - 1].X)") &&
+            source.Contains("ToMillimeters(path[index - 1].Y)") &&
+            source.Contains("ToMillimeters(path[index].X)") &&
+            source.Contains("ToMillimeters(path[index].Y)"),
+            "observer world paths use one millimetres-per-metre factor for x and y");
+    }
+
+    private static void VerifiesPainterStAxesUsePathSAndTimeUnits()
+    {
+        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
+
+        Verification.True(source.Contains("\"t (s)\""), "observer ST horizontal axis uses seconds");
+        Verification.True(source.Contains("\"PathS (m)\""), "observer ST vertical axis uses true PathS");
+    }
+
    private static void FreezesBootstrapVehicleForRollingRequests()
    {
        DateTimeOffset effectiveAt = new DateTimeOffset(2026, 8, 4, 2, 0, 0, TimeSpan.Zero);
@@ -4,6 +4,7 @@ using System.Globalization;
 using System.IO;
 using System.Linq;
 using System.Reflection;
+using System.Text;
 using System.Threading;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
@@ -22,6 +23,7 @@ internal static class TrajectoryObservationVisualizationChecks
        VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
        VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
        VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers();
+        VerifiesPainterBoundarySemanticsMatchWeb();
        VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks();
        VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput();
    }
@@ -386,6 +388,24 @@ internal static class TrajectoryObservationVisualizationChecks
        Verification.Equal(0, factoryCalls, "disabled web visualization does not build dynamic snapshots");
    }

+    private static void VerifiesPainterBoundarySemanticsMatchWeb()
+    {
+        string presentationPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot",
+            "ParkrobTrajplanner", "tarjplanner_movementtest", "TrajectoryObservationPresentation.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(presentationPath));
+
+        Verification.True(source.Contains("DrawBoundaryMarkers"),
+            "observer painter draws shared boundary markers");
+        foreach (string expected in new[]
+        {
+            "计划起点", "车辆", "换向点 ", " / s_end", "终点 / s_end",
+        })
+        {
+            Verification.True(source.Contains(expected),
+                "observer painter boundary semantics include " + expected);
+        }
+    }
+
    private static PlanningVisualizationStaticSnapshot CreateStaticSnapshot(TrajectoryObservationSettings settings)
    {
        TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings);