ParkingRobot/ClumsyPilot/ParkrobTrajplanner/Trajplanner_output/TrajectoryOutputDemoRunner.cs

using System;
using System.Collections.Generic;
using System.Threading;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;

namespace TrajectoryOutputDemo;

/// <summary>一次 Demo 运行的结果；成功时同时提供原始 EM 轨迹、控制只读序列和已完成的 CSV 路径。</summary>
public sealed class TrajectoryOutputDemoResult
{
    private TrajectoryOutputDemoResult(bool succeeded, string diagnostic, EmTrajectory? trajectory,
        ControlTrajectorySequence? controlTrajectory, string? csvPath)
    {
        Succeeded = succeeded;
        Diagnostic = diagnostic ?? string.Empty;
        Trajectory = trajectory;
        ControlTrajectory = controlTrajectory;
        CsvPath = csvPath;
    }

    public bool Succeeded { get; }
    public string Diagnostic { get; }
    public EmTrajectory? Trajectory { get; }
    public ControlTrajectorySequence? ControlTrajectory { get; }
    public string? CsvPath { get; }

    internal static TrajectoryOutputDemoResult Failure(string diagnostic) => new(false, diagnostic, null, null, null);
    internal static TrajectoryOutputDemoResult Success(EmTrajectory trajectory, ControlTrajectorySequence sequence, string csvPath) =>
        new(true, string.Empty, trajectory, sequence, csvPath);
}

/// <summary>
/// 编排一次真实轨迹输出：粗路径、Local G2 平滑、真实 OSQP EM 规划、控制序列投影和 CSV 导出。
/// 它只处理冻结的演示输入，绝不读取硬件状态或发送控制命令。
/// </summary>
public sealed class TrajectoryOutputDemoRunner
{
    /// <summary>在配置定义的演示场景运行一次完整规划；失败时不导出任何轨迹文件。</summary>
    public TrajectoryOutputDemoResult Run(TrajectoryOutputDemoConfiguration configuration)
    {
        if (configuration == null) throw new ArgumentNullException(nameof(configuration));

        CoarsePathPlanningJob job = CreateCoarseJob(configuration);
        TrajectoryObservationBootstrapResult bootstrap = new TrajectoryObservationBootstrapper().Bootstrap(job, CancellationToken.None);
        if (!bootstrap.Succeeded)
            return TrajectoryOutputDemoResult.Failure("粗路径或平滑阶段失败：" + bootstrap.FailureReason);
        if (bootstrap.Segments.Count == 0)
            return TrajectoryOutputDemoResult.Failure("平滑路径没有可供 EM 规划的方向段。");

        DateTimeOffset now = DateTimeOffset.UtcNow;
        DirectionSegmentView segment = bootstrap.Segments[0];
        var state = new VehicleMotionState(configuration.Start, configuration.InitialSignedSpeedMetersPerSecond,
            0d, now, 1L);
        var request = new EmPlanningRequest(bootstrap.SmoothedPath, bootstrap.Map, bootstrap.Vehicle, state,
            EmPlannerConfiguration.CreateDefault(), segment.SegmentIndex, null, now, now,
            "trajectory-output-demo-" + now.ToUnixTimeMilliseconds(), "trajectory-output-demo-reference", string.Empty,
            EmMotionModel.NonholonomicForwardReverse, EmPlanningScope.FullDirectionSegment);
        EmPlanningResult result = new EmPlanningService(new OsqpNativeSolver()).Plan(request, CancellationToken.None);
        bool accepted = (result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback) &&
            result.Trajectory != null && result.Trajectory.Points.Count > 0;
        if (!accepted)
            return TrajectoryOutputDemoResult.Failure("EM 规划失败：" + result.Status + "；" + result.FailureReason);

        EmTrajectory trajectory = result.Trajectory!;
        ControlTrajectorySequence sequence = new ControlModuleTrajectoryAdapter().Create(trajectory);
        string csvPath = new TrajectorySequenceExporter().Export(sequence, configuration.CsvOutputPath);
        return TrajectoryOutputDemoResult.Success(trajectory, sequence, csvPath);
    }

    private static CoarsePathPlanningJob CreateCoarseJob(TrajectoryOutputDemoConfiguration configuration)
    {
        return new CoarsePathPlanningJob
        {
            MapRequest = new PlanningMapRequest
            {
                Bounds = configuration.MapBounds,
                ResolutionMm = configuration.MapResolutionMillimeters,
                ObstacleSources = Array.Empty<IMapObstacleSource>(),
                AllowExplicitEmptyMap = true,
            },
            Start = configuration.Start,
            Goal = configuration.Goal,
            Vehicle = new VehicleParameters
            {
                LengthMeters = configuration.VehicleLengthMeters,
                WidthMeters = configuration.VehicleWidthMeters,
                SafetyMarginMeters = configuration.SafetyMarginMeters,
                MaximumCurvaturePerMeter = configuration.MaximumCurvaturePerMeter,
            },
            Configuration = new HybridAStarConfiguration(),
            StartDirection = TravelDirection.Forward,
            GoalDirection = GoalDirectionConstraint.Forward,
        };
    }
}