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,507 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+
+/// <summary>Clumsy 粗路径手动测试可选择的固定场景。</summary>
+public enum CoarsePathTestScenario
+{
+    /// <summary>明确允许的空地图直达场景。</summary>
+    ExplicitEmpty,
+
+    /// <summary>由中央矩形阻断直线的绕行场景。</summary>
+    RectangleDetour,
+
+    /// <summary>同时包含手工圆形、矩形与 TwoLeg 快照的多来源场景。</summary>
+    ManualAndTwoLeg,
+
+    /// <summary>与矩形绕行输入完全一致，用于在同一服务中验证输入缓存命中。</summary>
+    CacheHit,
+
+    /// <summary>起步前进、终点倒车进入的换向场景。</summary>
+    ReverseGearSwitch,
+
+    /// <summary>由贯穿边界的障碍带分隔起终点的无解场景。</summary>
+    NoFeasiblePath,
+}
+
+/// <summary>手动障碍物输入支持的世界几何类型。</summary>
+public enum ManualCoarsePathObstacleKind
+{
+    /// <summary>由世界中心和半径定义的圆形障碍物。</summary>
+    Circle,
+
+    /// <summary>由世界中心、X 方向长度和 Y 方向宽度定义的轴对齐矩形障碍物。</summary>
+    AxisAlignedRectangle,
+}
+
+/// <summary>
+/// 手动粗路径测试的不可变障碍物输入。
+/// 所有中心和尺寸均使用世界 mm；矩形始终与世界坐标轴平行，不包含旋转角。
+/// </summary>
+public sealed class ManualCoarsePathObstacle
+{
+    private ManualCoarsePathObstacle(ManualCoarsePathObstacleKind kind, double centerXMillimeters,
+        double centerYMillimeters, double sizeXMillimeters, double sizeYMillimeters)
+    {
+        Kind = kind;
+        CenterXMillimeters = centerXMillimeters;
+        CenterYMillimeters = centerYMillimeters;
+        SizeXMillimeters = sizeXMillimeters;
+        SizeYMillimeters = sizeYMillimeters;
+    }
+
+    /// <summary>障碍物的支持几何类型。</summary>
+    public ManualCoarsePathObstacleKind Kind { get; }
+
+    /// <summary>几何中心世界 X 坐标，单位 mm。</summary>
+    public double CenterXMillimeters { get; }
+
+    /// <summary>几何中心世界 Y 坐标，单位 mm。</summary>
+    public double CenterYMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 X 方向长度；单位 mm。</summary>
+    public double SizeXMillimeters { get; }
+
+    /// <summary>圆形时为半径，矩形时为 Y 方向宽度；单位 mm。</summary>
+    public double SizeYMillimeters { get; }
+
+    /// <summary>创建圆形障碍物。参数：圆心和半径均使用世界 mm，半径必须为有限正数。</summary>
+    public static ManualCoarsePathObstacle Circle(double centerXMillimeters, double centerYMillimeters,
+        double radiusMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(radiusMillimeters, nameof(radiusMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.Circle, centerXMillimeters,
+            centerYMillimeters, radiusMillimeters, radiusMillimeters);
+    }
+
+    /// <summary>
+    /// 创建轴对齐矩形障碍物。
+    /// 参数：中心、X 方向长度和 Y 方向宽度均使用世界 mm；两个尺寸必须为有限正数。
+    /// </summary>
+    public static ManualCoarsePathObstacle AxisAlignedRectangle(double centerXMillimeters,
+        double centerYMillimeters, double lengthXMillimeters, double widthYMillimeters)
+    {
+        EnsureFinite(centerXMillimeters, nameof(centerXMillimeters));
+        EnsureFinite(centerYMillimeters, nameof(centerYMillimeters));
+        EnsurePositiveFinite(lengthXMillimeters, nameof(lengthXMillimeters));
+        EnsurePositiveFinite(widthYMillimeters, nameof(widthYMillimeters));
+        return new ManualCoarsePathObstacle(ManualCoarsePathObstacleKind.AxisAlignedRectangle,
+            centerXMillimeters, centerYMillimeters, lengthXMillimeters, widthYMillimeters);
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}
+
+/// <summary>
+/// Clumsy 粗路径测试的纯输入工厂。
+/// 固定场景不读取 UI、传感器、定位或时钟；传入 AMR 位姿的手动入口仅在此处完成世界 mm/deg 到核心 m/rad 的转换。
+/// </summary>
+public static class CoarsePathScenarioFactory
+{
+    private const float MapXMinMillimeters = 0f;
+    private const float MapXMaxMillimeters = 6000f;
+    private const float MapYMinMillimeters = 0f;
+    private const float MapYMaxMillimeters = 4000f;
+    private const float ResolutionMillimeters = 50f;
+    private const double MillimetersPerMeter = 1000d;
+    private const double DegreesToRadians = Math.PI / 180d;
+    private const double ManualMapPaddingMillimeters = 8000d;
+    private const int MaximumManualObstacleCount = 20;
+
+    /// <summary>
+    /// 创建一个新的固定测试业务请求。
+    /// 返回：每次调用都返回独立的可变请求对象，供调用方安全地传入同一个长期存活的规划服务。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario)
+    {
+        return CreateCore(scenario, null);
+    }
+
+    /// <summary>
+    /// 创建以当前 AMR 世界位姿为起点的固定测试业务请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；地图、目标和障碍物仅随 AMR 坐标平移，TwoLeg 朝向保持不变。
+    /// </summary>
+    public static CoarsePathPlanningJob Create(CoarsePathTestScenario scenario,
+        double amrXMillimeters, double amrYMillimeters, double amrHeadingDegrees)
+    {
+        return CreateCore(scenario,
+            new FixedScenarioAnchor(amrXMillimeters, amrYMillimeters, amrHeadingDegrees));
+    }
+
+    private static CoarsePathPlanningJob CreateCore(CoarsePathTestScenario scenario, FixedScenarioAnchor anchor)
+    {
+        switch (scenario)
+        {
+            case CoarsePathTestScenario.ExplicitEmpty:
+                return CreateExplicitEmpty(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.RectangleDetour:
+            case CoarsePathTestScenario.CacheHit:
+                return CreateRectangleDetour(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.ManualAndTwoLeg:
+                return CreateManualAndTwoLeg(FixedScenarioTransform.From(1000d, 1000d, 0d, anchor));
+            case CoarsePathTestScenario.ReverseGearSwitch:
+                return CreateReverseGearSwitch(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            case CoarsePathTestScenario.NoFeasiblePath:
+                return CreateNoFeasiblePath(FixedScenarioTransform.From(1000d, 2000d, 0d, anchor));
+            default:
+                throw new ArgumentOutOfRangeException(nameof(scenario));
+        }
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿和手动世界终点的空图演示请求。
+    /// 参数：X/Y 使用世界 mm，航向使用 deg；返回请求中的 <see cref="Pose2D"/> 使用世界 m/rad。
+    /// 注意：这是坐标、路径和取消流程的演示空图，不能表示现场不存在障碍物。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualGoalDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees)
+    {
+        return CreateManualObstacleDemo(startXMillimeters, startYMillimeters, startHeadingDegrees,
+            goalXMillimeters, goalYMillimeters, goalHeadingDegrees,
+            Array.Empty<ManualCoarsePathObstacle>(), 0L);
+    }
+
+    /// <summary>
+    /// 创建传入 AMR 世界位姿、手动世界终点和手动障碍物快照的测试请求。
+    /// 参数：位姿 X/Y、障碍物中心和尺寸使用世界 mm，航向使用 deg；返回的 <see cref="Pose2D"/> 使用 m/rad。
+    /// 障碍物非空时 obstacleSnapshotVersion 必须为正数，以避免长期服务错误复用旧地图；零障碍物才创建显式空图演示。
+    /// </summary>
+    public static CoarsePathPlanningJob CreateManualObstacleDemo(
+        double startXMillimeters, double startYMillimeters, double startHeadingDegrees,
+        double goalXMillimeters, double goalYMillimeters, double goalHeadingDegrees,
+        IReadOnlyList<ManualCoarsePathObstacle> obstacles, long obstacleSnapshotVersion)
+    {
+        EnsureFinite(startXMillimeters, nameof(startXMillimeters));
+        EnsureFinite(startYMillimeters, nameof(startYMillimeters));
+        EnsureFinite(startHeadingDegrees, nameof(startHeadingDegrees));
+        EnsureFinite(goalXMillimeters, nameof(goalXMillimeters));
+        EnsureFinite(goalYMillimeters, nameof(goalYMillimeters));
+        EnsureFinite(goalHeadingDegrees, nameof(goalHeadingDegrees));
+
+        if (obstacles == null) throw new ArgumentNullException(nameof(obstacles));
+        if (obstacles.Count > MaximumManualObstacleCount)
+            throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle count exceeds the supported limit.");
+        if (obstacles.Count != 0 && obstacleSnapshotVersion <= 0L)
+            throw new ArgumentOutOfRangeException(nameof(obstacleSnapshotVersion), "Obstacle snapshots require a positive version.");
+
+        return CreateJob(
+            CreateManualDemoMap(startXMillimeters, startYMillimeters, goalXMillimeters, goalYMillimeters,
+                obstacles, obstacleSnapshotVersion),
+            ToPose(startXMillimeters, startYMillimeters, startHeadingDegrees),
+            ToPose(goalXMillimeters, goalYMillimeters, goalHeadingDegrees),
+            null,
+            GoalDirectionConstraint.Any);
+    }
+
+    private static CoarsePathPlanningJob CreateExplicitEmpty(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null,
+            GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateRectangleDetour(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 1L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2700f), transform.X(3300f),
+                    transform.Y(1200f), transform.Y(2800f)),
+            }),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d), null, GoalDirectionConstraint.Forward);
+        // 固定绕行场景保留最优启发式；30 秒覆盖较慢测试环境，UI 仍可随时取消。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateManualAndTwoLeg(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 2L, true, new IMapObstacle[]
+            {
+                new CircleObstacle(transform.X(2400f), transform.Y(1300f), 220f),
+                new AxisAlignedRectangleObstacle(transform.X(3000f), transform.X(3600f),
+                    transform.Y(2000f), transform.Y(2600f)),
+            }),
+            new TwoLegObstacleSource("two-leg", 1L, true, new TwoLegProjectionInput(true,
+                transform.X(3900f), transform.Y(2500f), 0d,
+                    -180f, -180f, -180f, 180f, 140f, "P1 fixed TwoLeg snapshot.")),
+        };
+        CoarsePathPlanningJob job = CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 1d, 0d),
+            transform.Pose(5d, 3d, 0d), null, GoalDirectionConstraint.Forward);
+        // 多来源场景的最优绕行会受机器负载影响；放宽演示总预算但保留全部碰撞与目标判定。
+        job.Configuration.SearchTimeout = TimeSpan.FromSeconds(15d);
+        return job;
+    }
+
+    private static CoarsePathPlanningJob CreateReverseGearSwitch(FixedScenarioTransform transform)
+    {
+        return CreateJob(
+            CreateMap(true, Array.Empty<IMapObstacleSource>(), transform),
+            transform.Pose(1d, 2d, 0d),
+            transform.Pose(4d, 2d, 0d),
+            TravelDirection.Forward,
+            GoalDirectionConstraint.Reverse);
+    }
+
+    private static CoarsePathPlanningJob CreateNoFeasiblePath(FixedScenarioTransform transform)
+    {
+        IMapObstacleSource[] sources =
+        {
+            new ManualObstacleSource("manual", 3L, true, new IMapObstacle[]
+            {
+                new AxisAlignedRectangleObstacle(transform.X(2900f), transform.X(3100f),
+                    transform.Y(0f), transform.Y(4000f)),
+            }),
+        };
+        return CreateJob(CreateMap(false, sources, transform), transform.Pose(1d, 2d, 0d),
+            transform.Pose(5d, 2d, 0d),
+            null, GoalDirectionConstraint.Forward);
+    }
+
+    private static CoarsePathPlanningJob CreateJob(PlanningMapRequest mapRequest, Pose2D start, Pose2D goal,
+        TravelDirection? startDirection, GoalDirectionConstraint goalDirection)
+    {
+        return new CoarsePathPlanningJob
+        {
+            MapRequest = mapRequest,
+            Start = start,
+            Goal = goal,
+            Vehicle = new VehicleParameters
+            {
+                LengthMeters = 0.80d,
+                WidthMeters = 0.60d,
+                SafetyMarginMeters = 0.05d,
+                MaximumCurvaturePerMeter = 1d / 1.20d,
+            },
+            Configuration = new HybridAStarConfiguration(),
+            StartDirection = startDirection,
+            GoalDirection = goalDirection,
+        };
+    }
+
+    private static PlanningMapRequest CreateMap(bool allowExplicitEmptyMap, IReadOnlyList<IMapObstacleSource> sources,
+        FixedScenarioTransform transform)
+    {
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(transform.X(MapXMinMillimeters), transform.X(MapXMaxMillimeters),
+                transform.Y(MapYMinMillimeters), transform.Y(MapYMaxMillimeters)),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = allowExplicitEmptyMap,
+        };
+    }
+
+    private sealed class FixedScenarioAnchor
+    {
+        public FixedScenarioAnchor(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFinite(xMillimeters, nameof(xMillimeters));
+            EnsureFinite(yMillimeters, nameof(yMillimeters));
+            EnsureFinite(headingDegrees, nameof(headingDegrees));
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingRadians = NormalizeRadians((headingDegrees % 360d) * DegreesToRadians);
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingRadians { get; }
+    }
+
+    private sealed class FixedScenarioTransform
+    {
+        private FixedScenarioTransform(double deltaXMillimeters, double deltaYMillimeters, double headingDeltaRadians)
+        {
+            DeltaXMillimeters = deltaXMillimeters;
+            DeltaYMillimeters = deltaYMillimeters;
+            HeadingDeltaRadians = headingDeltaRadians;
+        }
+
+        public double DeltaXMillimeters { get; }
+        public double DeltaYMillimeters { get; }
+        public double HeadingDeltaRadians { get; }
+
+        public static FixedScenarioTransform From(double baselineStartXMillimeters,
+            double baselineStartYMillimeters, double baselineStartHeadingRadians, FixedScenarioAnchor anchor)
+        {
+            if (anchor == null) return new FixedScenarioTransform(0d, 0d, 0d);
+            return new FixedScenarioTransform(anchor.XMillimeters - baselineStartXMillimeters,
+                anchor.YMillimeters - baselineStartYMillimeters,
+                NormalizeRadians(anchor.HeadingRadians - baselineStartHeadingRadians));
+        }
+
+        public float X(float value)
+        {
+            return ToFiniteFloat(value + DeltaXMillimeters, nameof(value));
+        }
+
+        public float Y(float value)
+        {
+            return ToFiniteFloat(value + DeltaYMillimeters, nameof(value));
+        }
+
+        public Pose2D Pose(double xMeters, double yMeters, double headingRadians)
+        {
+            return new Pose2D((xMeters * MillimetersPerMeter + DeltaXMillimeters) / MillimetersPerMeter,
+                (yMeters * MillimetersPerMeter + DeltaYMillimeters) / MillimetersPerMeter,
+                NormalizeRadians(headingRadians + HeadingDeltaRadians));
+        }
+    }
+
+    private static PlanningMapRequest CreateManualDemoMap(double startXMillimeters, double startYMillimeters,
+        double goalXMillimeters, double goalYMillimeters, IReadOnlyList<ManualCoarsePathObstacle> obstacles,
+        long obstacleSnapshotVersion)
+    {
+        double minimumX = Math.Min(startXMillimeters, goalXMillimeters);
+        double maximumX = Math.Max(startXMillimeters, goalXMillimeters);
+        double minimumY = Math.Min(startYMillimeters, goalYMillimeters);
+        double maximumY = Math.Max(startYMillimeters, goalYMillimeters);
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            double halfX;
+            double halfY;
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters;
+                    halfY = obstacle.SizeYMillimeters;
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    EnsurePositiveFinite(obstacle.SizeXMillimeters, nameof(obstacles));
+                    EnsurePositiveFinite(obstacle.SizeYMillimeters, nameof(obstacles));
+                    halfX = obstacle.SizeXMillimeters / 2d;
+                    halfY = obstacle.SizeYMillimeters / 2d;
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+
+            minimumX = Math.Min(minimumX, obstacle.CenterXMillimeters - halfX);
+            maximumX = Math.Max(maximumX, obstacle.CenterXMillimeters + halfX);
+            minimumY = Math.Min(minimumY, obstacle.CenterYMillimeters - halfY);
+            maximumY = Math.Max(maximumY, obstacle.CenterYMillimeters + halfY);
+        }
+
+        float xMin = ToGridLowerBound(minimumX - ManualMapPaddingMillimeters);
+        float xMax = ToGridUpperBound(maximumX + ManualMapPaddingMillimeters);
+        float yMin = ToGridLowerBound(minimumY - ManualMapPaddingMillimeters);
+        float yMax = ToGridUpperBound(maximumY + ManualMapPaddingMillimeters);
+        bool isExplicitEmptyMap = obstacles.Count == 0;
+        return new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(xMin, xMax, yMin, yMax),
+            ResolutionMm = ResolutionMillimeters,
+            ObstacleSources = isExplicitEmptyMap ? Array.Empty<IMapObstacleSource>() :
+                new IMapObstacleSource[]
+                {
+                    new ManualObstacleSource("manual-user-input", obstacleSnapshotVersion, true,
+                        ConvertManualObstacles(obstacles)),
+                },
+            AllowExplicitEmptyMap = isExplicitEmptyMap,
+        };
+    }
+
+    private static IMapObstacle[] ConvertManualObstacles(IReadOnlyList<ManualCoarsePathObstacle> obstacles)
+    {
+        var result = new IMapObstacle[obstacles.Count];
+        for (int index = 0; index < obstacles.Count; index++)
+        {
+            ManualCoarsePathObstacle obstacle = obstacles[index] ??
+                throw new ArgumentException("Manual obstacle entries cannot be null.", nameof(obstacles));
+            switch (obstacle.Kind)
+            {
+                case ManualCoarsePathObstacleKind.Circle:
+                    result[index] = new CircleObstacle(ToFiniteFloat(obstacle.CenterXMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.SizeXMillimeters, nameof(obstacles)));
+                    break;
+                case ManualCoarsePathObstacleKind.AxisAlignedRectangle:
+                    double halfX = obstacle.SizeXMillimeters / 2d;
+                    double halfY = obstacle.SizeYMillimeters / 2d;
+                    result[index] = new AxisAlignedRectangleObstacle(
+                        ToFiniteFloat(obstacle.CenterXMillimeters - halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterXMillimeters + halfX, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters - halfY, nameof(obstacles)),
+                        ToFiniteFloat(obstacle.CenterYMillimeters + halfY, nameof(obstacles)));
+                    break;
+                default:
+                    throw new ArgumentOutOfRangeException(nameof(obstacles), "Manual obstacle kind is not supported.");
+            }
+        }
+        return result;
+    }
+
+    private static Pose2D ToPose(double xMillimeters, double yMillimeters, double headingDegrees)
+    {
+        return new Pose2D(xMillimeters / MillimetersPerMeter, yMillimeters / MillimetersPerMeter,
+            headingDegrees * DegreesToRadians);
+    }
+
+    private static double NormalizeRadians(double angle)
+    {
+        double normalized = angle % (2d * Math.PI);
+        if (normalized <= -Math.PI) return normalized + 2d * Math.PI;
+        return normalized > Math.PI ? normalized - 2d * Math.PI : normalized;
+    }
+
+    private static float ToGridLowerBound(double millimeters)
+    {
+        double rounded = Math.Floor(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToGridUpperBound(double millimeters)
+    {
+        double rounded = Math.Ceiling(millimeters / ResolutionMillimeters) * ResolutionMillimeters;
+        return ToFiniteFloat(rounded, nameof(millimeters));
+    }
+
+    private static float ToFiniteFloat(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value) || value < float.MinValue || value > float.MaxValue)
+            throw new ArgumentOutOfRangeException(parameterName, "Value cannot be represented as a finite millimeter coordinate.");
+        return (float)value;
+    }
+
+    private static void EnsureFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be finite.");
+    }
+
+    private static void EnsurePositiveFinite(double value, string parameterName)
+    {
+        EnsureFinite(value, parameterName);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(parameterName, "Value must be positive.");
+    }
+}
@@ -0,0 +1,628 @@
+using System;
+using System.Collections.Generic;
+using System.Drawing;
+using System.Globalization;
+using System.Threading;
+using System.Threading.Tasks;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using FundamentalLib;
+using MDCSToolBox.Clumsy.Pilot;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+
+namespace MultiWheelC;
+
+/// <summary>
+/// 显式空图的粗路径规划测试入口。
+/// 只负责创建纯规划请求；规划、取消与可视化均由共享执行器处理，不会向底盘发送任何命令。
+/// </summary>
+[MovementTest(name = "粗路径规划-显式空图")]
+public sealed class CoarsePathExplicitEmptyTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ExplicitEmpty, "显式空图");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 单矩形绕行的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-单矩形绕行")]
+// TODO：#在该测试下发生了红色矩形栅格碰撞但依然规划成功，需要进一步核实与确认
+public sealed class CoarsePathRectangleDetourTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.RectangleDetour, "单矩形绕行");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 手工圆形、矩形和 TwoLeg 快照组合的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-多来源障碍")]
+public sealed class CoarsePathManualAndTwoLegTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ManualAndTwoLeg, "多来源障碍");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 重复输入地图缓存命中的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-缓存命中")]
+public sealed class CoarsePathCacheHitTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.CacheHit, "缓存命中");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 前进起步、倒车到达并显示换向点的粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-倒车换向")]
+public sealed class CoarsePathReverseGearSwitchTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.ReverseGearSwitch, "倒车换向");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 障碍带完全隔开起终点的无解粗路径规划测试入口。
+/// </summary>
+[MovementTest(name = "粗路径规划-无解")]
+public sealed class CoarsePathNoFeasiblePathTest : MovementTest
+{
+    /// <inheritdoc />
+    public override void Test() => CoarsePathPlanningTestRunner.RunScenario(CoarsePathTestScenario.NoFeasiblePath, "无解障碍带");
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+}
+
+/// <summary>
+/// 使用当前 AMR 车身几何中心位姿和人工终点的粗路径规划演示入口。
+/// 输入的 X/Y 使用世界 mm、航向使用 deg；进入规划核心前由场景工厂一次性转换为 m/rad。
+/// </summary>
+[MovementTest(name = "粗路径规划")]
+public sealed class CoarsePathPlanningTest : MovementTest
+{
+    private const int MaximumManualObstacleCount = 20;
+    private static long _nextManualObstacleSnapshotVersion;
+
+    /// <summary>
+    /// 读取一次 AMR 当前世界位姿、手动终点和障碍物快照后启动规划。
+    /// 注意：getCartLocation 在无定位时可能阻塞；全部输入会在启动后台任务前冻结，不会被规划线程重复读取。
+    /// </summary>
+    public override void Test()
+    {
+        try
+        {
+            var amrPose = DetourInterface.getCartLocation();
+            double goalXmm = ReadFiniteInput("粗路径终点 X（世界 mm）");
+            double goalYmm = ReadFiniteInput("粗路径终点 Y（世界 mm）");
+            double goalHeadingDeg = ReadFiniteInput("粗路径终点航向（世界 deg）");
+            TimeSpan searchTimeout = ReadPositiveTimeoutInput("粗路径规划总超时（秒，必须大于 0）");
+            IReadOnlyList<ManualCoarsePathObstacle> obstacles = ReadManualObstacles();
+            long snapshotVersion = obstacles.Count == 0 ? 0L :
+                Interlocked.Increment(ref _nextManualObstacleSnapshotVersion);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.CreateManualObstacleDemo(
+                amrPose.x, amrPose.y, amrPose.th, goalXmm, goalYmm, goalHeadingDeg,
+                obstacles, snapshotVersion);
+            job.Configuration.SearchTimeout = searchTimeout;
+            CoarsePathPlanningTestRunner.Run("AMR 位姿 + 手动终点 + 手动障碍物", job);
+        }
+        catch (Exception exception)
+        {
+            CoarsePathPlanningTestRunner.ShowInputFailure(exception);
+        }
+    }
+
+    /// <inheritdoc />
+    public override void TestStop() => CoarsePathPlanningTestRunner.Stop();
+
+    private static IReadOnlyList<ManualCoarsePathObstacle> ReadManualObstacles()
+    {
+        int count = ReadBoundedIntegerInput("手动障碍物数量（0-20）", 0, MaximumManualObstacleCount);
+        var obstacles = new List<ManualCoarsePathObstacle>(count);
+        for (int index = 0; index < count; index++)
+        {
+            string label = "障碍物 " + (index + 1);
+            int kind = ReadBoundedIntegerInput(label + " 类型（1圆形，2矩形）", 1, 2);
+            double centerXmm = ReadFiniteInput(label + " 中心 X（世界 mm）");
+            double centerYmm = ReadFiniteInput(label + " 中心 Y（世界 mm）");
+            if (kind == 1)
+            {
+                double radiusMm = ReadPositiveFiniteInput(label + " 半径 r（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.Circle(centerXmm, centerYmm, radiusMm));
+            }
+            else
+            {
+                double lengthXmm = ReadPositiveFiniteInput(label + " X方向长度（mm）");
+                double widthYmm = ReadPositiveFiniteInput(label + " Y方向宽度（mm）");
+                obstacles.Add(ManualCoarsePathObstacle.AxisAlignedRectangle(centerXmm, centerYmm,
+                    lengthXmm, widthYmm));
+            }
+        }
+        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);
+        if (value <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        return value;
+    }
+
+    private static TimeSpan ReadPositiveTimeoutInput(string prompt)
+    {
+        double timeoutSeconds = ReadFiniteInput(prompt);
+        if (timeoutSeconds <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入必须为正数：" + prompt);
+        try
+        {
+            return TimeSpan.FromSeconds(timeoutSeconds);
+        }
+        catch (OverflowException)
+        {
+            throw new ArgumentOutOfRangeException(nameof(prompt), "输入超出允许范围：" + prompt);
+        }
+    }
+
+    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);
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException("输入必须是有限数字：" + prompt);
+        return value;
+    }
+}
+
+/// <summary>
+/// 粗路径 MovementTest 的共享后台会话、停止和绘制实现。
+/// 同一时刻只允许一个会话绘制；启动新会话或停止时会取消旧会话，但不会等待旧任务退出。
+/// </summary>
+internal static class CoarsePathPlanningTestRunner
+{
+    private const string PainterLayerName = "CoarsePathPlanningV1";
+    private const float MillimetersPerMeter = 1000f;
+    private const int MaximumVisibleGridLines = 100;
+
+    private static readonly object SessionSync = new object();
+    private static readonly CoarsePathPlanningService PlanningService = new CoarsePathPlanningService();
+    private static readonly Painter Painter = UI.GetPainter(PainterLayerName, true);
+
+    private static CancellationTokenSource _activeCancellation;
+    private static Task<CoarsePathPlanningJobResult> _activeTask;
+    private static long _nextSessionId;
+    private static long _activeSessionId;
+
+    /// <summary>
+    /// 固定场景启动时冻结的 AMR 位姿。规划后台不会重新读取定位，确保输入一致。
+    /// </summary>
+    private sealed class AmrPoseSnapshot
+    {
+        public AmrPoseSnapshot(double xMillimeters, double yMillimeters, double headingDegrees)
+        {
+            EnsureFiniteAmrValue(xMillimeters, "X");
+            EnsureFiniteAmrValue(yMillimeters, "Y");
+            EnsureFiniteAmrValue(headingDegrees, "航向");
+            XMillimeters = xMillimeters;
+            YMillimeters = yMillimeters;
+            HeadingDegrees = headingDegrees;
+        }
+
+        public double XMillimeters { get; }
+        public double YMillimeters { get; }
+        public double HeadingDegrees { get; }
+
+        public string DisplayText
+        {
+            get
+            {
+                return "AMR 起点：X=" + XMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，Y=" + YMillimeters.ToString("F0", CultureInfo.InvariantCulture) +
+                    " mm，航向=" + HeadingDegrees.ToString("F1", CultureInfo.InvariantCulture) + " deg";
+            }
+        }
+    }
+
+    /// <summary>
+    /// 创建指定固定场景并将其提交给共享后台服务。
+    /// </summary>
+    internal static void RunScenario(CoarsePathTestScenario scenario, string scenarioName)
+    {
+        try
+        {
+            var pose = DetourInterface.getCartLocation();
+            if (ReferenceEquals(pose, null)) throw new ArgumentException("AMR 位姿为空。");
+            var snapshot = new AmrPoseSnapshot(pose.x, pose.y, pose.th);
+            CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenario,
+                snapshot.XMillimeters, snapshot.YMillimeters, snapshot.HeadingDegrees);
+            Run(scenarioName, job, snapshot);
+        }
+        catch (Exception exception)
+        {
+            ShowInputFailure(new ArgumentException("AMR 位姿不可用：" + exception.Message, exception));
+        }
+    }
+
+    /// <summary>
+    /// 提交已经冻结输入的一次规划请求。调用立即返回，结果只会由对应会话的完成回调绘制。
+    /// </summary>
+    internal static void Run(string scenarioName, CoarsePathPlanningJob job)
+    {
+        Run(scenarioName, job, null);
+    }
+
+    private static void Run(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        if (job == null) throw new ArgumentNullException(nameof(job));
+
+        var cancellation = new CancellationTokenSource();
+        CancellationTokenSource previousCancellation;
+        long sessionId;
+        lock (SessionSync)
+        {
+            previousCancellation = _activeCancellation;
+            _activeCancellation = cancellation;
+            _activeTask = null;
+            sessionId = ++_nextSessionId;
+            _activeSessionId = sessionId;
+        }
+
+        // 先发布新会话编号，再取消旧任务，避免旧完成回调覆盖新画面。
+        if (previousCancellation != null) previousCancellation.Cancel();
+        Painter.Clear();
+        DrawPending(scenarioName, job, amrPose);
+
+        Task<CoarsePathPlanningJobResult> task = Task.Run(() => PlanningService.Plan(job, cancellation.Token));
+        lock (SessionSync)
+        {
+            if (_activeSessionId == sessionId) _activeTask = task;
+        }
+
+        _ = task.ContinueWith(completed => Finish(sessionId, scenarioName, job, amrPose, cancellation, completed),
+            TaskScheduler.Default);
+    }
+
+    /// <summary>
+    /// 取消当前会话并清空专用图层；不等待后台任务结束。
+    /// 已取消任务的完成回调只释放资源，不再记录或绘制结果。
+    /// </summary>
+    internal static void Stop()
+    {
+        CancellationTokenSource cancellation;
+        lock (SessionSync)
+        {
+            cancellation = _activeCancellation;
+            _activeCancellation = null;
+            _activeTask = null;
+            _activeSessionId = 0;
+        }
+
+        if (cancellation != null) cancellation.Cancel();
+        Painter.Clear();
+        Hedingben.ToastText("粗路径规划已请求停止。", PainterLayerName);
+    }
+
+    /// <summary>
+    /// 显示输入读取或校验失败，且不改变现有规划任务。
+    /// </summary>
+    internal static void ShowInputFailure(Exception exception)
+    {
+        string message = exception == null ? "未知输入错误。" : exception.Message;
+        Painter.DrawText(Color.LightYellow, "粗路径规划未启动：" + message, 0f, 0f);
+        Hedingben.ToastText("粗路径规划未启动：" + message, PainterLayerName);
+    }
+
+    private static void Finish(long sessionId, string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CancellationTokenSource cancellation, Task<CoarsePathPlanningJobResult> completed)
+    {
+        try
+        {
+            CoarsePathPlanningJobResult result = completed.GetAwaiter().GetResult();
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (!isCurrent) return;
+            DrawResult(scenarioName, job, amrPose, result);
+            Hedingben.ToastText(BuildToastMessage(scenarioName, result), PainterLayerName);
+        }
+        catch (Exception exception)
+        {
+            bool isCurrent;
+            lock (SessionSync)
+            {
+                isCurrent = _activeSessionId == sessionId;
+                if (isCurrent)
+                {
+                    _activeTask = null;
+                    _activeCancellation = null;
+                }
+            }
+
+            if (isCurrent)
+                Hedingben.ToastText("粗路径规划任务异常：" + exception.GetType().Name + "。" + exception.Message,
+                    PainterLayerName);
+        }
+        finally
+        {
+            cancellation.Dispose();
+        }
+    }
+
+    private static void DrawPending(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose)
+    {
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawPose(job.Goal, Color.Orange, "终点");
+        Painter.DrawText(Color.LightGray, "场景：" + scenarioName + "（规划中）", 0f, 0f);
+        if (amrPose != null) Painter.DrawText(Color.LightGray, amrPose.DisplayText, 0f, -120f);
+    }
+
+    private static void DrawResult(string scenarioName, CoarsePathPlanningJob job, AmrPoseSnapshot amrPose,
+        CoarsePathPlanningJobResult result)
+    {
+        Painter.Clear();
+        PlanningGridMap map = result.MapResult.Map;
+        if (map != null) DrawMap(map);
+
+        DrawPose(job.Start, Color.LimeGreen, "起点");
+        DrawGoal(job.Goal, job.Configuration, Color.Orange);
+        if (result.PlanningResult.Status == PlanningStatus.Success)
+            DrawPath(result.PlanningResult, job.Vehicle);
+
+        DrawLegend(map);
+        DrawStatus(scenarioName, job, result, map, amrPose);
+    }
+
+    private static void DrawMap(PlanningGridMap map)
+    {
+        float xMin = map.Bounds.XMin;
+        float xMax = map.Bounds.XMax;
+        float yMin = map.Bounds.YMin;
+        float yMax = map.Bounds.YMax;
+        float resolution = map.ResolutionMm;
+        int gridStride = Math.Max(1, (int)Math.Ceiling(Math.Max(map.Rows, map.Cols) / (double)MaximumVisibleGridLines));
+
+        // 真实 ResolutionMm 决定网格位置，gridStride 只影响显示抽稀。
+        for (int col = 0; col <= map.Cols; col += gridStride)
+        {
+            float x = Math.Min(xMax, xMin + col * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), x, yMin, x, yMax, width: 1);
+        }
+        for (int row = 0; row <= map.Rows; row += gridStride)
+        {
+            float y = Math.Min(yMax, yMin + row * resolution);
+            Painter.DrawLine(Color.FromArgb(80, Color.SlateGray), xMin, y, xMax, y, width: 1);
+        }
+
+        DrawRectangle(Color.Gainsboro, xMin, yMin, xMax, yMax, 3);
+        if (xMin <= 0f && 0f < xMax) Painter.DrawLine(Color.DimGray, 0f, yMin, 0f, yMax, width: 2);
+        if (yMin <= 0f && 0f < yMax) Painter.DrawLine(Color.DimGray, xMin, 0f, xMax, 0f, width: 2);
+
+        for (int row = 0; row < map.Rows; row++)
+        {
+            for (int col = 0; col < map.Cols; col++)
+            {
+                if (!map.IsOccupied(row, col)) continue;
+                float x = xMin + col * resolution + resolution / 2f;
+                float y = yMin + row * resolution;
+                Painter.DrawLine(Color.FromArgb(150, Color.Firebrick), x, y, x, y + resolution,
+                    width: Math.Max(1, (int)Math.Round(resolution)));
+            }
+        }
+    }
+
+    private static void DrawPose(Pose2D pose, Color color, string label)
+    {
+        if (pose == null) return;
+        float x = ToMillimeters(pose.X);
+        float y = ToMillimeters(pose.Y);
+        Painter.DrawCircle(color, x, y, 80f);
+        DrawHeadingArrow(x, y, pose.Heading, color, 260f);
+        Painter.DrawText(color, label, x + 100f, y + 100f);
+    }
+
+    private static void DrawGoal(Pose2D goal, HybridAStarConfiguration configuration, Color color)
+    {
+        DrawPose(goal, color, "终点");
+        if (goal == null || configuration == null) return;
+        Painter.DrawCircle(Color.FromArgb(150, color), ToMillimeters(goal.X), ToMillimeters(goal.Y),
+            ToMillimeters(configuration.GoalPositionToleranceMeters));
+    }
+
+    private static void DrawPath(PlanningResult planningResult, VehicleParameters vehicle)
+    {
+        if (planningResult.Path == null || planningResult.Path.Count == 0) return;
+
+        int frameStride = Math.Max(1, planningResult.Path.Count / 10);
+        for (int index = 1; index < planningResult.Path.Count; index++)
+        {
+            CoarsePathPoint previous = planningResult.Path[index - 1];
+            CoarsePathPoint current = planningResult.Path[index];
+            Color color = current.Direction == TravelDirection.Forward ? Color.LimeGreen : Color.DeepSkyBlue;
+            Painter.DrawLine(color, ToMillimeters(previous.X), ToMillimeters(previous.Y),
+                ToMillimeters(current.X), ToMillimeters(current.Y), width: 4);
+
+            if (index % frameStride == 0 || current.IsGearSwitchPoint || index == planningResult.Path.Count - 1)
+                DrawVehicleFrame(current, vehicle);
+            if (index % Math.Max(1, frameStride / 2) == 0)
+                DrawHeadingArrow(ToMillimeters(current.X), ToMillimeters(current.Y),
+                    current.Heading + (current.Direction == TravelDirection.Reverse ? Math.PI : 0d), color, 140f);
+            if (!current.IsGearSwitchPoint) continue;
+
+            float x = ToMillimeters(current.X);
+            float y = ToMillimeters(current.Y);
+            Painter.DrawCircle(Color.MediumPurple, x, y, 100f);
+            Painter.DrawText(Color.MediumPurple, "换向", x + 110f, y - 110f);
+        }
+
+        DrawVehicleFrame(planningResult.Path[0], vehicle);
+    }
+
+    private static void DrawVehicleFrame(CoarsePathPoint point, VehicleParameters vehicle)
+    {
+        if (point == null || vehicle == null) return;
+        float halfLength = ToMillimeters(vehicle.LengthMeters / 2d + vehicle.SafetyMarginMeters);
+        float halfWidth = ToMillimeters(vehicle.WidthMeters / 2d + vehicle.SafetyMarginMeters);
+        float centerX = ToMillimeters(point.X);
+        float centerY = ToMillimeters(point.Y);
+        double cos = Math.Cos(point.Heading);
+        double sin = Math.Sin(point.Heading);
+
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, halfWidth, out float frontLeftX, out float frontLeftY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, halfLength, -halfWidth, out float frontRightX, out float frontRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, -halfWidth, out float rearRightX, out float rearRightY);
+        TransformVehicleCorner(centerX, centerY, cos, sin, -halfLength, halfWidth, out float rearLeftX, out float rearLeftY);
+        Painter.DrawLine(Color.Gold, frontLeftX, frontLeftY, frontRightX, frontRightY, width: 2);
+        Painter.DrawLine(Color.Gold, frontRightX, frontRightY, rearRightX, rearRightY, width: 2);
+        Painter.DrawLine(Color.Gold, rearRightX, rearRightY, rearLeftX, rearLeftY, width: 2);
+        Painter.DrawLine(Color.Gold, rearLeftX, rearLeftY, frontLeftX, frontLeftY, width: 2);
+    }
+
+    private static void TransformVehicleCorner(float centerX, float centerY, double cos, double sin,
+        float longitudinal, float lateral, out float x, out float y)
+    {
+        x = centerX + (float)(cos * longitudinal - sin * lateral);
+        y = centerY + (float)(sin * longitudinal + cos * lateral);
+    }
+
+    private static void DrawHeadingArrow(float x, float y, double headingRadians, Color color, float length)
+    {
+        float endX = x + (float)Math.Cos(headingRadians) * length;
+        float endY = y + (float)Math.Sin(headingRadians) * length;
+        Painter.DrawLine(color, x, y, endX, endY, endArrow: true, width: 3);
+    }
+
+    private static void DrawLegend(PlanningGridMap map)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? 250f : map.Bounds.YMax - 180f;
+        Painter.DrawText(Color.White, "图例", x, y);
+        DrawLegendItem(x, y - 130f, Color.Gainsboro, "边界 / 栅格");
+        DrawLegendItem(x, y - 260f, Color.Firebrick, "占据格");
+        DrawLegendItem(x, y - 390f, Color.LimeGreen, "起点 / 前进");
+        DrawLegendItem(x, y - 520f, Color.Orange, "终点 / 容差");
+        DrawLegendItem(x, y - 650f, Color.DeepSkyBlue, "倒车");
+        DrawLegendItem(x, y - 780f, Color.MediumPurple, "换向");
+        DrawLegendItem(x, y - 910f, Color.Gold, "扩大车体检查框");
+    }
+
+    private static void DrawLegendItem(float x, float y, Color color, string text)
+    {
+        Painter.DrawLine(color, x, y, x + 90f, y, width: 5);
+        Painter.DrawText(color, text, x + 120f, y - 30f);
+    }
+
+    private static void DrawStatus(string scenarioName, CoarsePathPlanningJob job,
+        CoarsePathPlanningJobResult result, PlanningGridMap map, AmrPoseSnapshot amrPose)
+    {
+        float x = map == null ? 0f : map.Bounds.XMin + 150f;
+        float y = map == null ? -250f : map.Bounds.YMin + 150f;
+        string snapshot = map == null ? "无" : map.SnapshotId.ToString(CultureInfo.InvariantCulture);
+        string resolution = map == null ? "无" : map.ResolutionMm.ToString("F0", CultureInfo.InvariantCulture) + " mm";
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string reason = diagnostics.TerminationReason ?? string.Empty;
+        string turningRadius = "无";
+        if (job != null && VehicleKinematics.TryGetMaximumCurvaturePerMeter(job.Vehicle, out double maximumCurvaturePerMeter))
+            turningRadius = (1d / maximumCurvaturePerMeter).ToString("F2", CultureInfo.InvariantCulture) + " m";
+        Painter.DrawText(Color.White, "场景：" + scenarioName, x, y);
+        float detailOffset = 0f;
+        if (amrPose != null)
+        {
+            Painter.DrawText(Color.White, amrPose.DisplayText, x, y + 120f);
+            detailOffset = 120f;
+        }
+        Painter.DrawText(Color.White, "地图：" + result.MapResult.Status + "，缓存：" + result.MapResult.CacheHit + "，快照：" + snapshot,
+            x, y + 120f + detailOffset);
+        Painter.DrawText(Color.White, "栅格：" + resolution + "，规划：" + result.PlanningResult.Status + "，总耗时：" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms，路径搜索：" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + " ms", x, y + 240f + detailOffset);
+        Painter.DrawText(Color.White, "节点：扩展=" + diagnostics.ExpandedNodeCount.ToString(CultureInfo.InvariantCulture) +
+            "，生成=" + diagnostics.GeneratedNodeCount.ToString(CultureInfo.InvariantCulture) + "，Open List峰值=" +
+            diagnostics.PeakOpenListCount.ToString(CultureInfo.InvariantCulture), x, y + 360f + detailOffset);
+        if (job != null && job.Vehicle != null)
+        {
+            Painter.DrawText(Color.White, "演示车辆：长=" + job.Vehicle.LengthMeters.ToString("F2", CultureInfo.InvariantCulture) +
+                " m，宽=" + job.Vehicle.WidthMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，余量=" +
+                job.Vehicle.SafetyMarginMeters.ToString("F2", CultureInfo.InvariantCulture) + " m，最小转弯半径=" +
+                turningRadius, x, y + 480f + detailOffset);
+        }
+        if (!string.IsNullOrEmpty(reason))
+            Painter.DrawText(Color.LightYellow, "原因：" + reason, x, y + 600f + detailOffset);
+    }
+
+    private static string BuildToastMessage(string scenarioName, CoarsePathPlanningJobResult result)
+    {
+        PlanningDiagnostics diagnostics = result.PlanningResult.Diagnostics;
+        string message = "粗路径[" + scenarioName + "]：地图=" + result.MapResult.Status + "，规划=" +
+            result.PlanningResult.Status + "，总耗时=" +
+            diagnostics.Elapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms，路径搜索=" +
+            diagnostics.PathSearchElapsed.TotalMilliseconds.ToString("F0", CultureInfo.InvariantCulture) + "ms";
+        if (result.PlanningResult.Status != PlanningStatus.Success && !string.IsNullOrEmpty(diagnostics.TerminationReason))
+            message += "，原因=" + diagnostics.TerminationReason;
+        return message + "。";
+    }
+
+    private static void DrawRectangle(Color color, float xMin, float yMin, float xMax, float yMax, int width)
+    {
+        Painter.DrawLine(color, xMin, yMin, xMax, yMin, width: width);
+        Painter.DrawLine(color, xMax, yMin, xMax, yMax, width: width);
+        Painter.DrawLine(color, xMax, yMax, xMin, yMax, width: width);
+        Painter.DrawLine(color, xMin, yMax, xMin, yMin, width: width);
+    }
+
+    private static float ToMillimeters(double meters) => (float)(meters * MillimetersPerMeter);
+
+    private static void EnsureFiniteAmrValue(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentException(name + " 必须是有限数。");
+    }
+}