diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index 4cdc8e7..4c089e8 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/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 int MaximumOsqpIterations = 12000; - /// 单次 ST 轨迹覆盖的未来时长，单位 s；默认 6s，不等于观察循环周期。 - public double TimeHorizonSeconds = 6d; + public double SolverTimeoutSeconds = 5.0d; + public int MaximumOsqpIterations = 100000; + /// 单次 ST 轨迹覆盖的未来时长，单位 s；默认 2s，不等于观察循环周期。 + public double TimeHorizonSeconds = 2d; /// Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；默认 0.10s，不等于观察循环周期。 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); diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md index 0bda336..2c3d463 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md +++ b/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 diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs index ac544e6..2436512 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs +++ b/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 int MaximumOsqpIterations { get; set; } = 12000; - /// 单次 ST 轨迹覆盖的未来时长，单位 s；不等于观察循环周期。 - public double TimeHorizonSeconds { get; set; } = 6d; + public double SolverTimeoutSeconds { get; set; } = 5d; + public int MaximumOsqpIterations { get; set; } = 100000; + /// 单次 ST 轨迹覆盖的未来时长，单位 s；默认 2 s，不等于观察循环周期。 + public double TimeHorizonSeconds { get; set; } = 2d; /// 发布 Trajectory 相邻 TimeFromStart 时间戳的间隔，单位 s；不等于观察循环周期。 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() diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index a47fde0..deaa55b 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/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"); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationSettingsChecks.cs new file mode 100644 index 0000000..d3189fa --- /dev/null +++ b/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 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); + } +}