fix: publish distinct EM observation semantics

2026-08-07 09:28:55 +08:00
parent 5d1c875584
commit d33257cc46
8 changed files with 313 additions and 30 deletions
@@ -21,6 +21,7 @@ internal static class TrajectoryObservationVisualizationChecks
        VerifiesKinematicChartsKeepKnotAndIntervalSemantics();
        VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure();
        VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints();
+        VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers();
        VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks();
        VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput();
    }
@@ -94,6 +95,17 @@ internal static class TrajectoryObservationVisualizationChecks
                    break;
                }
        Verification.True(verifiedOccupiedCell, "static fixture has an occupied cell");
+
+        EmPlannerConfiguration defaults = EmPlannerConfiguration.CreateDefault();
+        TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success(
+            bootstrap.Job, bootstrap.CoarseResult, bootstrap.SmoothedPath,
+            new[] { CreateGearSwitchSegment(0, 0d) });
+        PlanningVisualizationStaticSnapshot gearStatic = new TrajectoryObservationStaticSnapshotBuilder()
+            .Build(gearBootstrap, defaults, settings.CreateValidatedSnapshot(), 0L);
+        Verification.True(gearStatic.StaticMarkers.Any(x => x.Kind == "gear-switch-end"),
+            "static snapshot marks gear-switch s_end");
+        Verification.True(snapshot.StaticMarkers.Any(x => x.Kind == "final-goal"),
+            "static snapshot marks final goal s_end");
    }

    private static void VerifiesKinematicChartsKeepKnotAndIntervalSemantics()
@@ -103,8 +115,16 @@ internal static class TrajectoryObservationVisualizationChecks
        EmTrajectory trajectory = CreateTrajectory("rolling", DateTimeOffset.UnixEpoch, 21,
            EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);

+        var vehicle = new VehicleParameters
+        {
+            LengthMeters = 0.8d,
+            WidthMeters = 0.6d,
+            SafetyMarginMeters = 0.05d,
+            MaximumCurvaturePerMeter = 0.4d,
+            MinimumTurningRadiusMeters = 2.5d,
+        };
        IReadOnlyList<VisualizationChart> charts = new TrajectoryObservationKinematicChartBuilder()
-            .Build(trajectory, segment, configuration);
+            .Build(trajectory, segment, vehicle, configuration);

        Verification.Equal(8, charts.Count, "all eight kinematic charts exported");
        Verification.Equal(20, FindChart(charts, "jerk-t").Series[0].Points.Count, "jerk has N-1 intervals");
@@ -118,9 +138,49 @@ internal static class TrajectoryObservationVisualizationChecks
                id + " has an effective limit line");
        Verification.True(!FindChart(charts, "yaw-rate-t").Series.Any(x => x.LineStyle == VisualizationLineStyle.Limit),
            "yaw rate has no invented limit");
+        configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond = 1.25d;
+        charts = new TrajectoryObservationKinematicChartBuilder().Build(trajectory, segment, vehicle, configuration);
+        Verification.Equal("ω (rad/s)", FindChart(charts, "yaw-rate-t").YAxisLabel,
+            "yaw-rate axis uses omega unit");
+        Verification.NearlyEqual(0.4d, FindChart(charts, "curvature-s")
+            .Series.Single(x => x.Id == "curvature-s-positive-limit").Points[0].Y,
+            "curvature hard limit uses vehicle curvature");
+        Verification.True(FindChart(charts, "curvature-s").Series[0].Points.Any(p => Math.Abs(p.Y) > 1e-6),
+            "curvature-distance uses vehicle curvature data");
+        PropertyInfo? annotationsProperty = typeof(VisualizationChart).GetProperty("Annotations");
+        Verification.True(annotationsProperty != null, "chart exposes annotation collection");
+        if (annotationsProperty != null)
+        {
+            object annotationList = annotationsProperty!.GetValue(FindChart(charts, "st"))!;
+            Verification.True(annotationList is System.Collections.IEnumerable,
+                "ST annotations are enumerable");
+            var annotations = new List<object>();
+            foreach (object annotation in (System.Collections.IEnumerable)annotationList!)
+            {
+                annotations.Add(annotation);
+            }
+            Verification.True(annotations.Count > 0, "ST chart marks s_end");
+            Verification.True(annotations.Any(annotation =>
+                (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"),
+                "ST s_end annotation kind is explicit");
+            object lsAnnotationList = annotationsProperty!.GetValue(FindChart(charts, "ls"))!;
+            var lsAnnotations = new List<object>();
+            foreach (object annotation in (System.Collections.IEnumerable)lsAnnotationList!)
+            {
+                lsAnnotations.Add(annotation);
+            }
+            Verification.True(lsAnnotations.Count > 0, "LS chart marks s_end");
+            Verification.True(lsAnnotations.Any(annotation =>
+                (string)annotation.GetType().GetProperty("Kind")!.GetValue(annotation)! == "s-end"),
+                "LS s_end annotation kind is explicit");
+        }
+        Verification.True(typeof(TrajectoryObservationDynamicSnapshotBuilder).GetMethods().Any(method =>
+                method.Name == "Build" &&
+                method.GetParameters().Any(parameter => parameter.ParameterType == typeof(VehicleParameters))),
+            "dynamic builder accepts vehicle parameters");
        EmTrajectory unprojectable = CreateTrajectory("unprojectable", DateTimeOffset.UnixEpoch, 21,
            EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward, 100d);
-        Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, configuration), "ls")
+        Verification.True(FindChart(new TrajectoryObservationKinematicChartBuilder().Build(unprojectable, segment, vehicle, configuration), "ls")
            .NoteChinese.Contains("投影失败=21"), "LS chart reports every failed full-segment projection");
    }

@@ -166,12 +226,15 @@ internal static class TrajectoryObservationVisualizationChecks
            EmLongitudinalMode.RollingContinuation, TravelDirection.Forward);

        PlanningVisualizationDynamicSnapshot snapshot = new TrajectoryObservationDynamicSnapshotBuilder()
-            .Build(5L, tick, controller.ActiveSegment, previous, configuration);
+            .Build(5L, tick, controller.ActiveSegment, previous, bootstrap.Vehicle, configuration);

        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current"), "dynamic snapshot contains current trajectory");
        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "previous"), "dynamic snapshot contains previous trajectory");
        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "active-segment"), "dynamic snapshot contains active segment highlight");
-        Verification.True(snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"), "dynamic snapshot contains projected horizon");
+        Verification.Equal(1, snapshot.DynamicPolylines.Count(x => x.Id == "current"),
+            "same current trajectory is published once");
+        Verification.True(!snapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
+            "identical current trajectory is not duplicated as current horizon");
        foreach (string rawName in new[] { "ActiveSegmentIndex", "ActiveDirection", "PlanningStatus", "LongitudinalMode",
            "TerminalType", "PlanningElapsedMilliseconds", "TrajectoryAgeSeconds", "LsProjectionFailures",
            "RollingTerminalConstraint", "TerminalVelocity", "TerminalAcceleration", "DeltaPositionMeters",
@@ -197,7 +260,8 @@ internal static class TrajectoryObservationVisualizationChecks
        TrajectoryObservationLoopTick exactTick = new TrajectoryObservationLoop(exactController)
            .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
        PlanningVisualizationDynamicSnapshot exactSnapshot = new TrajectoryObservationDynamicSnapshotBuilder().Build(6L,
-            exactTick, exactController.ActiveSegment, null, ReadEffectiveConfiguration(exactController));
+            exactTick, exactController.ActiveSegment, null, bootstrap.Vehicle,
+            ReadEffectiveConfiguration(exactController));
        Verification.True(exactSnapshot.StatusValues.Single(x => x.RawName == "RollingTerminalConstraint").Value.Contains("精确停车锚点"),
            "exact-stop status describes the true stop anchor");
        Verification.NearlyEqual(exactStop.Points[exactStop.Points.Count - 1].SignedLongitudinalVelocity,
@@ -213,11 +277,50 @@ internal static class TrajectoryObservationVisualizationChecks
        TrajectoryObservationLoopTick offSegmentTick = new TrajectoryObservationLoop(offSegmentController)
            .Tick(now.AddSeconds(settings.ObserverPeriodSeconds), state, CancellationToken.None);
        PlanningVisualizationDynamicSnapshot recovered = new TrajectoryObservationDynamicSnapshotBuilder().Build(7L,
-            offSegmentTick, offSegmentController.ActiveSegment, null, ReadEffectiveConfiguration(offSegmentController));
+            offSegmentTick, offSegmentController.ActiveSegment, null, bootstrap.Vehicle,
+            ReadEffectiveConfiguration(offSegmentController));
        Verification.Equal("21", recovered.StatusValues.Single(x => x.RawName == "LsProjectionFailures").Value,
            "projection failure is reported while dynamic snapshot construction remains available");
    }

+    private static void VerifiesFullModeSnapshotPublishesSingleCurrentTrajectoryAndBoundaryMarkers()
+    {
+        var settings = new TrajectoryObservationSettings();
+        TrajectoryObservationBootstrapResult goalBootstrap = Bootstrap(settings);
+        TrajectoryObservationBootstrapResult gearBootstrap = TrajectoryObservationBootstrapResult.Success(
+            goalBootstrap.Job, goalBootstrap.CoarseResult, goalBootstrap.SmoothedPath,
+            new[] { CreateGearSwitchSegment(0, 0d) });
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+
+        PlanningVisualizationDynamicSnapshot gearSnapshot = BuildDynamicSnapshot(gearBootstrap, settings, configuration,
+            CreateTrajectory("full-gear", DateTimeOffset.UnixEpoch.AddSeconds(1d), 21, EmTerminalType.GearSwitch,
+                EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d,
+                EmPlanningScope.FullDirectionSegment));
+        Verification.Equal(1, gearSnapshot.DynamicPolylines.Count(x => x.Kind == "current"),
+            "full mode publishes one current trajectory");
+        Verification.True(!gearSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
+            "full mode does not duplicate current horizon");
+        Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "vehicle"),
+            "dynamic snapshot marks the vehicle");
+        Verification.True(gearSnapshot.DynamicMarkers.Any(x => x.Kind == "plan-start"),
+            "dynamic snapshot marks the plan start");
+        VisualizationMarker gearEnd = gearSnapshot.DynamicMarkers.Single(x => x.Kind == "gear-switch-end");
+        Verification.True(gearEnd.LabelChinese.Contains("换向点") && gearEnd.LabelChinese.Contains("s_end"),
+            "gear switch boundary uses the explicit s_end label");
+
+        PlanningVisualizationDynamicSnapshot goalSnapshot = BuildDynamicSnapshot(goalBootstrap, settings, configuration,
+            CreateTrajectory("full-goal", DateTimeOffset.UnixEpoch.AddSeconds(2d), 21, EmTerminalType.Goal,
+                EmLongitudinalMode.ExactStopAtBoundary, TravelDirection.Forward, 0d, 0d,
+                EmPlanningScope.FullDirectionSegment));
+        Verification.Equal(1, goalSnapshot.DynamicPolylines.Count(x => x.Kind == "current"),
+            "goal full mode publishes one current trajectory");
+        Verification.True(!goalSnapshot.DynamicPolylines.Any(x => x.Id == "current-horizon"),
+            "goal full mode does not duplicate current horizon");
+        VisualizationMarker finalGoal = goalSnapshot.DynamicMarkers.Single(x => x.Kind == "final-goal");
+        Verification.True(finalGoal.LabelChinese.Contains("终点") && finalGoal.LabelChinese.Contains("s_end"),
+            "final goal boundary uses the explicit s_end label");
+    }
+
    private static void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks()
    {
        var settings = new TrajectoryObservationSettings { EnableWebVisualization = true };
@@ -444,6 +547,22 @@ internal static class TrajectoryObservationVisualizationChecks
        return charts.Single(x => x.Id == id);
    }

+    private static PlanningVisualizationDynamicSnapshot BuildDynamicSnapshot(
+        TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationSettings settings,
+        EmPlannerConfiguration configuration, EmTrajectory trajectory)
+    {
+        DateTimeOffset now = DateTimeOffset.UnixEpoch.AddMinutes(1);
+        var controller = new TrajectoryObservationController(bootstrap, settings,
+            new FixedPlanningService(trajectory), "snapshot-" + trajectory.Metadata.TrajectoryId);
+        var loop = new TrajectoryObservationLoop(controller);
+        var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0.1d, null, now, 1L);
+        controller.StartCycle(now, state, CancellationToken.None).GetAwaiter().GetResult();
+        TrajectoryObservationLoopTick tick = loop.Tick(now.AddSeconds(settings.ObserverPeriodSeconds),
+            state, CancellationToken.None);
+        return new TrajectoryObservationDynamicSnapshotBuilder().Build(
+            1L, tick, controller.ActiveSegment, null, bootstrap.Vehicle, configuration);
+    }
+
    private static DirectionSegmentView CreateStraightSegment(int index, double sourceStart)
    {
        var points = new List<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
@@ -458,8 +577,23 @@ internal static class TrajectoryObservationVisualizationChecks
            new ReferenceBoundary(index, 10d, EmBoundaryType.Goal, sourceStart + 10d), sourceStart);
    }

+    private static DirectionSegmentView CreateGearSwitchSegment(int index, double sourceStart)
+    {
+        var points = new List<MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint>
+        {
+            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(0d, 0d, 0d, 0d, 0d,
+                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
+            new MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPoint(10d, 0d, 0d, 0d, 10d,
+                TravelDirection.Forward, 0d, 0d, 1d, false, MultiWheelC.TrajectoryPlanning.PathSmoothing.SmoothedPathPointSource.Anchor),
+        };
+        return new DirectionSegmentView(index, TravelDirection.Forward, points,
+            new ReferenceBoundary(index, 0d, EmBoundaryType.None, sourceStart),
+            new ReferenceBoundary(index, 10d, EmBoundaryType.GearSwitchApproach, sourceStart + 10d), sourceStart);
+    }
+
    private static EmTrajectory CreateTrajectory(string id, DateTimeOffset effectiveAt, int count, EmTerminalType terminal,
-        EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d)
+        EmLongitudinalMode mode, TravelDirection direction, double startX = 0d, double startPathS = 1000d,
+        EmPlanningScope planningScope = EmPlanningScope.RollingHorizon)
    {
        var points = new List<EmTrajectoryPoint>();
        for (int index = 0; index < count; index++)
@@ -470,7 +604,7 @@ internal static class TrajectoryObservationVisualizationChecks
                0.2d + t, 0.3d + t));
        }
        return new EmTrajectory(new EmTrajectoryMetadata(id, effectiveAt, effectiveAt, 1L, "visualization-reference", 1L,
-            string.Empty, 0, direction, terminal, mode, EmPlanningScope.RollingHorizon), points);
+            string.Empty, 0, direction, terminal, mode, planningScope), points);
    }

    private sealed class FixedPlanningService : IEmPlanningService
@@ -13,6 +13,7 @@ internal static class ContractChecks
        CopiesCollectionsAndRejectsNonFiniteNumbers();
        StoresOccupancyAsDefensiveCompactBits();
        ExposesImmutableSnapshotContracts();
+        ExposesChartAnnotationContract();
        DocumentsPublicSafetyAndLifecycleBoundary();
    }

@@ -87,6 +88,36 @@ internal static class ContractChecks
        }
    }

+    private static void ExposesChartAnnotationContract()
+    {
+        Type chartType = typeof(VisualizationChart);
+        PropertyInfo annotationsProperty = chartType.GetProperty("Annotations");
+        Verification.True(annotationsProperty != null, "chart exposes immutable annotations");
+        Type annotationType = chartType.Assembly.GetType("TrajectoryPlanningVisualization.VisualizationChartAnnotation");
+        Verification.True(annotationType != null, "chart annotation contract type exists");
+
+        var chart = new VisualizationChart("annotation-legacy", "标题", "x (m)", "y (m)",
+            new[]
+            {
+                new VisualizationSeries("s", "轨迹", VisualizationLineStyle.Solid,
+                    new[] { new VisualizationPoint(0d, 0d) })
+            });
+        if (annotationsProperty == null)
+        {
+            return;
+        }
+
+        object value = annotationsProperty.GetValue(chart);
+        Verification.True(value is System.Collections.IEnumerable, "chart annotations are enumerable");
+        int count = 0;
+        foreach (object annotation in (System.Collections.IEnumerable)value)
+        {
+            count++;
+        }
+        Verification.Equal(0, count, "legacy chart constructor keeps annotations empty");
+        AssertNoSetters(chartType);
+    }
+
    private static void DocumentsPublicSafetyAndLifecycleBoundary()
    {
        string readmePath = Path.GetFullPath(Path.Combine(AppContext.BaseDirectory,
@@ -20,7 +20,12 @@ internal static class SampleSnapshotFactory
                new VisualizationPolyline("global", "全局参考路径", "global", VisualizationLineStyle.Solid,
                    Points((0d, 1d), (3d, 1d), (6d, 2d), (8d, 4d), (11d, 6d)))
            },
-            new[] { new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)) },
+            new[]
+            {
+                new VisualizationMarker("gear-1", "gear-switch", "换向 1", new VisualizationPoint(6d, 2d)),
+                new VisualizationMarker("gear-1-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d)),
+                new VisualizationMarker("final-goal", "final-goal", "终点 / s_end", new VisualizationPoint(11d, 6d))
+            },
            new[]
            {
                new VisualizationDirectionSegment(0, "forward", false, true, Points((0d, 1d), (3d, 1d), (6d, 2d))),
@@ -55,7 +60,12 @@ internal static class SampleSnapshotFactory
                new VisualizationPolyline("previous", "上一轮轨迹", "previous", VisualizationLineStyle.Dashed, Points((1d, 1d), (2.5d, 1d), (4d, 1.3d))),
                new VisualizationPolyline("current", "当前轨迹", "current", VisualizationLineStyle.Solid, Points((2d + offset, 1d), (3.8d + offset, 1.2d), (5.8d + offset, 1.9d)))
            },
-            new[] { new VisualizationMarker("handoff", "handoff", "交接", new VisualizationPoint(2d + offset, 1d)) },
+            new[]
+            {
+                new VisualizationMarker("vehicle", "vehicle", "车辆", new VisualizationPoint(2d + offset, 1d)),
+                new VisualizationMarker("plan-start", "plan-start", "计划起点", new VisualizationPoint(2d + offset, 1d)),
+                new VisualizationMarker("gear-switch-end", "gear-switch-end", "换向点 1 / s_end", new VisualizationPoint(6d, 2d))
+            },
            CreateCharts(offset),
            new[]
            {
@@ -70,21 +80,26 @@ internal static class SampleSnapshotFactory
    {
        return new[]
        {
-            Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Points((0d, .05d), (2d, .08d), (4d, .02d))),
-            Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d))),
+            Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", Points((0d, .05d), (2d, .08d), (4d, .02d)),
+                new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end", 4d) }),
+            Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points((0d, 0d), (1d, 1.2d), (2d, 3.0d)),
+                new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", 2d) }),
            Chart("curvature-s", "曲率—距离", "s (m)", "κ (m⁻¹)", Points((0d, .01d), (2d, .04d), (4d, .02d))),
            Chart("curvature-t", "曲率—时间", "t (s)", "κ (m⁻¹)", Points((0d, .01d), (1d, .04d), (2d, .02d))),
            Chart("velocity-t", "速度", "t (s)", "v (m/s)", Points((0d, 0d), (1d, 1d), (2d, 1.2d + offset))),
            Chart("acceleration-t", "加速度", "t (s)", "a (m/s²)", Points((0d, .8d), (1d, .2d), (2d, 0d))),
            Chart("jerk-t", "加加速度", "t (s)", "j (m/s³)", Points((0d, -.6d), (1d, -.2d))),
-            Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d)))
+            Chart("yaw-rate-t", "横摆角速度", "t (s)", "ω (rad/s)", Points((0d, 0d), (1d, .08d), (2d, .04d)))
        };
    }

-    private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis, IReadOnlyList<VisualizationPoint> points)
+    private static VisualizationChart Chart(string id, string title, string xAxis, string yAxis,
+        IReadOnlyList<VisualizationPoint> points,
+        IReadOnlyList<VisualizationChartAnnotation> annotations = null)
    {
        return new VisualizationChart(id, title, xAxis, yAxis,
-            new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) });
+            new[] { new VisualizationSeries(id + "-current", "当前轨迹", VisualizationLineStyle.Solid, points) },
+            "", annotations ?? Array.Empty<VisualizationChartAnnotation>());
    }

    private static IReadOnlyList<VisualizationPoint> Points(params (double x, double y)[] values)