From 37c27bdb1b41502ad23783a041a42512b3a49648 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Fri, 7 Aug 2026 10:58:18 +0800 Subject: [PATCH] fix: correct EM observation painter geometry --- .../TrajectoryObservationPresentation.cs | 98 ++++++++++++++++--- .../TrajectoryObservationChecks.cs | 46 ++++++++- ...rajectoryObservationVisualizationChecks.cs | 20 ++++ 3 files changed, 149 insertions(+), 15 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs index 9241bb9..bd84550 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPresentation.cs @@ -58,9 +58,9 @@ public sealed class TrajectoryObservationLsPresentationModel new List(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 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 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 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 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) diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index fae1757..6843a47 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -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); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index 2ca1609..b167358 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -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);