feat: add observation test map inputs

2026-08-04 15:40:47 +08:00
parent 13d7e51b93
commit 5df198bf69
3 changed files with 266 additions and 2 deletions
@@ -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<TrajectoryObservationObstacle> 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<IMapObstacle>(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<IMapObstacleSource> sources;
+        bool allowExplicitEmptyMap = mapObstacles.Count == 0;
+        if (allowExplicitEmptyMap)
+            sources = Array.Empty<IMapObstacleSource>();
+        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.");
+    }
+}