ParkingRobot/.task8-sweep/ParkrobTrajplanner/Map/Test/MovementTest.MapTest.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Mathematics;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MultiWheelC.TrajectoryPlanning.Mapping;

namespace MultiWheelC;

/// <summary>
/// 用于手工调试的 Clumsy 地图测试入口，只通过公开的 <see cref="PlanningMapFactory"/> 创建快照。
///
/// 注意：终端日志和 PNG 开关只影响调试输出，不参与建图输入、地图内容或缓存键。
/// </summary>
[MovementTest(name = "规划地图快照测试V1")]
public sealed class PlanningMapTest : MovementTest
{
    private readonly PlanningMapFactory _mapFactory = new PlanningMapFactory();
    private const bool EnableTerminalDebugLog = true;
    private const bool SavePng = true;
    private const float MapXMinMm = 0f;
    private const float MapXMaxMm = 6000f;
    private const float MapYMinMm = 0f;
    private const float MapYMaxMm = 4000f;
    private const float GridResolutionMm = 50f;
    private const bool AllowExplicitEmptyMap = false;
    private const bool EnableTwoLegSnapshot = false;

    /// <summary>
    /// 执行一次配置好的地图创建与调试输出。
    ///
    /// 返回：无。测试使用本文件顶部的地图范围、分辨率、障碍物来源、日志和 PNG 开关；不会接入实时 TwoLeg 检测。
    /// </summary>
    public override void Test()
    {
        IMapObstacleSource[] sources = CreateObstacleSources();
        Log("========== 规划地图创建开始 ==========");
        Log("参数：地图范围=(" + MapXMinMm + "," + MapXMaxMm + ")x(" + MapYMinMm + "," + MapYMaxMm
            + ")mm，分辨率=" + GridResolutionMm + "mm，允许空图=" + AllowExplicitEmptyMap + "，导出图片=" + SavePng);
        Log("障碍物来源：" + FormatSources(sources));
        var result = _mapFactory.Create(new PlanningMapRequest
        {
            Bounds = new MapBoundsMm(MapXMinMm, MapXMaxMm, MapYMinMm, MapYMaxMm),
            ResolutionMm = GridResolutionMm,
            ObstacleSources = sources,
            AllowExplicitEmptyMap = AllowExplicitEmptyMap,
        });
        Log("投影结果：" + FormatProjectionResults(sources, result.SourceResults));
        if (!result.Succeeded || result.Map == null || !result.Map.PlanningReady)
        {
            Log("规划地图创建失败：" + result.FailureReason);
            Log("========== 规划地图创建结束 ==========");
            return;
        }
        Log("地图快照：编号=" + result.Map.SnapshotId + "，缓存=" + FormatCacheHit(result.CacheHit) + "，栅格=" + result.Map.Cols + "x" + result.Map.Rows
            + "，可规划=" + result.Map.PlanningReady + "，指纹=" + result.Map.InputFingerprint);
        PlanningMapImageExportResult image = PlanningMapImageExporter.ExportIfEnabled(SavePng,
            new PlanningMapImageExportRequest { Map = result.Map, OutputRootDirectory = Environment.CurrentDirectory });
        Log("图片导出：" + FormatImageResult(image));
        Log("========== 规划地图创建完成 ==========");
    }

    private static IMapObstacleSource[] CreateObstacleSources()
    {
        IMapObstacle[] manualObstacles =
        {
            new AxisAlignedRectangleObstacle(2400f, 2800f, 800f, 1800f),
            new CircleObstacle(3600f, 1200f, 180f),
        };
        return new IMapObstacleSource[]
        {
            new ManualObstacleSource("manual", 1L, true, manualObstacles),
            new TwoLegObstacleSource("two-leg", EnableTwoLegSnapshot ? 1L : 0L, false,
                new TwoLegProjectionInput(EnableTwoLegSnapshot, 0f, 0f, 0d, 0f, 0f, 0f, 0f, 0f, "No current TwoLeg snapshot.")),
        };
    }

    private static string FormatSources(IReadOnlyList<IMapObstacleSource> sources)
    {
        return string.Join("；", sources.Select(source => source.SourceId + "（版本=" + source.SourceVersion + "，必需=" + source.IsRequired + "）"));
    }

    private static string FormatProjectionResults(IReadOnlyList<IMapObstacleSource> sources, IReadOnlyList<ObstacleProjectionResult> results)
    {
        IMapObstacleSource[] sortedSources = sources.OrderBy(source => source.SourceId, StringComparer.Ordinal).ToArray();
        return string.Join("；", results.Select((result, index) => sortedSources[index].SourceId + "=" + FormatSourceStatus(result.Status) + "（" + result.Obstacles.Count + " 个障碍物）"));
    }

    private static string FormatSourceStatus(ObstacleSourceStatus status)
    {
        switch (status)
        {
            case ObstacleSourceStatus.Applied: return "已应用";
            case ObstacleSourceStatus.Empty: return "空";
            case ObstacleSourceStatus.Unavailable: return "不可用";
            case ObstacleSourceStatus.Invalid: return "无效";
            default: return "未知";
        }
    }

    private static string FormatCacheHit(PlanningMapCacheHit cacheHit)
    {
        switch (cacheHit)
        {
            case PlanningMapCacheHit.Input: return "输入命中";
            case PlanningMapCacheHit.Occupancy: return "占据图命中";
            default: return "未命中";
        }
    }

    private static string FormatImageResult(PlanningMapImageExportResult image)
    {
        if (image.Skipped) return "未启用";
        if (image.Saved) return "已保存：" + image.FilePath;
        return "失败：" + image.Message;
    }

    private static void Log(string message)
    {
        if (EnableTerminalDebugLog) Console.WriteLine("[PlanningMapTest] " + message);
    }

    /// <summary>停止测试。当前测试不启动持续任务，因此无需额外清理资源。</summary>
    public override void TestStop() { }
}