feat: configure observation visualization

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs

@@ -31,16 +31,26 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
     public float MapResolutionMm = 50f;
     public double ReplanPeriodSeconds = 0.20d;
     public double ObserverPeriodSeconds = 0.05d;
-    public double SolverTimeoutSeconds = 0.50d;
+    public double SolverTimeoutSeconds = 5.0d;
-    public int MaximumOsqpIterations = 12000;
+    public int MaximumOsqpIterations = 100000;
-    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 6s，不等于观察循环周期。</summary>
+    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 2s，不等于观察循环周期。</summary>
-    public double TimeHorizonSeconds = 6d;
+    public double TimeHorizonSeconds = 2d;
     /// <summary>Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。</summary>
     public double OutputTimeStepSeconds = 0.10d;
     public double VehicleLengthMeters = 0.80d;
     public double VehicleWidthMeters = 0.60d;
     public double SafetyMarginMeters = 0.05d;
     public double MaximumCurvaturePerMeter = 1d / 1.20d;
+    public bool EnableWebVisualization = false;
+    public bool AutoOpenWebVisualization = true;
+    public int WebVisualizationPort = 0;
+    public double WebRefreshRateHz = 10d;
+    public int VisualizationHistoryCycleLimit = 60;
+    public bool EnableNativePainterVisualization = false;
+    public double DirectionConfirmationSpeedMetersPerSecond = 0.02d;
+    public int DirectionConfirmationSamples = 3;
+    public double GearSwitchProjectionToleranceMeters = 0.50d;
+    public double GearSwitchStopHoldSeconds = 0.20d;
 
     public override void Test()
     {
@@ -73,6 +83,16 @@ public sealed class TrajectoryObservationMovementTest : MovementTest
                 VehicleWidthMeters = VehicleWidthMeters,
                 SafetyMarginMeters = SafetyMarginMeters,
                 MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
+                EnableWebVisualization = EnableWebVisualization,
+                AutoOpenWebVisualization = AutoOpenWebVisualization,
+                WebVisualizationPort = WebVisualizationPort,
+                WebRefreshRateHz = WebRefreshRateHz,
+                VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
+                EnableNativePainterVisualization = EnableNativePainterVisualization,
+                DirectionConfirmationSpeedMetersPerSecond = DirectionConfirmationSpeedMetersPerSecond,
+                DirectionConfirmationSamples = DirectionConfirmationSamples,
+                GearSwitchProjectionToleranceMeters = GearSwitchProjectionToleranceMeters,
+                GearSwitchStopHoldSeconds = GearSwitchStopHoldSeconds,
             };
             settings = settings.CreateValidatedSnapshot();
             TimeSpan observerPeriod = TimeSpan.FromSeconds(settings.ObserverPeriodSeconds);

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md

@@ -27,10 +27,24 @@ single S-T output duration. Only a real Goal or gear-switch boundary may publish
 | `MapResolutionMm` | mm | `50` | Local occupancy-grid resolution. |
 | `ReplanPeriodSeconds` | s | `0.20` | Minimum interval between EM planning cycles. |
 | `ObserverPeriodSeconds` | s | `0.05` | Live-state sampling and redraw interval. |
+| `SolverTimeoutSeconds` | s | `5.0` | EM solver timeout frozen for the session. |
+| `MaximumOsqpIterations` | iterations | `100000` | OSQP iteration limit frozen for the session. |
+| `TimeHorizonSeconds` | s | `2.0` | Single ST trajectory horizon, not the observer period. |
+| `OutputTimeStepSeconds` | s | `0.10` | Published trajectory timestamp spacing. |
 | `VehicleLengthMeters` | m | `0.80` | Vehicle envelope length supplied to coarse, smoothing, and EM planning. |
 | `VehicleWidthMeters` | m | `0.60` | Vehicle envelope width supplied to planning. |
 | `SafetyMarginMeters` | m | `0.05` | Additional planning clearance outside the vehicle envelope. |
 | `MaximumCurvaturePerMeter` | 1/m | `1 / 1.20` | Maximum allowed vehicle curvature (about `0.8333 1/m`). |
+| `EnableWebVisualization` | bool | `false` | Enables the local web dashboard session. |
+| `AutoOpenWebVisualization` | bool | `true` | Opens the tokenized local dashboard URI when web output is enabled. |
+| `WebVisualizationPort` | port | `0` | `0` selects an ephemeral port; otherwise use `1024` through `65535`. |
+| `WebRefreshRateHz` | Hz | `10` | Maximum dynamic web snapshot publication rate. |
+| `VisualizationHistoryCycleLimit` | cycles | `60` | Bounded web history length. |
+| `EnableNativePainterVisualization` | bool | `false` | Enables the optional native Painter fallback. |
+| `DirectionConfirmationSpeedMetersPerSecond` | m/s | `0.02` | Minimum signed-speed magnitude used to confirm the next direction. |
+| `DirectionConfirmationSamples` | samples | `3` | Strictly increasing matching-direction samples required after the stop hold. |
+| `GearSwitchProjectionToleranceMeters` | m | `0.50` | Maximum switch-point projection distance for both adjacent segments. |
+| `GearSwitchStopHoldSeconds` | s | `0.20` | Continuous real-stop duration required before direction samples count. |
 
 The four vehicle fields are public editable MovementTest inputs. They, the map/timing fields, goal, and manual obstacles
 are validated and copied into a frozen input snapshot before the background session starts. Later edits cannot change an

ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs

@@ -12,16 +12,26 @@ public sealed class TrajectoryObservationSettings
     public float MapResolutionMillimeters { get; set; } = 50f;
     public double ReplanPeriodSeconds { get; set; } = 0.20d;
     public double ObserverPeriodSeconds { get; set; } = 0.05d;
-    public double SolverTimeoutSeconds { get; set; } = 0.50d;
+    public double SolverTimeoutSeconds { get; set; } = 5d;
-    public int MaximumOsqpIterations { get; set; } = 12000;
+    public int MaximumOsqpIterations { get; set; } = 100000;
-    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；不等于观察循环周期。</summary>
+    /// <summary>单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。</summary>
-    public double TimeHorizonSeconds { get; set; } = 6d;
+    public double TimeHorizonSeconds { get; set; } = 2d;
     /// <summary>发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。</summary>
     public double OutputTimeStepSeconds { get; set; } = 0.10d;
     public double VehicleLengthMeters { get; set; } = 0.80d;
     public double VehicleWidthMeters { get; set; } = 0.60d;
     public double SafetyMarginMeters { get; set; } = 0.05d;
     public double MaximumCurvaturePerMeter { get; set; } = 1d / 1.20d;
+    public bool EnableWebVisualization { get; set; } = false;
+    public bool AutoOpenWebVisualization { get; set; } = true;
+    public int WebVisualizationPort { get; set; } = 0;
+    public double WebRefreshRateHz { get; set; } = 10d;
+    public int VisualizationHistoryCycleLimit { get; set; } = 60;
+    public bool EnableNativePainterVisualization { get; set; } = false;
+    public double DirectionConfirmationSpeedMetersPerSecond { get; set; } = 0.02d;
+    public int DirectionConfirmationSamples { get; set; } = 3;
+    public double GearSwitchProjectionToleranceMeters { get; set; } = 0.50d;
+    public double GearSwitchStopHoldSeconds { get; set; } = 0.20d;
 
     public TrajectoryObservationSettings CreateValidatedSnapshot()
     {
@@ -39,6 +49,16 @@ public sealed class TrajectoryObservationSettings
             VehicleWidthMeters = VehicleWidthMeters,
             SafetyMarginMeters = SafetyMarginMeters,
             MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
+            EnableWebVisualization = EnableWebVisualization,
+            AutoOpenWebVisualization = AutoOpenWebVisualization,
+            WebVisualizationPort = WebVisualizationPort,
+            WebRefreshRateHz = WebRefreshRateHz,
+            VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
+            EnableNativePainterVisualization = EnableNativePainterVisualization,
+            DirectionConfirmationSpeedMetersPerSecond = DirectionConfirmationSpeedMetersPerSecond,
+            DirectionConfirmationSamples = DirectionConfirmationSamples,
+            GearSwitchProjectionToleranceMeters = GearSwitchProjectionToleranceMeters,
+            GearSwitchStopHoldSeconds = GearSwitchStopHoldSeconds,
         };
         snapshot.Validate();
         return snapshot;
@@ -62,6 +82,18 @@ public sealed class TrajectoryObservationSettings
         EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters));
         EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters));
         EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter));
+        if (WebVisualizationPort != 0 && (WebVisualizationPort < 1024 || WebVisualizationPort > 65535))
+            throw new ArgumentOutOfRangeException(nameof(WebVisualizationPort),
+                "Port must be zero or in the inclusive range 1024 through 65535.");
+        EnsurePositiveFinite(WebRefreshRateHz, nameof(WebRefreshRateHz));
+        if (VisualizationHistoryCycleLimit <= 0)
+            throw new ArgumentOutOfRangeException(nameof(VisualizationHistoryCycleLimit), "Value must be positive.");
+        EnsurePositiveFinite(DirectionConfirmationSpeedMetersPerSecond,
+            nameof(DirectionConfirmationSpeedMetersPerSecond));
+        if (DirectionConfirmationSamples <= 0)
+            throw new ArgumentOutOfRangeException(nameof(DirectionConfirmationSamples), "Value must be positive.");
+        EnsurePositiveFinite(GearSwitchProjectionToleranceMeters, nameof(GearSwitchProjectionToleranceMeters));
+        EnsurePositiveFinite(GearSwitchStopHoldSeconds, nameof(GearSwitchStopHoldSeconds));
     }
 
     public VehicleParameters CreateVehicle()

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs

@@ -17,6 +17,7 @@ internal static class TrajectoryObservationChecks
 {
     public static void Run()
     {
+        TrajectoryObservationSettingsChecks.Run();
         VerifiesObservationSourceHasNoActuatorCalls();
         VerifiesObservationSourceUsesRequiredOperatorText();
         VerifiesOperatorDocumentationUsesExactUiEntry();
@@ -138,12 +139,6 @@ internal static class TrajectoryObservationChecks
         Verification.True(normalizedSource.Contains(
                 "VehicleMotionState state = ReadVehicleState();\n            DateTimeOffset now = state.CapturedAtUtc;"),
             "observer host uses the fresh state snapshot time for each observation tick");
-        Verification.NearlyEqual(0.50d, new TrajectoryObservationSettings().SolverTimeoutSeconds,
-            "observer settings use the test solver timeout default");
-        Verification.Equal(12000, new TrajectoryObservationSettings().MaximumOsqpIterations,
-            "observer settings use the test OSQP iteration default");
-        Verification.NearlyEqual(6d, new TrajectoryObservationSettings().TimeHorizonSeconds,
-            "observer settings use the ST time-horizon default");
         Verification.NearlyEqual(0.10d, new TrajectoryObservationSettings().OutputTimeStepSeconds,
             "observer settings use the ST timestamp-spacing default");
 

ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs

@@ -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);
+    }
+}