134 lines
5.8 KiB
C#
134 lines
5.8 KiB
C#
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() { }
|
||
}
|