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
@@ -321,7 +321,8 @@ internal static class TrajectoryObservationMovementTestRunner
            DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic);
            webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build(
                Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment,
-                controller.PreviousTrajectoryForVisualization, controller.CreateEffectiveConfigurationSnapshot()));
+                controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
+                controller.CreateEffectiveConfigurationSnapshot()));
            if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted)
            {
                LogIfCurrent(sessionId, runtimeState.WorldNotice);
@@ -13,10 +13,12 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
    private readonly TrajectoryObservationHandoffAnalyzer handoff = new TrajectoryObservationHandoffAnalyzer();

    public PlanningVisualizationDynamicSnapshot Build(long sequence, TrajectoryObservationLoopTick tick,
-        DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization, EmPlannerConfiguration configuration)
+        DirectionSegmentView activeSegment, EmTrajectory previousTrajectoryForVisualization,
+        VehicleParameters vehicle, EmPlannerConfiguration configuration)
    {
        if (tick == null) throw new ArgumentNullException(nameof(tick));
        if (activeSegment == null) throw new ArgumentNullException(nameof(activeSegment));
+        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));

        EmTrajectory current = tick.Observation.PublishedTrajectory;
@@ -24,14 +26,17 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
        AddSegment(polylines, activeSegment);
        AddTrajectory(polylines, "previous", "上一轨迹", previousTrajectoryForVisualization, VisualizationLineStyle.Dashed);
        AddTrajectory(polylines, "current", "当前轨迹", current, VisualizationLineStyle.Solid);
-        AddTrajectory(polylines, "current-horizon", "当前规划视界", current, VisualizationLineStyle.Solid);

        IReadOnlyList<VisualizationChart> chartList = current == null
            ? Array.Empty<VisualizationChart>()
-            : charts.Build(current, activeSegment, configuration);
+            : charts.Build(current, activeSegment, vehicle, configuration);
        int projectionFailures = current == null ? 0 : charts.CountProjectionFailures(current, activeSegment, configuration);
        TrajectoryObservationHandoffMetrics metrics = handoff.Analyze(current, previousTrajectoryForVisualization, activeSegment);
        EmTrajectoryPoint terminal = current == null ? null : current.Points[current.Points.Count - 1];
+        var markers = new List<VisualizationMarker>();
+        AddVehicleMarker(markers, tick);
+        AddPlanStartMarker(markers, current);
+        AddBoundaryMarker(markers, activeSegment);
        var status = new List<VisualizationValue>
        {
            Value("活动段", "ActiveSegmentIndex", activeSegment.SegmentIndex.ToString(CultureInfo.InvariantCulture), "", "normal"),
@@ -57,7 +62,7 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
        return new PlanningVisualizationDynamicSnapshot(sequence, tick.Observation.ObservedAtUtc, "OBSERVE_ONLY",
            activeSegment.SegmentIndex, activeSegment.Direction.ToString(), new VisualizationPose(
                tick.Observation.VehicleState.Pose.X, tick.Observation.VehicleState.Pose.Y,
-                tick.Observation.VehicleState.Pose.Heading), polylines, Array.Empty<VisualizationMarker>(), chartList, status,
+                tick.Observation.VehicleState.Pose.Heading), polylines, markers, chartList, status,
            new VisualizationCycleSummary(cycle == null ? 0L : cycle.Version, tick.Observation.ObservedAtUtc,
                cycle == null ? "pending" : cycle.Result.Status.ToString(), cycle != null && cycle.Published,
                Math.Max(0d, tick.LatestPlanningElapsed.TotalMilliseconds), activeSegment.SegmentIndex,
@@ -84,6 +89,39 @@ public sealed class TrajectoryObservationDynamicSnapshotBuilder
        lines.Add(new VisualizationPolyline(id, legend, id, style, points));
    }

+    private static void AddVehicleMarker(ICollection<VisualizationMarker> markers,
+        TrajectoryObservationLoopTick tick)
+    {
+        markers.Add(new VisualizationMarker("vehicle", "vehicle", "车辆",
+            new VisualizationPoint(tick.Observation.VehicleState.Pose.X,
+                tick.Observation.VehicleState.Pose.Y)));
+    }
+
+    private static void AddPlanStartMarker(ICollection<VisualizationMarker> markers, EmTrajectory trajectory)
+    {
+        if (trajectory == null || trajectory.Points.Count == 0) return;
+        EmTrajectoryPoint start = trajectory.Points[0];
+        markers.Add(new VisualizationMarker("plan-start", "plan-start", "计划起点",
+            new VisualizationPoint(start.X, start.Y)));
+    }
+
+    private static void AddBoundaryMarker(ICollection<VisualizationMarker> markers,
+        DirectionSegmentView segment)
+    {
+        var point = segment.Points[segment.Points.Count - 1];
+        if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
+        {
+            markers.Add(new VisualizationMarker("gear-switch-end", "gear-switch-end",
+                "换向点 " + (segment.SegmentIndex + 1) + " / s_end",
+                new VisualizationPoint(point.X, point.Y)));
+        }
+        else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
+        {
+            markers.Add(new VisualizationMarker("final-goal", "final-goal", "终点 / s_end",
+                new VisualizationPoint(point.X, point.Y)));
+        }
+    }
+
    private static string RollingTerminalText(EmTrajectory trajectory)
    {
        if (trajectory == null) return "不可用";
@@ -10,26 +10,32 @@ namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
 public sealed class TrajectoryObservationKinematicChartBuilder
 {
    public IReadOnlyList<VisualizationChart> Build(EmTrajectory trajectory, DirectionSegmentView segment,
-        EmPlannerConfiguration configuration)
+        VehicleParameters vehicle, EmPlannerConfiguration configuration)
    {
        if (trajectory == null) throw new ArgumentNullException(nameof(trajectory));
        if (segment == null) throw new ArgumentNullException(nameof(segment));
+        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));

        LsBuildResult ls = BuildLs(trajectory, segment, configuration.Frenet.MaximumProjectionDistanceMeters);
+        double curvatureLimit = CurvatureLimit(vehicle);
+        double terminalTime = trajectory.Points.Count == 0 ? 0d : trajectory.Points[trajectory.Points.Count - 1].TimeFromStart;
        return new[]
        {
            Chart("ls", "横向偏移", "ReferenceS (m)", "l (m)", ls.Points,
-                "完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture)),
-            Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), ""),
+                "完整活动方向段投影；投影失败=" + ls.ProjectionFailureCount.ToString(CultureInfo.InvariantCulture),
+                new[] { new VisualizationChartAnnotation("ls-s-end", "s-end", "s_end",
+                    segment.SourceStartArcLength + segment.LengthMeters) }),
+            Chart("st", "时空轨迹", "t (s)", "PathS (m)", Points(trajectory, p => p.PathS), "",
+                new[] { new VisualizationChartAnnotation("st-s-end", "s-end", "s_end", terminalTime) }),
            WithSymmetricLimit("curvature-s", "曲率-距离", "s (m)", "κ (m⁻¹)",
-                PointsBySegmentS(trajectory), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
+                PointsBySegmentS(trajectory), curvatureLimit),
            WithSymmetricLimit("curvature-t", "曲率-时间", "t (s)", "κ (m⁻¹)",
-                Points(trajectory, p => p.VehicleCurvature), configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond),
+                Points(trajectory, p => p.VehicleCurvature), curvatureLimit),
            Velocity(trajectory, configuration),
            Acceleration(trajectory, configuration),
            Jerk(trajectory, configuration),
-            Chart("yaw-rate-t", "横摆角速度", "t (s)", "yaw rate (rad/s)",
+            Chart("yaw-rate-t", "横摆角速度", "t (s)", "ω (rad/s)",
                Points(trajectory, p => p.YawRate), "生效配置没有独立横摆角速度上限"),
        };
    }
@@ -98,10 +104,12 @@ public sealed class TrajectoryObservationKinematicChartBuilder
    }

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

    private static IReadOnlyList<VisualizationPoint> Points(EmTrajectory trajectory, Func<EmTrajectoryPoint, double> y)
@@ -120,6 +128,21 @@ public sealed class TrajectoryObservationKinematicChartBuilder
        return points;
    }

+    private static double CurvatureLimit(VehicleParameters vehicle)
+    {
+        if (vehicle.MaximumCurvaturePerMeter.HasValue)
+        {
+            return Math.Abs(vehicle.MaximumCurvaturePerMeter.Value);
+        }
+
+        if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
+        {
+            return 1d / vehicle.MinimumTurningRadiusMeters.Value;
+        }
+
+        throw new ArgumentException("Vehicle curvature limit is required.", nameof(vehicle));
+    }
+
    private static LsBuildResult BuildLs(EmTrajectory trajectory, DirectionSegmentView segment,
        double maximumProjectionDistanceMeters)
    {
@@ -57,12 +57,21 @@ public sealed class TrajectoryObservationStaticSnapshotBuilder
    {
        var markers = new List<VisualizationMarker>();
        foreach (DirectionSegmentView segment in bootstrap.Segments)
+        {
            if (segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach)
            {
                var point = segment.Points[segment.Points.Count - 1];
-                markers.Add(new VisualizationMarker("gear-switch-" + segment.SegmentIndex, "gear-switch",
-                    "换向 " + (segment.SegmentIndex + 1), new VisualizationPoint(point.X, point.Y)));
+                markers.Add(new VisualizationMarker("gear-switch-end-" + segment.SegmentIndex, "gear-switch-end",
+                    "换向点 " + (segment.SegmentIndex + 1) + " / s_end",
+                    new VisualizationPoint(point.X, point.Y)));
            }
+            else if (segment.EndBoundary.BoundaryType == EmBoundaryType.Goal)
+            {
+                var point = segment.Points[segment.Points.Count - 1];
+                markers.Add(new VisualizationMarker("final-goal-" + segment.SegmentIndex, "final-goal",
+                    "终点 / s_end", new VisualizationPoint(point.X, point.Y)));
+            }
+        }
        return markers;
    }