feat: export EM observation configuration

2026-08-06 11:17:57 +08:00
parent fffcfdb8ae
commit 0a1cbe9fbd
3 changed files with 108 additions and 0 deletions
@@ -0,0 +1,93 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using TrajectoryPlanningVisualization;
+
+namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+public sealed class TrajectoryObservationStaticSnapshotBuilder
+{
+    public PlanningVisualizationStaticSnapshot Build(TrajectoryObservationBootstrapResult bootstrap,
+        EmPlannerConfiguration effectiveConfiguration, TrajectoryObservationSettings settings, long obstacleSnapshotVersion)
+    {
+        if (bootstrap == null) throw new ArgumentNullException(nameof(bootstrap));
+        if (effectiveConfiguration == null) throw new ArgumentNullException(nameof(effectiveConfiguration));
+        if (settings == null) throw new ArgumentNullException(nameof(settings));
+        var map = bootstrap.Map ?? throw new ArgumentException("Bootstrap map is required.", nameof(bootstrap));
+        var bounds = new VisualizationBounds(map.Bounds.XMin / 1000d, map.Bounds.XMax / 1000d,
+            map.Bounds.YMin / 1000d, map.Bounds.YMax / 1000d);
+        return new PlanningVisualizationStaticSnapshot("EM轨迹规划观察", bounds, CreateGrid(map, bounds),
+            CreatePolylines(bootstrap), CreateMarkers(bootstrap), CreateSegments(bootstrap),
+            CreateConfiguration(effectiveConfiguration, settings, map, obstacleSnapshotVersion));
+    }
+
+    private static VisualizationOccupancyGrid CreateGrid(Mapping.PlanningGridMap map, VisualizationBounds bounds)
+    {
+        var bits = new byte[(map.Rows * map.Cols + 7) / 8];
+        for (int row = 0; row < map.Rows; row++)
+            for (int column = 0; column < map.Cols; column++)
+                if (map.IsOccupied(row, column)) bits[(row * map.Cols + column) >> 3] |= (byte)(1 << ((row * map.Cols + column) & 7));
+        return new VisualizationOccupancyGrid(bounds, map.ResolutionMeters, map.Rows, map.Cols, bits);
+    }
+
+    private static IReadOnlyList<VisualizationPolyline> CreatePolylines(TrajectoryObservationBootstrapResult bootstrap)
+    {
+        var lines = new List<VisualizationPolyline>();
+        var coarse = new List<VisualizationPoint>();
+        foreach (var point in bootstrap.CoarseResult.PlanningResult.Path) coarse.Add(new VisualizationPoint(point.X, point.Y));
+        if (coarse.Count > 0) lines.Add(new VisualizationPolyline("coarse-path", "粗路径", "coarse", VisualizationLineStyle.Dashed, coarse));
+        var smooth = new List<VisualizationPoint>();
+        foreach (var point in bootstrap.SmoothedPath.Path) smooth.Add(new VisualizationPoint(point.X, point.Y));
+        if (smooth.Count > 0) lines.Add(new VisualizationPolyline("local-g2-path", "Local G2路径", "local-g2", VisualizationLineStyle.Solid, smooth));
+        return lines;
+    }
+
+    private static IReadOnlyList<VisualizationMarker> CreateMarkers(TrajectoryObservationBootstrapResult bootstrap)
+    {
+        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)));
+            }
+        return markers;
+    }
+
+    private static IReadOnlyList<VisualizationDirectionSegment> CreateSegments(TrajectoryObservationBootstrapResult bootstrap)
+    {
+        var result = new List<VisualizationDirectionSegment>();
+        foreach (DirectionSegmentView segment in bootstrap.Segments)
+        {
+            var points = new List<VisualizationPoint>();
+            foreach (var point in segment.Points) points.Add(new VisualizationPoint(point.X, point.Y));
+            result.Add(new VisualizationDirectionSegment(segment.SegmentIndex, segment.Direction.ToString(),
+                segment.StartBoundary.BoundaryType == EmBoundaryType.GearSwitchDeparture,
+                segment.EndBoundary.BoundaryType == EmBoundaryType.GearSwitchApproach, points));
+        }
+        return result;
+    }
+
+    private static IReadOnlyList<VisualizationConfigurationGroup> CreateConfiguration(EmPlannerConfiguration configuration,
+        TrajectoryObservationSettings settings, Mapping.PlanningGridMap map, long obstacleSnapshotVersion)
+    {
+        string F(double value) => value.ToString("R", CultureInfo.InvariantCulture);
+        var groups = new List<VisualizationConfigurationGroup>();
+        groups.Add(Group("调度", V("重规划周期", "ReplanPeriodSeconds", F(configuration.Scheduling.ReplanPeriodSeconds), "s"),
+            V("时域", "TimeHorizonSeconds", F(configuration.Scheduling.TimeHorizonSeconds), "s"),
+            V("距离视界", "DistanceHorizonMeters", F(configuration.Scheduling.DistanceHorizonMeters), "m"),
+            V("输出步长", "OutputTimeStepSeconds", F(configuration.Scheduling.OutputTimeStepSeconds), "s"),
+            V("求解超时", "SolverTimeoutSeconds", F(configuration.Scheduling.SolverTimeoutSeconds), "s")));
+        groups.Add(Group("OSQP", V("最大外层迭代", "MaximumOuterIterations", configuration.Solver.MaximumOuterIterations.ToString(CultureInfo.InvariantCulture), ""),
+            V("最大OSQP迭代", "MaximumOsqpIterations", configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture), ""),
+            V("绝对容差", "AbsoluteTolerance", F(configuration.Solver.AbsoluteTolerance), ""), V("相对容差", "RelativeTolerance", F(configuration.Solver.RelativeTolerance), ""), V("严格残差容差", "StrictResidualTolerance", F(configuration.Solver.StrictResidualTolerance), "")));
+        groups.Add(Group("地图", V("快照版本", "ObstacleSnapshotVersion", obstacleSnapshotVersion.ToString(CultureInfo.InvariantCulture), ""), V("分辨率", "ResolutionMm", F(map.ResolutionMm), "mm"), V("X最小", "Bounds.XMin", F(map.Bounds.XMin), "mm"), V("X最大", "Bounds.XMax", F(map.Bounds.XMax), "mm"), V("Y最小", "Bounds.YMin", F(map.Bounds.YMin), "mm"), V("Y最大", "Bounds.YMax", F(map.Bounds.YMax), "mm")));
+        groups.Add(Group("可视化与换向确认", V("网页刷新率", "WebRefreshRateHz", settings.WebRefreshRateHz.ToString(CultureInfo.InvariantCulture), "Hz"), V("历史容量", "VisualizationHistoryCycleLimit", settings.VisualizationHistoryCycleLimit.ToString(CultureInfo.InvariantCulture), "cycles"), V("方向确认速度", "DirectionConfirmationSpeedMetersPerSecond", F(settings.DirectionConfirmationSpeedMetersPerSecond), "m/s"), V("方向确认样本", "DirectionConfirmationSamples", settings.DirectionConfirmationSamples.ToString(CultureInfo.InvariantCulture), "")));
+        return groups;
+    }
+
+    private static VisualizationConfigurationGroup Group(string title, params VisualizationValue[] values) => new VisualizationConfigurationGroup(title, values);
+    private static VisualizationValue V(string chinese, string raw, string value, string unit) => new VisualizationValue(chinese, raw, value, unit, "normal");
+}
@@ -19,6 +19,7 @@ internal static class TrajectoryObservationChecks
    {
        TrajectoryObservationSettingsChecks.Run();
        TrajectoryObservationSegmentChecks.Run();
+        TrajectoryObservationVisualizationChecks.Run();
        VerifiesObservationSourceHasNoActuatorCalls();
        VerifiesObservationSourceUsesRequiredOperatorText();
        VerifiesOperatorDocumentationUsesExactUiEntry();
@@ -0,0 +1,14 @@
+using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+namespace EMPlannerVerificationHost;
+
+internal static class TrajectoryObservationVisualizationChecks
+{
+    public static void Run()
+    {
+        var staticBuilder = new TrajectoryObservationStaticSnapshotBuilder();
+        var chartBuilder = new TrajectoryObservationKinematicChartBuilder();
+        Verification.True(staticBuilder != null && chartBuilder != null,
+            "observation visualization adapters are available");
+    }
+}