test: add path smoothing scenarios and fixtures

2026-07-30 08:43:35 +08:00
parent 20215defdd
commit 78722383bf
8 changed files with 20454 additions and 0 deletions
@@ -0,0 +1,69 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
+
+/// <summary>将快速夹具或现有粗路径业务结果转换为独立平滑比较请求。</summary>
+public static class SmoothingScenarioFactory
+{
+    /// <summary>不运行 Hybrid A*，从已验证的 JSON 夹具创建八个快速比较请求。</summary>
+    public static IReadOnlyList<PathSmoothingComparisonRequest> CreateFixtureRequests(string fixturePath)
+    {
+        IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
+        var requests = new List<PathSmoothingComparisonRequest>(fixtures.Count);
+        for (int index = 0; index < fixtures.Count; index++)
+        {
+            SmoothingScenarioFixture fixture = fixtures[index];
+            requests.Add(new PathSmoothingComparisonRequest(
+                new PathSmoothingRequest(
+                    fixture.Path,
+                    fixture.Segments,
+                    SmoothingScenarioFixtureLoader.BuildMap(fixture),
+                    SmoothingScenarioFixtureLoader.BuildVehicle(fixture),
+                    new PathSmoothingConfiguration())));
+        }
+        return requests.AsReadOnly();
+    }
+
+    /// <summary>把现有 <see cref="CoarsePathScenarioFactory"/> 已规划成功的结果转换为比较请求。</summary>
+    public static PathSmoothingComparisonRequest CreateEndToEndRequest(
+        CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult result)
+    {
+        if (job == null || result == null || result.MapResult == null || result.MapResult.Map == null ||
+            result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
+            throw new ArgumentException("只能从包含成功地图和粗路径的业务结果创建平滑比较请求。", nameof(result));
+
+        return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
+            CopyWithFiniteClearance(result.PlanningResult.Path, result.MapResult.Map),
+            result.PlanningResult.Segments,
+            result.MapResult.Map,
+            job.Vehicle,
+            new PathSmoothingConfiguration()));
+    }
+
+    internal static IReadOnlyList<CoarsePathPoint> CopyWithFiniteClearance(
+        IReadOnlyList<CoarsePathPoint> source,
+        MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap map)
+    {
+        double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
+        double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
+        double finiteEmptyMapClearance = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
+        var copy = new List<CoarsePathPoint>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+        {
+            CoarsePathPoint point = source[index];
+            double clearance = double.IsPositiveInfinity(point.BodyClearance)
+                ? finiteEmptyMapClearance
+                : point.BodyClearance;
+            copy.Add(new CoarsePathPoint(
+                point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
+                point.VehicleCurvature, clearance, point.IsGearSwitchPoint, point.Source));
+        }
+        return copy.AsReadOnly();
+    }
+}