feat: 发布 EM 轨迹规划首个版本

2026-08-11 20:35:59 +08:00
parent 569de5f13c
commit 1903e71fc1
522 changed files with 4188 additions and 119188 deletions
@@ -0,0 +1,697 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.Threading;
+using System.Threading.Tasks;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MDCSToolBox.Clumsy.Pilot;
+using MultiWheelC.Trajectory;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.EMPlanner;
+using MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
+using MyParking.Shared;
+using TrajectoryPlanningVisualization;
+using PlanningPose2D = MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D;
+
+namespace MultiWheelC;
+
+/// <summary>规划一次并冻结首个 EM 方向段，再使用现有几何控制器完成实车状态反馈闭环。</summary>
+[MovementTest(name = "EM闭环测试")]
+public sealed class EmClosedLoopMovementTest : MovementTest
+{
+    private const int MaximumManualObstacleCount = 20;
+    private static long nextObstacleSnapshotVersion;
+
+    public double GoalXmm = double.NaN;
+    public double GoalYmm = double.NaN;
+    public double GoalYawDeg;
+    public double MapPaddingMeters = 2d;
+    public float MapResolutionMm = 50f;
+    public double SolverTimeoutSeconds = 5d;
+    public int MaximumOsqpIterations = 100000;
+    public double OutputTimeStepSeconds = 0.10d;
+    public double VehicleLengthMeters = 0.80d;
+    public double VehicleWidthMeters = 0.60d;
+    public double SafetyMarginMeters = 0.05d;
+    public double MaximumCurvaturePerMeter = 1d / 1.20d;
+    public bool EnableWebVisualization = true;
+    public bool AutoOpenWebVisualization = true;
+    public int WebVisualizationPort;
+    public double WebRefreshRateHz = 10d;
+    public int VisualizationHistoryCycleLimit = 60;
+    public bool EnableNativePainterVisualization = true;
+    public double MaximumCommandSpeedMetersPerSecond = 1.00d;
+    public double MaximumDistanceToTrajectoryMeters = 0.30d;
+    public double ExecutionTimeoutSeconds = 120d;
+    public float WheelAlignmentToleranceDegrees = 2f;
+
+    /// <summary>读取冻结输入并启动一次规划、一次控制器接管的后台会话。</summary>
+    public override void Test()
+    {
+        try
+        {
+            double goalXMillimeters = GoalXmm;
+            double goalYMillimeters = GoalYmm;
+            double goalYawDegrees = GoalYawDeg;
+            if (!IsFinite(goalXMillimeters) || !IsFinite(goalYMillimeters))
+            {
+                goalXMillimeters = ReadFiniteInput("EM闭环终点 X（世界 mm）");
+                goalYMillimeters = ReadFiniteInput("EM闭环终点 Y（世界 mm）");
+                goalYawDegrees = ReadFiniteInput("EM闭环终点航向（世界 deg）");
+            }
+
+            EnsureFinite(goalXMillimeters, nameof(GoalXmm));
+            EnsureFinite(goalYMillimeters, nameof(GoalYmm));
+            EnsureFinite(goalYawDegrees, nameof(GoalYawDeg));
+            EnsurePositive(MaximumCommandSpeedMetersPerSecond,
+                nameof(MaximumCommandSpeedMetersPerSecond));
+            EnsurePositive(MaximumDistanceToTrajectoryMeters,
+                nameof(MaximumDistanceToTrajectoryMeters));
+            EnsurePositive(ExecutionTimeoutSeconds, nameof(ExecutionTimeoutSeconds));
+            EnsurePositive(WheelAlignmentToleranceDegrees,
+                nameof(WheelAlignmentToleranceDegrees));
+
+            var settings = new TrajectoryObservationSettings
+            {
+                PlanningScope = EmPlanningScope.FullDirectionSegment,
+                MapPaddingMeters = MapPaddingMeters,
+                MapResolutionMillimeters = MapResolutionMm,
+                SolverTimeoutSeconds = SolverTimeoutSeconds,
+                MaximumOsqpIterations = MaximumOsqpIterations,
+                OutputTimeStepSeconds = OutputTimeStepSeconds,
+                VehicleLengthMeters = VehicleLengthMeters,
+                VehicleWidthMeters = VehicleWidthMeters,
+                SafetyMarginMeters = SafetyMarginMeters,
+                MaximumCurvaturePerMeter = MaximumCurvaturePerMeter,
+                EnableWebVisualization = EnableWebVisualization,
+                AutoOpenWebVisualization = AutoOpenWebVisualization,
+                WebVisualizationPort = WebVisualizationPort,
+                WebRefreshRateHz = WebRefreshRateHz,
+                VisualizationHistoryCycleLimit = VisualizationHistoryCycleLimit,
+                EnableNativePainterVisualization = EnableNativePainterVisualization,
+            }.CreateValidatedSnapshot();
+
+            IReadOnlyList<TrajectoryObservationObstacle> obstacles = ReadManualObstacles();
+            long obstacleSnapshotVersion = obstacles.Count == 0
+                ? 0L
+                : Interlocked.Increment(ref nextObstacleSnapshotVersion);
+            var goal = new PlanningPose2D(
+                goalXMillimeters / 1000d,
+                goalYMillimeters / 1000d,
+                goalYawDegrees * Math.PI / 180d);
+
+            EmClosedLoopMovementTestRunner.Start(goal, settings, obstacles,
+                obstacleSnapshotVersion, MaximumCommandSpeedMetersPerSecond,
+                MaximumDistanceToTrajectoryMeters, ExecutionTimeoutSeconds,
+                WheelAlignmentToleranceDegrees);
+        }
+        catch (Exception exception)
+        {
+            EmClosedLoopMovementTestRunner.ShowFailure("测试未启动", exception);
+        }
+    }
+
+    /// <summary>取消尚未完成的规划并停止正在运行的控制任务。</summary>
+    public override void TestStop()
+    {
+        EmClosedLoopMovementTestRunner.Stop();
+    }
+
+    private static IReadOnlyList<TrajectoryObservationObstacle> ReadManualObstacles()
+    {
+        int count = ReadBoundedIntegerInput("EM闭环手动障碍物数量（0-20）",
+            0, MaximumManualObstacleCount);
+        var obstacles = new List<TrajectoryObservationObstacle>(count);
+        for (int index = 0; index < count; index++)
+        {
+            string label = "障碍物 " + (index + 1).ToString(CultureInfo.InvariantCulture);
+            int kind = ReadBoundedIntegerInput(label + " 类型（1=圆形，2=轴对齐矩形）", 1, 2);
+            double centerXMillimeters = ReadFiniteInput(label + " 中心 X（世界 mm）");
+            double centerYMillimeters = ReadFiniteInput(label + " 中心 Y（世界 mm）");
+            if (kind == 1)
+            {
+                double radiusMillimeters = ReadPositiveFiniteInput(label + " 半径（mm）");
+                obstacles.Add(TrajectoryObservationObstacle.Circle(
+                    centerXMillimeters, centerYMillimeters, radiusMillimeters));
+            }
+            else
+            {
+                double lengthXMillimeters = ReadPositiveFiniteInput(label + " X 方向长度（mm）");
+                double widthYMillimeters = ReadPositiveFiniteInput(label + " Y 方向宽度（mm）");
+                obstacles.Add(TrajectoryObservationObstacle.Rectangle(
+                    centerXMillimeters - lengthXMillimeters / 2d,
+                    centerXMillimeters + lengthXMillimeters / 2d,
+                    centerYMillimeters - widthYMillimeters / 2d,
+                    centerYMillimeters + widthYMillimeters / 2d));
+            }
+        }
+
+        return obstacles;
+    }
+
+    private static int ReadBoundedIntegerInput(string prompt, int minimum, int maximum)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        int value;
+        if (!int.TryParse(text, NumberStyles.Integer, CultureInfo.CurrentCulture, out value) &&
+            !int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out value))
+        {
+            throw new ArgumentException("输入必须是整数：" + prompt);
+        }
+
+        if (value < minimum || value > maximum)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        return value;
+    }
+
+    private static double ReadPositiveFiniteInput(string prompt)
+    {
+        double value = ReadFiniteInput(prompt);
+        EnsurePositive(value, prompt);
+        return value;
+    }
+
+    private static double ReadFiniteInput(string prompt)
+    {
+        object raw = UI.GetInput(prompt);
+        string text = Convert.ToString(raw, CultureInfo.CurrentCulture);
+        double value;
+        if (!double.TryParse(text, NumberStyles.Float, CultureInfo.CurrentCulture, out value) &&
+            !double.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value))
+        {
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        }
+
+        EnsureFinite(value, prompt);
+        return value;
+    }
+
+    private static void EnsurePositive(double value, string name)
+    {
+        EnsureFinite(value, name);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(name, "输入必须是正数：" + name);
+    }
+
+    private static void EnsureFinite(double value, string name)
+    {
+        if (!IsFinite(value))
+            throw new ArgumentException("输入必须是有限数字：" + name);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+/// <summary>拥有单次规划和单个控制任务的会话生命周期，并保证停止操作幂等。</summary>
+internal static class EmClosedLoopMovementTestRunner
+{
+    private static readonly object SessionSync = new object();
+    private static CancellationTokenSource activeCancellation;
+    private static Task activeTask;
+    private static DriveTask activeDriveTask;
+    private static TrajectoryObservationPresentation activePresentation;
+    private static TrajectoryObservationVisualizationPublisher activeWebPublisher;
+    private static long nextSessionId;
+    private static long activeSessionId;
+    private static long stateSequence;
+
+    internal static void Start(PlanningPose2D goal, TrajectoryObservationSettings settings,
+        IReadOnlyList<TrajectoryObservationObstacle> obstacles, long obstacleSnapshotVersion,
+        double maximumCommandSpeedMetersPerSecond,
+        double maximumDistanceToTrajectoryMeters,
+        double executionTimeoutSeconds,
+        float wheelAlignmentToleranceDegrees)
+    {
+        if (goal == null) throw new ArgumentNullException(nameof(goal));
+        if (settings == null) throw new ArgumentNullException(nameof(settings));
+        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
+
+        var cancellation = new CancellationTokenSource();
+        CancellationTokenSource previousCancellation;
+        DriveTask previousDriveTask;
+        TrajectoryObservationPresentation previousPresentation;
+        TrajectoryObservationVisualizationPublisher previousWebPublisher;
+        long sessionId;
+        lock (SessionSync)
+        {
+            previousCancellation = activeCancellation;
+            previousDriveTask = activeDriveTask;
+            previousPresentation = activePresentation;
+            previousWebPublisher = activeWebPublisher;
+            activeCancellation = cancellation;
+            activeDriveTask = null;
+            activePresentation = null;
+            activeWebPublisher = null;
+            activeTask = null;
+            activeSessionId = sessionId = ++nextSessionId;
+        }
+
+        Cancel(previousCancellation);
+        StopDriveTask(previousDriveTask);
+        ClearPresentation(previousPresentation);
+        StopWebPublisher(previousWebPublisher);
+        PrintStatus("会话 " + sessionId.ToString(CultureInfo.InvariantCulture) +
+            " 已启动；车辆运动前将只规划一次。");
+
+        Task task = Task.Run(() => RunSessionAsync(sessionId, goal, settings,
+            obstacles, obstacleSnapshotVersion, maximumCommandSpeedMetersPerSecond,
+            maximumDistanceToTrajectoryMeters, executionTimeoutSeconds,
+            wheelAlignmentToleranceDegrees, cancellation.Token), cancellation.Token);
+        lock (SessionSync)
+        {
+            if (activeSessionId == sessionId)
+                activeTask = task;
+        }
+
+        _ = task.ContinueWith(completed => Finish(sessionId, cancellation, completed),
+            TaskScheduler.Default);
+    }
+
+    internal static void Stop()
+    {
+        CancellationTokenSource cancellation;
+        DriveTask driveTask;
+        TrajectoryObservationPresentation presentation;
+        TrajectoryObservationVisualizationPublisher webPublisher;
+        lock (SessionSync)
+        {
+            cancellation = activeCancellation;
+            driveTask = activeDriveTask;
+            presentation = activePresentation;
+            webPublisher = activeWebPublisher;
+            activeCancellation = null;
+            activeDriveTask = null;
+            activePresentation = null;
+            activeWebPublisher = null;
+            activeTask = null;
+            activeSessionId = 0L;
+        }
+
+        Cancel(cancellation);
+        StopDriveTask(driveTask);
+        ClearPresentation(presentation);
+        StopWebPublisher(webPublisher);
+        PrintStatus("已请求取消规划并停止控制任务。");
+    }
+
+    internal static void ShowFailure(string context, Exception exception)
+    {
+        PrintStatus(context + "：" + (exception == null ? "未知错误" : exception.Message));
+    }
+
+    private static async Task RunSessionAsync(long sessionId, PlanningPose2D goal,
+        TrajectoryObservationSettings settings,
+        IReadOnlyList<TrajectoryObservationObstacle> obstacles,
+        long obstacleSnapshotVersion,
+        double maximumCommandSpeedMetersPerSecond,
+        double maximumDistanceToTrajectoryMeters,
+        double executionTimeoutSeconds,
+        float wheelAlignmentToleranceDegrees,
+        CancellationToken token)
+    {
+        token.ThrowIfCancellationRequested();
+        VehicleMotionState initialState = ReadPlanningState();
+        using var planningDeadline = new TrajectoryObservationPlanningDeadline(
+            TimeSpan.FromSeconds(settings.SolverTimeoutSeconds), token);
+        CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(
+            initialState.Pose, goal, settings, obstacles, obstacleSnapshotVersion);
+        TrajectoryObservationBootstrapResult bootstrap =
+            new TrajectoryObservationBootstrapper().Bootstrap(job, planningDeadline, token);
+        if (!bootstrap.Succeeded)
+            throw new InvalidOperationException(bootstrap.FailureReason);
+
+        var controller = new TrajectoryObservationController(
+            bootstrap, settings, new EmPlanningService(new OsqpNativeSolver()),
+            "em-closed-loop-" + sessionId.ToString(CultureInfo.InvariantCulture));
+        PlanningCycleResult cycle = await controller.StartCycle(
+            initialState.CapturedAtUtc, initialState, planningDeadline, token).ConfigureAwait(false);
+        if (!cycle.Published || controller.PublishedTrajectory == null)
+        {
+            string reason = cycle.Result == null ? string.Empty : cycle.Result.FailureReason;
+            throw new InvalidOperationException(string.IsNullOrWhiteSpace(reason)
+                ? cycle.Diagnostic
+                : reason);
+        }
+
+        EmTrajectory emTrajectory = controller.PublishedTrajectory;
+        Trajectory2D controlTrajectory = new EmControlTrajectoryAdapter().Create(emTrajectory);
+        TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualization(
+            sessionId, bootstrap, controller, settings, obstacleSnapshotVersion, token);
+        TrajectoryObservationPresentation presentation = settings.EnableNativePainterVisualization
+            ? new TrajectoryObservationPresentation()
+            : null;
+        if (!TryRegisterPresentation(sessionId, presentation, token))
+        {
+            ClearPresentation(presentation);
+            token.ThrowIfCancellationRequested();
+            return;
+        }
+        UpdateVisualization(sessionId, presentation, bootstrap, controller, emTrajectory,
+            initialState, cycle, "规划已冻结，尚未向底盘下发控制指令。");
+        PrintStatus("规划完成：EM点数=" + emTrajectory.Points.Count.ToString(CultureInfo.InvariantCulture) +
+            "，控制点数=" + controlTrajectory.Count.ToString(CultureInfo.InvariantCulture) +
+            "，长度=" + controlTrajectory.TotalLengthMeters.ToString("F3", CultureInfo.InvariantCulture) +
+            " m，方向=" + emTrajectory.Metadata.Direction +
+            "，终端=" + emTrajectory.Metadata.TerminalType + "。");
+
+        token.ThrowIfCancellationRequested();
+        EmClosedLoopWheelSafety.RequireWheelsForward(wheelAlignmentToleranceDegrees);
+        var movement = new TrajectoryTrackingMovement
+        {
+            Trajectory = controlTrajectory,
+            MaximumCommandSpeedMetersPerSecond = maximumCommandSpeedMetersPerSecond,
+            MaximumDistanceToTrajectoryMeters = maximumDistanceToTrajectoryMeters,
+            ExecutionTimeoutSeconds = executionTimeoutSeconds,
+            CycleObserver = control => UpdatePresentationFromControlCycle(
+                sessionId, presentation, bootstrap, controller, emTrajectory, cycle, control),
+        };
+        var driveTask = new DriveTask(movement.Get());
+        if (!TryRegisterDriveTask(sessionId, driveTask, token))
+        {
+            StopDriveTask(driveTask);
+            token.ThrowIfCancellationRequested();
+            return;
+        }
+
+        try
+        {
+            PrintStatus("冻结轨迹已交给现有控制器，开始单方向段闭环跟踪。");
+            driveTask.Wait();
+            if (emTrajectory.Metadata.TerminalType == EmTerminalType.GearSwitch)
+                PrintStatus("已在换向边界停车；本测试不启动下一方向段。");
+            else
+                PrintStatus("首个方向段已完成并停车。");
+        }
+        finally
+        {
+            StopDriveTask(driveTask);
+            ClearDriveTask(sessionId, driveTask);
+        }
+    }
+
+    private static void UpdatePresentationFromControlCycle(long sessionId,
+        TrajectoryObservationPresentation presentation,
+        TrajectoryObservationBootstrapResult bootstrap,
+        TrajectoryObservationController planningController,
+        EmTrajectory trajectory,
+        PlanningCycleResult cycle,
+        MultiWheelC.Control.Execution.ParkingGeometricController control)
+    {
+        if (control == null || !control.LastVehicleState.HasValue)
+            return;
+
+        var state = control.LastVehicleState.Value;
+        var planningState = new VehicleMotionState(
+            new PlanningPose2D(state.PoseInWorld.XMeters, state.PoseInWorld.YMeters,
+                state.PoseInWorld.YawRadians),
+            state.TwistInBody.VxMetersPerSecond, null, DateTimeOffset.UtcNow,
+            Interlocked.Increment(ref stateSequence));
+        string diagnostic = "现有控制器正在执行冻结轨迹";
+        if (control.LastCommand.HasValue)
+        {
+            var command = control.LastCommand.Value;
+            diagnostic += "：底盘速度=" +
+                command.SpeedMetersPerSecond.ToString("F3", CultureInfo.InvariantCulture) +
+                " m/s，前GCP=" +
+                (command.FrontAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) +
+                " deg，后GCP=" +
+                (command.RearAngleRadians * 180d / Math.PI).ToString("F2", CultureInfo.InvariantCulture) + " deg。";
+        }
+
+        UpdateVisualization(sessionId, presentation, bootstrap, planningController,
+            trajectory, planningState, cycle, diagnostic);
+    }
+
+    private static void UpdateVisualization(long sessionId,
+        TrajectoryObservationPresentation presentation,
+        TrajectoryObservationBootstrapResult bootstrap,
+        TrajectoryObservationController controller,
+        EmTrajectory trajectory,
+        VehicleMotionState state,
+        PlanningCycleResult cycle,
+        string diagnostic)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId)
+                return;
+            var observation = new TrajectoryObservationObservation(
+                DateTimeOffset.UtcNow, state, trajectory, null, null, null);
+            DirectionSegmentView segment = controller.ActiveSegment;
+            TrajectoryObservationCharts charts = TrajectoryObservationCharts.Build(
+                trajectory, segment,
+                controller.CreateEffectiveConfigurationSnapshot().Frenet.MaximumProjectionDistanceMeters);
+            if (presentation != null && ReferenceEquals(activePresentation, presentation))
+            {
+                presentation.DrawWorld(bootstrap, observation,
+                    TrajectoryObservationRuntimeState.Create(DateTimeOffset.UtcNow, trajectory), diagnostic);
+                presentation.DrawLs(charts, diagnostic);
+                presentation.DrawSt(charts, diagnostic);
+            }
+
+            TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher;
+            if (publisher != null)
+            {
+                var tick = new TrajectoryObservationLoopTick(observation, cycle, TimeSpan.Zero,
+                    false, false, true, false, controller.SegmentState);
+                publisher.TryPublish(observation.ObservedAtUtc, () =>
+                {
+                    PlanningVisualizationDynamicSnapshot source =
+                        new TrajectoryObservationDynamicSnapshotBuilder().Build(
+                            Interlocked.Increment(ref stateSequence), tick, segment,
+                            controller.PreviousTrajectoryForVisualization, bootstrap.Vehicle,
+                            controller.CreateEffectiveConfigurationSnapshot());
+                    return new PlanningVisualizationDynamicSnapshot(source.Sequence,
+                        source.ObservedAtUtc, "EM闭环控制中", source.ActiveSegmentIndex,
+                        source.ActiveDirection, source.VehiclePose, source.DynamicPolylines,
+                        source.DynamicMarkers, source.Charts, source.StatusValues, source.CycleSummary);
+                });
+            }
+        }
+    }
+
+    private static TrajectoryObservationVisualizationPublisher StartWebVisualization(
+        long sessionId, TrajectoryObservationBootstrapResult bootstrap,
+        TrajectoryObservationController controller, TrajectoryObservationSettings settings,
+        long obstacleSnapshotVersion, CancellationToken token)
+    {
+        if (!settings.EnableWebVisualization)
+            return null;
+        var publisher = new TrajectoryObservationVisualizationPublisher(settings,
+            new PlanningVisualizationSessionSink(), PrintStatus);
+        try
+        {
+            PlanningVisualizationStaticSnapshot source =
+                new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap,
+                    controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion);
+            var closedLoopSnapshot = new PlanningVisualizationStaticSnapshot("EM闭环测试",
+                source.WorldBounds, source.OccupancyGrid, source.StaticPolylines,
+                source.StaticMarkers, source.DirectionSegments, source.ConfigurationGroups);
+            PlanningVisualizationSessionInfo info = publisher.Start(closedLoopSnapshot);
+            if (info == null)
+                return null;
+            lock (SessionSync)
+            {
+                if (activeSessionId != sessionId || token.IsCancellationRequested)
+                {
+                    publisher.Stop();
+                    return null;
+                }
+                activeWebPublisher = publisher;
+            }
+            PrintStatus("网页可视化地址（含会话令牌）：" + info.Uri.AbsoluteUri);
+            if (settings.AutoOpenWebVisualization)
+            {
+                Process.Start(new ProcessStartInfo
+                {
+                    FileName = info.Uri.AbsoluteUri,
+                    UseShellExecute = true,
+                });
+            }
+            return publisher;
+        }
+        catch (Exception exception)
+        {
+            publisher.Disable(exception);
+            PrintStatus("网页可视化未启动：" + exception.Message);
+            return null;
+        }
+    }
+
+    private static bool TryRegisterPresentation(long sessionId,
+        TrajectoryObservationPresentation presentation, CancellationToken token)
+    {
+        if (presentation == null)
+            return true;
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId || activeCancellation == null ||
+                activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
+                return false;
+            activePresentation = presentation;
+            return true;
+        }
+    }
+
+    private static VehicleMotionState ReadPlanningState()
+    {
+        var location = DetourInterface.getCartLocation();
+        if (location == null)
+            throw new InvalidOperationException("实时定位不可用。");
+        if (BasicPilotBase.Chassis == null)
+            throw new InvalidOperationException("实时底盘读接口不可用。");
+
+        var speed = BasicPilotBase.Chassis.GetCarSpeed(true);
+        return new VehicleMotionState(
+            new PlanningPose2D(location.x / 1000d, location.y / 1000d,
+                location.th * Math.PI / 180d),
+            speed.Vx, null, DateTimeOffset.UtcNow,
+            Interlocked.Increment(ref stateSequence));
+    }
+
+    private static bool TryRegisterDriveTask(long sessionId, DriveTask driveTask,
+        CancellationToken token)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId != sessionId || activeCancellation == null ||
+                activeCancellation.IsCancellationRequested || token.IsCancellationRequested)
+            {
+                return false;
+            }
+
+            activeDriveTask = driveTask;
+            return true;
+        }
+    }
+
+    private static void ClearDriveTask(long sessionId, DriveTask driveTask)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId == sessionId && ReferenceEquals(activeDriveTask, driveTask))
+                activeDriveTask = null;
+                ClearPresentation(activePresentation);
+                activePresentation = null;
+                StopWebPublisher(activeWebPublisher);
+                activeWebPublisher = null;
+        }
+    }
+
+    private static void Finish(long sessionId, CancellationTokenSource cancellation,
+        Task completed)
+    {
+        lock (SessionSync)
+        {
+            if (activeSessionId == sessionId)
+            {
+                activeCancellation = null;
+                activeDriveTask = null;
+                activeTask = null;
+                activeSessionId = 0L;
+            }
+        }
+
+        if (completed.IsFaulted)
+        {
+            Exception failure = completed.Exception == null
+                ? null
+                : completed.Exception.GetBaseException();
+            ShowFailure("闭环会话失败并已停车", failure);
+        }
+        else if (completed.IsCanceled)
+        {
+            PrintStatus("闭环会话已取消。");
+        }
+
+        cancellation.Dispose();
+    }
+
+    private static void Cancel(CancellationTokenSource cancellation)
+    {
+        if (cancellation == null)
+            return;
+        try
+        {
+            cancellation.Cancel();
+        }
+        catch (ObjectDisposedException)
+        {
+        }
+    }
+
+    private static void StopDriveTask(DriveTask driveTask)
+    {
+        if (driveTask == null)
+            return;
+        try
+        {
+            driveTask.Stop();
+        }
+        catch (Exception exception)
+        {
+            ShowFailure("停止控制任务失败", exception);
+        }
+    }
+
+    private static void ClearPresentation(TrajectoryObservationPresentation presentation)
+    {
+        if (presentation == null)
+            return;
+        try
+        {
+            presentation.ClearAll();
+        }
+        catch (Exception exception)
+        {
+            ShowFailure("清理闭环可视化失败", exception);
+        }
+    }
+
+    private static void StopWebPublisher(TrajectoryObservationVisualizationPublisher publisher)
+    {
+        if (publisher == null)
+            return;
+        try
+        {
+            publisher.Stop();
+        }
+        catch (Exception exception)
+        {
+            ShowFailure("停止闭环网页可视化失败", exception);
+        }
+    }
+
+    private static void PrintStatus(string message)
+    {
+        Console.WriteLine("[EM闭环测试] " + message);
+    }
+}
+
+/// <summary>在控制器接管底盘前只读检查四个舵轮是否已经与车体前向对齐。</summary>
+internal static class EmClosedLoopWheelSafety
+{
+    internal static void RequireWheelsForward(float toleranceDegrees)
+    {
+        var chassis = PilotDefinition.Chassis as MultiWheelChassis;
+        if (chassis == null)
+            throw new InvalidOperationException("当前底盘不是 MultiWheelChassis。");
+
+        var adapter = new MultiWheelChassisAdapter(chassis, PilotDefinition.Self.CarNum);
+        double toleranceRadians = AngleMath.DegreesToRadians(toleranceDegrees);
+        if (!adapter.AreParallelWheelsAligned(0d, toleranceRadians))
+        {
+            throw new InvalidOperationException(
+                "四个舵轮尚未与车头方向一致，控制器未接管底盘。");
+        }
+    }
+}