From 5df198bf694a4d45085c5bf0a9c4187494315bdf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 15:40:47 +0800 Subject: [PATCH] feat: add observation test map inputs --- .../TrajectoryObservationContracts.cs | 212 ++++++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 9 +- .../TrajectoryObservationChecks.cs | 47 ++++ 3 files changed, 266 insertions(+), 2 deletions(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs new file mode 100644 index 0000000..cc08c4b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationContracts.cs @@ -0,0 +1,212 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade; +using MultiWheelC.TrajectoryPlanning.Mapping; + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +public sealed class TrajectoryObservationSettings +{ + public double MapPaddingMeters { get; set; } = 2d; + public float MapResolutionMillimeters { get; set; } = 50f; + public double ReplanPeriodSeconds { get; set; } = 0.20d; + public double ObserverPeriodSeconds { get; set; } = 0.05d; + 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 void Validate() + { + EnsurePositiveFinite(MapPaddingMeters, nameof(MapPaddingMeters)); + EnsurePositiveFinite(MapResolutionMillimeters, nameof(MapResolutionMillimeters)); + EnsurePositiveFinite(ReplanPeriodSeconds, nameof(ReplanPeriodSeconds)); + EnsurePositiveFinite(ObserverPeriodSeconds, nameof(ObserverPeriodSeconds)); + EnsurePositiveFinite(VehicleLengthMeters, nameof(VehicleLengthMeters)); + EnsurePositiveFinite(VehicleWidthMeters, nameof(VehicleWidthMeters)); + EnsurePositiveFinite(SafetyMarginMeters, nameof(SafetyMarginMeters)); + EnsurePositiveFinite(MaximumCurvaturePerMeter, nameof(MaximumCurvaturePerMeter)); + } + + public VehicleParameters CreateVehicle() + { + Validate(); + return new VehicleParameters + { + LengthMeters = VehicleLengthMeters, + WidthMeters = VehicleWidthMeters, + SafetyMarginMeters = SafetyMarginMeters, + MaximumCurvaturePerMeter = MaximumCurvaturePerMeter, + }; + } + + private static void EnsurePositiveFinite(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value) || value <= 0d) + throw new ArgumentOutOfRangeException(parameterName, "Value must be finite and positive."); + } +} + +public sealed class TrajectoryObservationObstacle +{ + private readonly bool _isCircle; + private readonly double _first; + private readonly double _second; + private readonly double _third; + private readonly double _fourth; + + private TrajectoryObservationObstacle(bool isCircle, double first, double second, double third, double fourth) + { + _isCircle = isCircle; + _first = first; + _second = second; + _third = third; + _fourth = fourth; + } + + public static TrajectoryObservationObstacle Circle(double centerXMillimeters, double centerYMillimeters, + double radiusMillimeters) + { + EnsureFinite(centerXMillimeters, nameof(centerXMillimeters)); + EnsureFinite(centerYMillimeters, nameof(centerYMillimeters)); + EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters)); + return new TrajectoryObservationObstacle(true, centerXMillimeters, centerYMillimeters, radiusMillimeters, 0d); + } + + public static TrajectoryObservationObstacle Rectangle(double xMinMillimeters, double xMaxMillimeters, + double yMinMillimeters, double yMaxMillimeters) + { + EnsureFinite(xMinMillimeters, nameof(xMinMillimeters)); + EnsureFinite(xMaxMillimeters, nameof(xMaxMillimeters)); + EnsureFinite(yMinMillimeters, nameof(yMinMillimeters)); + EnsureFinite(yMaxMillimeters, nameof(yMaxMillimeters)); + if (xMaxMillimeters <= xMinMillimeters || yMaxMillimeters <= yMinMillimeters) + throw new ArgumentOutOfRangeException(nameof(xMaxMillimeters), "Rectangle bounds must be non-degenerate."); + return new TrajectoryObservationObstacle(false, xMinMillimeters, xMaxMillimeters, yMinMillimeters, yMaxMillimeters); + } + + public MapBoundsMm GetBounds() + { + return _isCircle + ? new MapBoundsMm(ToFiniteFloat(_first - _third), ToFiniteFloat(_first + _third), + ToFiniteFloat(_second - _third), ToFiniteFloat(_second + _third)) + : new MapBoundsMm(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third), ToFiniteFloat(_fourth)); + } + + public IMapObstacle ToMapObstacle() + { + return _isCircle + ? new CircleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second), ToFiniteFloat(_third)) + : new AxisAlignedRectangleObstacle(ToFiniteFloat(_first), ToFiniteFloat(_second), + ToFiniteFloat(_third), ToFiniteFloat(_fourth)); + } + + private static float ToFiniteFloat(double value) + { + if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue) + throw new ArgumentOutOfRangeException(nameof(value), "Value cannot be represented as a finite millimeter coordinate."); + return (float)value; + } + + private static void EnsureFinite(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value)) + throw new ArgumentOutOfRangeException(parameterName, "Value must be finite."); + } + + private static void EnsurePositiveFinite(double value, string parameterName) + { + EnsureFinite(value, parameterName); + if (value <= 0d) throw new ArgumentOutOfRangeException(parameterName, "Value must be positive."); + } +} + +public static class TrajectoryObservationSetupFactory +{ + public static CoarsePathPlanningJob CreateBootstrapJob(Pose2D start, Pose2D goal, + TrajectoryObservationSettings settings, IReadOnlyList obstacles, + long obstacleSnapshotVersion) + { + if (start == null) throw new ArgumentNullException(nameof(start)); + if (goal == null) throw new ArgumentNullException(nameof(goal)); + if (settings == null) throw new ArgumentNullException(nameof(settings)); + if (obstacles == null) throw new ArgumentNullException(nameof(obstacles)); + ValidatePose(start, nameof(start)); + ValidatePose(goal, nameof(goal)); + settings.Validate(); + + double padMm = settings.MapPaddingMeters * 1000d; + var bounds = new MapBoundsMm( + ToGridLower(Math.Min(start.X, goal.X) * 1000d - padMm, settings.MapResolutionMillimeters), + ToGridUpper(Math.Max(start.X, goal.X) * 1000d + padMm, settings.MapResolutionMillimeters), + ToGridLower(Math.Min(start.Y, goal.Y) * 1000d - padMm, settings.MapResolutionMillimeters), + ToGridUpper(Math.Max(start.Y, goal.Y) * 1000d + padMm, settings.MapResolutionMillimeters)); + + var mapObstacles = new List(obstacles.Count); + for (int index = 0; index < obstacles.Count; index++) + { + TrajectoryObservationObstacle obstacle = obstacles[index] ?? throw new ArgumentNullException(nameof(obstacles)); + MapBoundsMm obstacleBounds = obstacle.GetBounds(); + if (obstacleBounds.XMin < bounds.XMin || obstacleBounds.XMax > bounds.XMax || + obstacleBounds.YMin < bounds.YMin || obstacleBounds.YMax > bounds.YMax) + throw new ArgumentOutOfRangeException(nameof(obstacles), "Obstacle envelope must fit within map bounds."); + mapObstacles.Add(obstacle.ToMapObstacle()); + } + + IReadOnlyList sources; + bool allowExplicitEmptyMap = mapObstacles.Count == 0; + if (allowExplicitEmptyMap) + sources = Array.Empty(); + else + { + if (obstacleSnapshotVersion <= 0L) + throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version."); + sources = new IMapObstacleSource[] + { + new ManualObstacleSource("trajectory-observer-manual", obstacleSnapshotVersion, true, mapObstacles), + }; + } + + return new CoarsePathPlanningJob + { + MapRequest = new PlanningMapRequest + { + Bounds = bounds, + ResolutionMm = settings.MapResolutionMillimeters, + ObstacleSources = sources, + AllowExplicitEmptyMap = allowExplicitEmptyMap, + }, + Start = start, + Goal = goal, + Vehicle = settings.CreateVehicle(), + Configuration = new HybridAStarConfiguration(), + StartDirection = null, + GoalDirection = GoalDirectionConstraint.Any, + }; + } + + private static float ToGridLower(double millimeters, float resolutionMm) + { + return ToFiniteFloat(Math.Floor(millimeters / resolutionMm) * resolutionMm, nameof(millimeters)); + } + + private static float ToGridUpper(double millimeters, float resolutionMm) + { + return ToFiniteFloat(Math.Ceiling(millimeters / resolutionMm) * resolutionMm, nameof(millimeters)); + } + + private static float ToFiniteFloat(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue) + throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate."); + return (float)value; + } + + private static void ValidatePose(Pose2D pose, string parameterName) + { + if (double.IsNaN(pose.X) || double.IsInfinity(pose.X) || double.IsNaN(pose.Y) || double.IsInfinity(pose.Y) || + double.IsNaN(pose.Heading) || double.IsInfinity(pose.Heading)) + throw new ArgumentOutOfRangeException(parameterName, "Pose values must be finite."); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 535c3c1..02670f1 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -13,9 +13,9 @@ internal static class Program args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" && args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" && args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" && - args[0] != "executor" && args[0] != "plugin-package" && args[0] != "em-all")) + args[0] != "executor" && args[0] != "plugin-package" && args[0] != "trajectory-observation" && args[0] != "em-all")) { - Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|em-all"); + Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|trajectory-observation|em-all"); return 2; } @@ -124,6 +124,11 @@ internal static class Program PluginPackagingChecks.Run(); Console.WriteLine("PASS plugin-package"); } + if (args[0] == "trajectory-observation" || args[0] == "em-all") + { + TrajectoryObservationChecks.Run(); + Console.WriteLine("PASS trajectory-observation"); + } if (args[0] == "em-all") { CoordinatorChecks.RunRollingEndToEnd(); diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs new file mode 100644 index 0000000..288f28d --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -0,0 +1,47 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade; +using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +namespace EMPlannerVerificationHost; + +internal static class TrajectoryObservationChecks +{ + public static void Run() + { + VerifiesStartGoalBoundsUseOnlyConfiguredPadding(); + RejectsObstacleOutsideConfiguredBounds(); + } + + private static void VerifiesStartGoalBoundsUseOnlyConfiguredPadding() + { + var settings = new TrajectoryObservationSettings + { + MapPaddingMeters = 2d, + MapResolutionMillimeters = 50f, + }; + CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob( + new Pose2D(10d, -5d, 0d), new Pose2D(13d, -1d, 0d), settings, + Array.Empty(), 17L); + + Verification.NearlyEqual(8000d, job.MapRequest.Bounds.XMin, "observer map x min"); + Verification.NearlyEqual(15000d, job.MapRequest.Bounds.XMax, "observer map x max"); + Verification.NearlyEqual(-7000d, job.MapRequest.Bounds.YMin, "observer map y min"); + Verification.NearlyEqual(1000d, job.MapRequest.Bounds.YMax, "observer map y max"); + Verification.NearlyEqual(50d, job.MapRequest.ResolutionMm, "observer map resolution"); + } + + private static void RejectsObstacleOutsideConfiguredBounds() + { + Verification.True(Throws(() => TrajectoryObservationSetupFactory.CreateBootstrapJob( + new Pose2D(0d, 0d, 0d), new Pose2D(1d, 1d, 0d), new TrajectoryObservationSettings(), + new[] { TrajectoryObservationObstacle.Rectangle(-2100d, -2000d, 0d, 100d) }, 1L)), + "observer obstacle outside configured bounds"); + } + + private static bool Throws(Action action) + { + try { action(); return false; } + catch (ArgumentOutOfRangeException) { return true; } + } +}