ParkingRobot/ClumsyPilot/ParkrobTrajplanner/Trajplanner_guide/EmPlannerTrajectoryReplacementExample.cs

// 此文件是供“停车机器人-合并测试”闭环测试维护者复制阅读的替换示例，
// 不属于可执行项目，也不读取实车定位或向底盘发送命令。
// 只允许替换轨迹来源；状态源、控制参数、观测器和安全策略仍由原测试拥有。

using System;
using System.Threading;
using ControllerStateProvider = MultiWheelC.StateEstimation.IVehicleStateProvider;
using MultiWheelC.TrajectoryPlanning.EMPlanner;
using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
using MyParking.Shared;
using Trajectory2D = MultiWheelC.Trajectory.Trajectory2D;

namespace MultiWheelC.TrajectoryPlanning.Guide;

/// <summary>
/// 闭环控制测试中从人工轨迹切换到 EM 正式轨迹的逐段对照。
/// </summary>
public static class EmPlannerTrajectoryReplacementExample
{
    // 替换前：NewControllerTrackingTests 中的完整人工测试轨迹来源。
    // 它只生成控制器测试数据，不调用正式规划器、地图构建、粗路径、平滑或 EM 服务。
    private static void ArtificialTrajectorySource(
        Pose2D trajectoryStartPose,
        CancellationToken cancellationToken)
    {
        _ = cancellationToken; // 此人工来源不使用取消令牌，保留形参仅便于与替换后位置对照。

        Trajectory2D trajectory =
            TestTrajectoryFactory.CreateStraight4Meters(
                trajectoryStartPose,
                CruiseSpeedMetersPerSecond,
                AccelerationMetersPerSecondSquared,
                DecelerationMetersPerSecondSquared,
                PointSpacingMeters);

        _ = trajectory;
    }

    // 替换后（推荐）：复用 Task 2 的固定离线完整管线输出。
    // Task 2 内部同步调用 EmPlanningService.Plan(request, cancellationToken)，
    // 并在返回前完成 Success/SuccessWithFallback、非空轨迹和可控点的发布闸门检查。
    private static void WrapperReplacement(
        ControllerStateProvider stateProvider,
        bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
        CancellationToken cancellationToken)
    {
        FixedOfflinePlanningOutput planningOutput =
            EmPlannerFullPipelineDemo.CreateFixedOfflineTrajectory(
                cancellationToken);

        EmTrajectory emTrajectory = planningOutput.EmTrajectory;
        Trajectory2D trajectory = planningOutput.ControllerTrajectory;

        if (emTrajectory == null || trajectory == null)
        {
            throw new InvalidOperationException(
                "固定离线 EM 包装器未返回完整控制轨迹。");
        }

        EnsureSingleDirectionTrackingIsAuthorized(
            emTrajectory,
            gearSwitchHandoffIsSafetyReviewedAndAuthorized);

        // 原测试已有的状态源继续交给动作；仅 trajectory 的来源由人工数据改为 EM 输出。
        var movement = new TrajectoryTrackingMovement
        {
            Trajectory = trajectory,
            StateProvider = stateProvider
        };

        // 若原测试使用组合动作计划，则使用同一条已适配的控制轨迹创建段。
        var segment = new TrackMotionPlanSegment(trajectory);

        // 若本段以 GearSwitch 结束，授权只允许跟踪当前方向段到精确停车。
        // 动作完成后，外部状态机必须确认已停稳和下一方向，再单独请求/执行下一段。
        // 绝不把两个方向的点串接起来，也不自动跨越换向边界。

        _ = movement;
        _ = segment;
    }

    // 直接服务方案：当测试已按当前地图、车辆、状态和活动方向段构造冻结的 EmPlanningRequest 时使用。
    // “请求”就是进程内同步方法调用：Plan 的返回值就是本次 EmPlanningResult；
    // 没有轮询、HTTP、消息队列或轨迹仓库查询，也不能从其他调用的结果中取轨迹。
    private static void DirectServiceReplacement(
        EmPlanningRequest request,
        ControllerStateProvider stateProvider,
        bool gearSwitchHandoffIsSafetyReviewedAndAuthorized,
        CancellationToken cancellationToken)
    {
        var planningService = new EmPlanningService(new OsqpNativeSolver());
        EmPlanningResult result = planningService.Plan(request, cancellationToken);

        bool succeeded = result.Status == EmPlanningStatus.Success ||
            result.Status == EmPlanningStatus.SuccessWithFallback;
        if (!succeeded || result.Trajectory == null)
        {
            // 失败、取消、超时或空轨迹不得退化为人工轨迹后继续执行；由测试自己的安全策略停止或报告。
            throw new InvalidOperationException(
                "EM 规划未返回可交给控制器的完整轨迹：" + result.FailureReason);
        }

        EmTrajectory emTrajectory = result.Trajectory;
        EnsureSingleDirectionTrackingIsAuthorized(
            emTrajectory,
            gearSwitchHandoffIsSafetyReviewedAndAuthorized);

        // 适配器使用 EM 的世界位姿、车辆曲率和有符号纵向速度，重建 Trajectory2D 的弧长。
        Trajectory2D trajectory =
            new EmControlTrajectoryAdapter().Create(result.Trajectory);

        // 直接单段消费路径：不要在这里覆盖原测试已有的控制器调参、完成容差、观测器或安全检查。
        var movement = new TrajectoryTrackingMovement
        {
            Trajectory = trajectory,
            StateProvider = stateProvider
        };

        // 组合计划消费路径：原测试的 ConfigureTrackingMovement、每段 finish tolerances、
        // SegmentStarted/TrackingCycleObserver 等观测器以及状态与安全检查仍保持测试所有权。
        var segment = new TrackMotionPlanSegment(trajectory);

        // GearSwitch 授权不是跨边界授权：只跟踪本次返回的完整方向段到精确停车。
        // 停稳后由外部状态机确认方向，然后发起一次新请求并创建新的单段动作。
        // 不得拼接方向点，不得在当前 movement/segment 中自动继续到下一段。

        _ = movement;
        _ = segment;
    }

    // 单个 TrajectoryTrackingMovement 只能消费一个方向段。
    // 完整方向轨迹以 GearSwitch 结束并不代表轨迹本身跨界：它可安全跟踪到边界精确停车。
    // 但调用方必须显式传入已通过安全评审的换向交接授权；未授权时在跟踪前拒绝。
    private static void EnsureSingleDirectionTrackingIsAuthorized(
        EmTrajectory emTrajectory,
        bool gearSwitchHandoffIsSafetyReviewedAndAuthorized)
    {
        if (emTrajectory == null)
        {
            throw new InvalidOperationException(
                "EM 规划输出为空，不能创建 TrajectoryTrackingMovement。");
        }

        if (emTrajectory.Metadata.TerminalType != EmTerminalType.GearSwitch)
        {
            return;
        }

        if (emTrajectory.Metadata.PlanningScope != EmPlanningScope.FullDirectionSegment ||
            emTrajectory.Metadata.LongitudinalMode != EmLongitudinalMode.ExactStopAtBoundary)
        {
            throw new InvalidOperationException(
                "GearSwitch 只能作为 FullDirectionSegment 的 ExactStopAtBoundary 末端交给单段跟踪。");
        }

        if (!gearSwitchHandoffIsSafetyReviewedAndAuthorized)
        {
            throw new InvalidOperationException(
                "GearSwitch 方向段需要调用方显式提供已通过安全评审的换向交接授权；" +
                "未授权时不得创建跟踪动作。");
        }

        // 通过检查只授权当前段执行到精确停车。外部状态机仍必须在停稳后
        // 确认下一方向并另行请求/执行下一段；不得拼接点或自动跨越边界。
    }

    // CruiseSpeedMetersPerSecond、AccelerationMetersPerSecondSquared、
    // DecelerationMetersPerSecondSquared 和 PointSpacingMeters 是外部现有测试类的成员；
    // 复制替换时保留其原值和测试所有权，本说明文件不会重新定义或修改它们。
}