ParkingRobot/.task8-sweep/ParkrobTrajplanner/PathSmoothing/Test/PathSmoothingComparisonDemo.cs

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);
    }
}