feat: configure observation visualization

2026-08-06 10:16:48 +08:00
parent 0bc8df9d5e
commit c5ea4f6d91
5 changed files with 201 additions and 14 deletions
@@ -0,0 +1,126 @@
+using System;
+using System.IO;
+using System.Text;
+using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+
+namespace EMPlannerVerificationHost;
+
+internal static class TrajectoryObservationSettingsChecks
+{
+    public static void Run()
+    {
+        VerifiesDefaults();
+        VerifiesValidatedSnapshotFreezesVisualizationSettings();
+        RejectsInvalidVisualizationSettings();
+        VerifiesMovementTestMapsEveryFrozenSetting();
+    }
+
+    private static void VerifiesDefaults()
+    {
+        var defaults = new TrajectoryObservationSettings();
+
+        Verification.NearlyEqual(5d, defaults.SolverTimeoutSeconds, "observer solver timeout default");
+        Verification.Equal(100000, defaults.MaximumOsqpIterations, "observer OSQP default");
+        Verification.NearlyEqual(2d, defaults.TimeHorizonSeconds, "observer ST horizon default");
+        Verification.True(!defaults.EnableWebVisualization, "web defaults off");
+        Verification.True(defaults.AutoOpenWebVisualization, "web auto-open defaults on");
+        Verification.True(!defaults.EnableNativePainterVisualization, "Painter defaults off");
+        Verification.Equal(0, defaults.WebVisualizationPort, "dynamic port default");
+        Verification.NearlyEqual(10d, defaults.WebRefreshRateHz, "web refresh default");
+        Verification.Equal(60, defaults.VisualizationHistoryCycleLimit, "history default");
+        Verification.NearlyEqual(0.02d, defaults.DirectionConfirmationSpeedMetersPerSecond,
+            "direction speed threshold");
+        Verification.Equal(3, defaults.DirectionConfirmationSamples, "direction sample default");
+        Verification.NearlyEqual(0.50d, defaults.GearSwitchProjectionToleranceMeters,
+            "gear projection default");
+        Verification.NearlyEqual(0.20d, defaults.GearSwitchStopHoldSeconds, "gear stop hold default");
+    }
+
+    private static void VerifiesValidatedSnapshotFreezesVisualizationSettings()
+    {
+        var source = new TrajectoryObservationSettings
+        {
+            EnableWebVisualization = true,
+            AutoOpenWebVisualization = false,
+            WebVisualizationPort = 2048,
+            WebRefreshRateHz = 15d,
+            VisualizationHistoryCycleLimit = 17,
+            EnableNativePainterVisualization = true,
+            DirectionConfirmationSpeedMetersPerSecond = 0.03d,
+            DirectionConfirmationSamples = 4,
+            GearSwitchProjectionToleranceMeters = 0.6d,
+            GearSwitchStopHoldSeconds = 0.3d,
+        };
+        TrajectoryObservationSettings snapshot = source.CreateValidatedSnapshot();
+
+        source.EnableWebVisualization = false;
+        source.AutoOpenWebVisualization = true;
+        source.WebVisualizationPort = 0;
+        source.WebRefreshRateHz = 12d;
+        source.VisualizationHistoryCycleLimit = 18;
+        source.EnableNativePainterVisualization = false;
+        source.DirectionConfirmationSpeedMetersPerSecond = 0.04d;
+        source.DirectionConfirmationSamples = 5;
+        source.GearSwitchProjectionToleranceMeters = 0.7d;
+        source.GearSwitchStopHoldSeconds = 0.4d;
+
+        Verification.True(snapshot.EnableWebVisualization, "snapshot freezes web switch");
+        Verification.True(!snapshot.AutoOpenWebVisualization, "snapshot freezes web auto-open switch");
+        Verification.Equal(2048, snapshot.WebVisualizationPort, "snapshot freezes web port");
+        Verification.NearlyEqual(15d, snapshot.WebRefreshRateHz, "snapshot freezes web refresh");
+        Verification.Equal(17, snapshot.VisualizationHistoryCycleLimit, "snapshot freezes history limit");
+        Verification.True(snapshot.EnableNativePainterVisualization, "snapshot freezes Painter switch");
+        Verification.NearlyEqual(0.03d, snapshot.DirectionConfirmationSpeedMetersPerSecond,
+            "snapshot freezes direction speed");
+        Verification.Equal(4, snapshot.DirectionConfirmationSamples, "snapshot freezes direction samples");
+        Verification.NearlyEqual(0.6d, snapshot.GearSwitchProjectionToleranceMeters,
+            "snapshot freezes gear projection tolerance");
+        Verification.NearlyEqual(0.3d, snapshot.GearSwitchStopHoldSeconds, "snapshot freezes gear stop hold");
+    }
+
+    private static void RejectsInvalidVisualizationSettings()
+    {
+        AssertInvalid(settings => settings.WebVisualizationPort = -1, "negative web port");
+        AssertInvalid(settings => settings.WebVisualizationPort = 1, "low reserved web port");
+        AssertInvalid(settings => settings.WebVisualizationPort = 1023, "last reserved web port");
+        AssertInvalid(settings => settings.WebVisualizationPort = 65536, "overflow web port");
+        AssertInvalid(settings => settings.WebRefreshRateHz = 0d, "zero web refresh");
+        AssertInvalid(settings => settings.VisualizationHistoryCycleLimit = 0, "zero history limit");
+        AssertInvalid(settings => settings.DirectionConfirmationSpeedMetersPerSecond = 0d,
+            "zero direction speed threshold");
+        AssertInvalid(settings => settings.DirectionConfirmationSamples = 0, "zero direction samples");
+        AssertInvalid(settings => settings.GearSwitchProjectionToleranceMeters = 0d,
+            "zero gear projection tolerance");
+        AssertInvalid(settings => settings.GearSwitchStopHoldSeconds = 0d, "zero gear stop hold");
+    }
+
+    private static void VerifiesMovementTestMapsEveryFrozenSetting()
+    {
+        string path = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ParkrobTrajplanner",
+            "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs");
+        string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(path));
+        foreach (string field in new[]
+        {
+            "EnableWebVisualization", "AutoOpenWebVisualization", "WebVisualizationPort", "WebRefreshRateHz",
+            "VisualizationHistoryCycleLimit", "EnableNativePainterVisualization",
+            "DirectionConfirmationSpeedMetersPerSecond", "DirectionConfirmationSamples",
+            "GearSwitchProjectionToleranceMeters", "GearSwitchStopHoldSeconds",
+        })
+        {
+            Verification.True(source.Contains("public ") && source.Contains(" " + field + " ="),
+                "MovementTest exposes " + field);
+            Verification.True(source.Contains(field + " = " + field + ","),
+                "MovementTest maps " + field + " into settings");
+        }
+    }
+
+    private static void AssertInvalid(Action<TrajectoryObservationSettings> mutate, string name)
+    {
+        var settings = new TrajectoryObservationSettings();
+        mutate(settings);
+        bool threw = false;
+        try { settings.Validate(); }
+        catch (ArgumentOutOfRangeException) { threw = true; }
+        Verification.True(threw, "observer rejects invalid " + name);
+    }
+}