From 1a94cc2a6b9e1209ccd3ea0d95ded31ce2f723c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 5 Aug 2026 12:03:28 +0800 Subject: [PATCH] feat: configure MovementTest OSQP iterations --- .../MovementTest.TrajectoryObservationTest.cs | 2 ++ .../TrajectoryObservationContracts.cs | 4 ++++ .../TrajectoryObservationPipeline.cs | 1 + .../TrajectoryObservationChecks.cs | 17 ++++++++++++++++- 4 files changed, 23 insertions(+), 1 deletion(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index ccef5d2..1eb715d 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -32,6 +32,7 @@ public sealed class TrajectoryObservationMovementTest : MovementTest public double ReplanPeriodSeconds = 0.20d; public double ObserverPeriodSeconds = 0.05d; public double SolverTimeoutSeconds = 0.50d; + public int MaximumOsqpIterations = 12000; public double VehicleLengthMeters = 0.80d; public double VehicleWidthMeters = 0.60d; public double SafetyMarginMeters = 0.05d; @@ -61,6 +62,7 @@ public sealed class TrajectoryObservationMovementTest : MovementTest ReplanPeriodSeconds = ReplanPeriodSeconds, ObserverPeriodSeconds = ObserverPeriodSeconds, SolverTimeoutSeconds = SolverTimeoutSeconds, + MaximumOsqpIterations = MaximumOsqpIterations, VehicleLengthMeters = VehicleLengthMeters, VehicleWidthMeters = VehicleWidthMeters, SafetyMarginMeters = SafetyMarginMeters, diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs index a82038f..e028f3b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs @@ -13,6 +13,7 @@ public sealed class TrajectoryObservationSettings 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; public double VehicleLengthMeters { get; set; } = 0.80d; public double VehicleWidthMeters { get; set; } = 0.60d; public double SafetyMarginMeters { get; set; } = 0.05d; @@ -27,6 +28,7 @@ public sealed class TrajectoryObservationSettings ReplanPeriodSeconds = ReplanPeriodSeconds, ObserverPeriodSeconds = ObserverPeriodSeconds, SolverTimeoutSeconds = SolverTimeoutSeconds, + MaximumOsqpIterations = MaximumOsqpIterations, VehicleLengthMeters = VehicleLengthMeters, VehicleWidthMeters = VehicleWidthMeters, SafetyMarginMeters = SafetyMarginMeters, @@ -43,6 +45,8 @@ public sealed class TrajectoryObservationSettings EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds)); EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds)); EnsurePositiveFinite(SolverTimeoutSeconds, nameof(SolverTimeoutSeconds)); + if (MaximumOsqpIterations <= 0) + throw new ArgumentOutOfRangeException(nameof(MaximumOsqpIterations), "Value must be positive."); EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters)); EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters)); EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters)); diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs index 78c3540..51534a1 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationPipeline.cs @@ -208,6 +208,7 @@ public sealed class TrajectoryObservationController configuration = EmPlannerConfiguration.CreateDefault(); configuration.Scheduling.ReplanPeriodSeconds = settings.ReplanPeriodSeconds; configuration.Scheduling.SolverTimeoutSeconds = settings.SolverTimeoutSeconds; + configuration.Solver.MaximumOsqpIterations = settings.MaximumOsqpIterations; coordinator = new EmPlanningCoordinator(planningService); executor = new TrajectoryExecutor(configuration); this.sessionId = sessionId; diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 239a28d..36a4aed 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -108,6 +108,8 @@ internal static class TrajectoryObservationChecks "observer MovementTest exposes maximum curvature with Task-1 default"); Verification.True(source.Contains("public double SolverTimeoutSeconds = 0.50d;"), "observer MovementTest exposes the test solver timeout default"); + Verification.True(source.Contains("public int MaximumOsqpIterations = 12000;"), + "observer MovementTest exposes the test OSQP iteration default"); Verification.True(source.Contains("VehicleLengthMeters = VehicleLengthMeters,"), "observer MovementTest copies vehicle length into settings"); Verification.True(source.Contains("VehicleWidthMeters = VehicleWidthMeters,"), @@ -118,12 +120,16 @@ internal static class TrajectoryObservationChecks "observer MovementTest copies maximum curvature into settings"); Verification.True(source.Contains("SolverTimeoutSeconds = SolverTimeoutSeconds,"), "observer MovementTest copies solver timeout into settings"); + Verification.True(source.Contains("MaximumOsqpIterations = MaximumOsqpIterations,"), + "observer MovementTest copies OSQP iteration limit into settings"); string normalizedSource = source.Replace("\r\n", "\n"); 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"); var configured = new TrajectoryObservationSettings { @@ -132,6 +138,7 @@ internal static class TrajectoryObservationChecks SafetyMarginMeters = 0.08d, MaximumCurvaturePerMeter = 0.55d, SolverTimeoutSeconds = 0.42d, + MaximumOsqpIterations = 9000, }; TrajectoryObservationSettings snapshot = configured.CreateValidatedSnapshot(); configured.VehicleLengthMeters = 9.10d; @@ -139,6 +146,7 @@ internal static class TrajectoryObservationChecks configured.SafetyMarginMeters = 9.30d; configured.MaximumCurvaturePerMeter = 9.40d; configured.SolverTimeoutSeconds = 9.50d; + configured.MaximumOsqpIterations = 9500; Verification.NearlyEqual(1.10d, snapshot.VehicleLengthMeters, "observer settings snapshot freezes vehicle length"); @@ -150,6 +158,8 @@ internal static class TrajectoryObservationChecks "observer settings snapshot freezes maximum curvature"); Verification.NearlyEqual(0.42d, snapshot.SolverTimeoutSeconds, "observer settings snapshot freezes solver timeout"); + Verification.Equal(9000, snapshot.MaximumOsqpIterations, + "observer settings snapshot freezes OSQP iteration limit"); CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), snapshot, @@ -171,6 +181,8 @@ internal static class TrajectoryObservationChecks AssertInvalidSetting(settings => settings.MaximumCurvaturePerMeter = double.PositiveInfinity, "maximum curvature"); AssertInvalidSetting(settings => settings.SolverTimeoutSeconds = 0d, "solver timeout"); + AssertInvalidSetting(settings => settings.MaximumOsqpIterations = 0, "OSQP iteration limit"); + AssertInvalidSetting(settings => settings.MaximumOsqpIterations = -1, "negative OSQP iteration limit"); } private static void AssertInvalidSetting(Action mutate, string name) @@ -392,7 +404,7 @@ internal static class TrajectoryObservationChecks Verification.NearlyEqual(10.25d, charts.LsSamples[0].PathS, "observer first LS path S"); Verification.NearlyEqual(0.10d, charts.LsSamples[0].LateralOffset, "observer first LS offset"); - var settings = new TrajectoryObservationSettings(); + var settings = new TrajectoryObservationSettings { MaximumOsqpIterations = 9000 }; CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( new Pose2D(0d, 0d, 0d), new Pose2D(1d, 0d, 0d), settings, Array.Empty(), 0L); @@ -416,6 +428,9 @@ internal static class TrajectoryObservationChecks Verification.NearlyEqual(settings.SolverTimeoutSeconds, planningService.Requests[0].Configuration.Scheduling.SolverTimeoutSeconds, "observer configured solver timeout"); + Verification.Equal(settings.MaximumOsqpIterations, + planningService.Requests[0].Configuration.Solver.MaximumOsqpIterations, + "observer configured OSQP iteration limit"); TrajectoryObservationObservation observation = controller.Observe(effectiveAt.AddSeconds(0.5d), state); Verification.NearlyEqual(0.5d, observation.SelectedPoint.TimeFromStart,