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,133 @@
+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() { }
+}