chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,161 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
+
+/// <summary>开发人员离线导出八个快速夹具和四个 Hybrid A* 端到端场景的比较报告。</summary>
+public sealed class PathSmoothingComparisonDemo
+{
+    private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
+    private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
+    private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
+    private readonly SmoothingReportExporter _reportExporter = new SmoothingReportExporter();
+
+    /// <summary>导出八个已验证夹具；不运行 Hybrid A*。</summary>
+    public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportFixtureReports(
+        string fixturePath,
+        string outputDirectory,
+        CancellationToken cancellationToken = default)
+    {
+        IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
+        IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
+        var results = new List<PathSmoothingComparisonScenarioResult>(requests.Count);
+        for (int index = 0; index < requests.Count; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            results.Add(Export(fixtures[index].Id, requests[index], outputDirectory, cancellationToken));
+        }
+        return results.AsReadOnly();
+    }
+
+    /// <summary>运行四个固定 Hybrid A* 场景，并仅为成功粗路径导出平滑比较报告。</summary>
+    public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportEndToEndReports(
+        string outputDirectory,
+        CancellationToken cancellationToken = default)
+    {
+        CoarsePathTestScenario[] scenarios =
+        {
+            CoarsePathTestScenario.ExplicitEmpty,
+            CoarsePathTestScenario.RectangleDetour,
+            CoarsePathTestScenario.ManualAndTwoLeg,
+            CoarsePathTestScenario.ReverseGearSwitch,
+        };
+        var planner = new CoarsePathPlanningService();
+        var results = new List<PathSmoothingComparisonScenarioResult>(scenarios.Length);
+        for (int index = 0; index < scenarios.Length; index++)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenarios[index]);
+            CoarsePathPlanningJobResult coarse = planner.Plan(job, cancellationToken);
+            if (coarse.PlanningResult.Status != PlanningStatus.Success)
+            {
+                results.Add(PathSmoothingComparisonScenarioResult.CoarsePathFailure(
+                    scenarios[index].ToString(), coarse.PlanningResult.Status, coarse.PlanningResult.Diagnostics.TerminationReason));
+                continue;
+            }
+            results.Add(Export(
+                scenarios[index].ToString(),
+                SmoothingScenarioFactory.CreateEndToEndRequest(job, coarse),
+                outputDirectory,
+                cancellationToken));
+        }
+        return results.AsReadOnly();
+    }
+
+    /// <summary>供业务调用示例使用：仅在已成功的粗路径上调用平滑服务。</summary>
+    public PathSmoothingResult SmoothSuccessfulCoarsePath(
+        CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult coarseResult,
+        PathSmoothingConfiguration configuration,
+        CancellationToken cancellationToken = default)
+    {
+        if (job == null || coarseResult == null || coarseResult.PlanningResult.Status != PlanningStatus.Success)
+            throw new ArgumentException("只有成功的粗路径能够进入平滑流程。", nameof(coarseResult));
+        return _smoothingService.Smooth(new PathSmoothingRequest(
+            coarseResult.PlanningResult.Path,
+            coarseResult.PlanningResult.Segments,
+            coarseResult.MapResult.Map,
+            job.Vehicle,
+            configuration), cancellationToken);
+    }
+
+    /// <summary>业务调用方只可消费成功平滑或显式回退的路径。</summary>
+    public static bool IsConsumable(PathSmoothingStatus status)
+    {
+        return status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath;
+    }
+
+    private PathSmoothingComparisonScenarioResult Export(
+        string scenarioId,
+        PathSmoothingComparisonRequest request,
+        string outputDirectory,
+        CancellationToken cancellationToken)
+    {
+        if (request == null || request.SmoothingRequest == null || request.SmoothingRequest.CoarsePath.Count == 0)
+            throw new ArgumentException("比较请求必须包含粗路径。", nameof(request));
+        PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
+        CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
+        CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
+        SmoothingFigureModel model = _figureBuilder.Build(
+            comparison,
+            request.SmoothingRequest.Map,
+            new Pose2D(first.X, first.Y, first.Heading),
+            new Pose2D(last.X, last.Y, last.Heading),
+            scenarioId,
+            scenarioId);
+        SmoothingReportExportResult report = _reportExporter.Export(new SmoothingReportExportRequest
+        {
+            Model = model,
+            OutputDirectory = System.IO.Path.Combine(outputDirectory, scenarioId),
+            FileStem = "comparison",
+        });
+        return PathSmoothingComparisonScenarioResult.ComparisonComplete(scenarioId, comparison, report);
+    }
+}
+
+/// <summary>批处理为一个场景发布的粗路径状态、平滑比较和报告结果。</summary>
+public sealed class PathSmoothingComparisonScenarioResult
+{
+    private PathSmoothingComparisonScenarioResult(
+        string scenarioId,
+        PlanningStatus coarsePathStatus,
+        string diagnostic,
+        PathSmoothingComparisonResult comparison,
+        SmoothingReportExportResult report)
+    {
+        ScenarioId = scenarioId ?? string.Empty;
+        CoarsePathStatus = coarsePathStatus;
+        Diagnostic = diagnostic ?? string.Empty;
+        Comparison = comparison;
+        Report = report;
+    }
+
+    public string ScenarioId { get; }
+    public PlanningStatus CoarsePathStatus { get; }
+    public string Diagnostic { get; }
+    public PathSmoothingComparisonResult Comparison { get; }
+    public SmoothingReportExportResult Report { get; }
+
+    internal static PathSmoothingComparisonScenarioResult CoarsePathFailure(
+        string scenarioId,
+        PlanningStatus status,
+        string diagnostic)
+    {
+        return new PathSmoothingComparisonScenarioResult(scenarioId, status, diagnostic, null, null);
+    }
+
+    internal static PathSmoothingComparisonScenarioResult ComparisonComplete(
+        string scenarioId,
+        PathSmoothingComparisonResult comparison,
+        SmoothingReportExportResult report)
+    {
+        return new PathSmoothingComparisonScenarioResult(scenarioId, PlanningStatus.Success, string.Empty, comparison, report);
+    }
+}