chore: save current workspace progress
This commit is contained in:
梁薄云
@@ -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() { }
|
||||
}
|
||||
+10
@@ -0,0 +1,10 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 调试图导出请求，只读取不可变的规划地图快照。</summary>
|
||||
public sealed class PlanningMapImageExportRequest
|
||||
{
|
||||
/// <summary>待渲染的规划地图快照。不能为空；PNG 导出不会改动该地图或其缓存。</summary>
|
||||
public PlanningGridMap Map { get; set; }
|
||||
/// <summary>PNG 输出根目录。导出器会在其中创建 PlanningMapExports 子目录；不能为空。</summary>
|
||||
public string OutputRootDirectory { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>一次可选 PNG 导出的处理结果，包含状态、输出路径、像素尺寸和诊断信息。</summary>
|
||||
public sealed class PlanningMapImageExportResult
|
||||
{
|
||||
/// <summary>是否已经成功写入并完成 PNG 文件。</summary>
|
||||
public bool Saved { get; set; }
|
||||
/// <summary>是否因导出开关关闭而跳过。true 时不创建文件，也不影响地图构建或缓存。</summary>
|
||||
public bool Skipped { get; set; }
|
||||
/// <summary>成功保存时的 PNG 完整路径；未保存时通常为 null。</summary>
|
||||
public string FilePath { get; set; }
|
||||
/// <summary>保存、跳过或失败的诊断信息。</summary>
|
||||
public string Message { get; set; }
|
||||
/// <summary>成功 PNG 的文件字节数；未保存时为零。</summary>
|
||||
public long FileSizeBytes { get; set; }
|
||||
/// <summary>输出图像宽度，单位为像素。</summary>
|
||||
public int PixelWidth { get; set; }
|
||||
/// <summary>输出图像高度，单位为像素。</summary>
|
||||
public int PixelHeight { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>可选 PNG 导出器；它永远不参与地图构建、障碍物投影或缓存指纹计算。</summary>
|
||||
public static class PlanningMapImageExporter
|
||||
{
|
||||
/// <summary>每个规划栅格在输出图中占用的边长，单位为像素。</summary>
|
||||
public const int PixelsPerCell = 4;
|
||||
/// <summary>输出 PNG 单边允许的最大像素数，超过时拒绝导出。</summary>
|
||||
public const int MaximumImageEdgePixels = 4000;
|
||||
/// <summary>输出 PNG 允许的最大文件大小，单位为字节（50 MiB）。</summary>
|
||||
public const long MaximumFileSizeBytes = 50L * 1024L * 1024L;
|
||||
/// <summary>写入 PNG 的物理分辨率元数据，单位 DPI。</summary>
|
||||
public const float OutputDpi = 300f;
|
||||
private const int MaximumFilenameAttempts = 1024;
|
||||
|
||||
/// <summary>
|
||||
/// 在开关开启时导出规划地图 PNG。
|
||||
///
|
||||
/// 参数：enabled 为导出开关；request 包含不可变地图和输出根目录。
|
||||
/// 返回：开关关闭时返回 Skipped；成功时返回 Saved、路径、字节数与像素尺寸；输入或尺寸不合法时返回失败信息。
|
||||
/// 注意：该方法仅用于调试，绝不会影响地图创建和缓存结果。
|
||||
/// </summary>
|
||||
public static PlanningMapImageExportResult ExportIfEnabled(bool enabled, PlanningMapImageExportRequest request)
|
||||
{
|
||||
if (!enabled) return new PlanningMapImageExportResult { Skipped = true, Message = "Planning map image export is disabled." };
|
||||
if (request == null || request.Map == null) return Rejected("Planning map image export requires a PlanningGridMap.");
|
||||
if (string.IsNullOrWhiteSpace(request.OutputRootDirectory)) return Rejected("Planning map image export requires an output root directory.");
|
||||
long width = (long)request.Map.Cols * PixelsPerCell, height = (long)request.Map.Rows * PixelsPerCell;
|
||||
if (width <= 0 || height <= 0 || width > MaximumImageEdgePixels || height > MaximumImageEdgePixels) return Rejected("Planning map image dimensions exceed the permitted edge.", width, height);
|
||||
string temporary = null;
|
||||
try
|
||||
{
|
||||
string directory = Path.Combine(request.OutputRootDirectory, "PlanningMapExports");
|
||||
Directory.CreateDirectory(directory);
|
||||
using (FileStream stream = CreateTemporaryFile(directory, out string finalPath, out temporary))
|
||||
{
|
||||
byte[] rgba = PlanningMapImageRenderer.Render(request.Map, PixelsPerCell, out int pixelWidth, out int pixelHeight);
|
||||
ValidatedPngWriter.Write(rgba, pixelWidth, pixelHeight, stream);
|
||||
}
|
||||
long length = new FileInfo(temporary).Length;
|
||||
if (length > MaximumFileSizeBytes) return Rejected("Planning map image PNG exceeds 50 MiB.", width, height);
|
||||
string completed = temporary.Substring(0, temporary.Length - 4);
|
||||
File.Move(temporary, completed); temporary = null;
|
||||
return new PlanningMapImageExportResult { Saved = true, FilePath = completed, FileSizeBytes = length, PixelWidth = (int)width, PixelHeight = (int)height, Message = "Planning map image export saved." };
|
||||
}
|
||||
catch (Exception exception) { return Rejected("Planning map image export failed: " + exception.Message, width, height); }
|
||||
finally { if (temporary != null && File.Exists(temporary)) File.Delete(temporary); }
|
||||
}
|
||||
|
||||
private static PlanningMapImageExportResult Rejected(string message, long width = 0, long height = 0) { return new PlanningMapImageExportResult { Message = message, PixelWidth = width > int.MaxValue ? int.MaxValue : (int)width, PixelHeight = height > int.MaxValue ? int.MaxValue : (int)height }; }
|
||||
private static FileStream CreateTemporaryFile(string directory, out string finalPath, out string temporaryPath)
|
||||
{
|
||||
string timestamp = DateTime.Now.ToString("yyyyMMdd_HHmmss_fff");
|
||||
for (int index = 0; index < MaximumFilenameAttempts; index++)
|
||||
{
|
||||
string suffix = index == 0 ? string.Empty : "_" + index;
|
||||
finalPath = Path.Combine(directory, "PlanningMap_" + timestamp + suffix + ".png"); temporaryPath = finalPath + ".tmp";
|
||||
if (File.Exists(finalPath)) continue;
|
||||
try { return new FileStream(temporaryPath, FileMode.CreateNew, FileAccess.Write, FileShare.None); }
|
||||
catch (IOException) { }
|
||||
}
|
||||
finalPath = null; temporaryPath = null; throw new IOException("Could not reserve a unique PlanningMap PNG filename.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>从只读规划地图生成简单的 RGBA 占据图字节数组。</summary>
|
||||
internal static class PlanningMapImageRenderer
|
||||
{
|
||||
/// <summary>
|
||||
/// 渲染规划地图为行主序 RGBA 像素。
|
||||
///
|
||||
/// 参数：map 为只读规划快照；pixelsPerCell 为每个栅格的像素边长；width、height 返回图像像素尺寸。
|
||||
/// 返回：长度为 width × height × 4 的 RGBA 字节数组；不会修改地图。
|
||||
/// </summary>
|
||||
public static byte[] Render(PlanningGridMap map, int pixelsPerCell, out int width, out int height)
|
||||
{
|
||||
if (map == null) throw new ArgumentNullException(nameof(map));
|
||||
width = checked(map.Cols * pixelsPerCell);
|
||||
height = checked(map.Rows * pixelsPerCell);
|
||||
var rgba = new byte[checked(width * height * 4)];
|
||||
for (int row = 0; row < map.Rows; row++)
|
||||
for (int col = 0; col < map.Cols; col++)
|
||||
{
|
||||
bool occupied = map.IsOccupied(row, col);
|
||||
byte red = occupied ? (byte)220 : (byte)245;
|
||||
byte green = occupied ? (byte)45 : (byte)245;
|
||||
byte blue = occupied ? (byte)45 : (byte)245;
|
||||
int displayRow = map.Rows - 1 - row;
|
||||
for (int py = 0; py < pixelsPerCell; py++)
|
||||
for (int px = 0; px < pixelsPerCell; px++)
|
||||
{
|
||||
int index = ((displayRow * pixelsPerCell + py) * width + col * pixelsPerCell + px) * 4;
|
||||
rgba[index] = red; rgba[index + 1] = green; rgba[index + 2] = blue; rgba[index + 3] = 255;
|
||||
}
|
||||
}
|
||||
return rgba;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,98 @@
|
||||
using System;
|
||||
using System.IO;
|
||||
using StbImageWriteSharp;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
|
||||
/// <summary>编码 RGBA 像素并校验 PNG 文件头、必要块和全部 CRC 的内部写入器。</summary>
|
||||
internal static class ValidatedPngWriter
|
||||
{
|
||||
private static readonly byte[] Signature = { 137, 80, 78, 71, 13, 10, 26, 10 };
|
||||
private static readonly byte[] PhysType = { 112, 72, 89, 115 };
|
||||
|
||||
/// <summary>
|
||||
/// 将 RGBA 像素写入经过校验的 PNG 流。
|
||||
///
|
||||
/// 参数：rgba 为行主序、每像素四字节的红绿蓝透明度数组；width、height 为像素尺寸；output 为可写目标流。
|
||||
/// 返回：无。输入长度不等于 width × height × 4 或 PNG 校验失败时抛出异常。
|
||||
/// </summary>
|
||||
public static void Write(byte[] rgba, int width, int height, Stream output)
|
||||
{
|
||||
Write(rgba, width, height, output, 11811u);
|
||||
}
|
||||
|
||||
/// <summary>将 RGBA 像素写入带指定物理分辨率元数据的经过校验的 PNG 流。</summary>
|
||||
internal static void Write(byte[] rgba, int width, int height, Stream output, uint pixelsPerMeter)
|
||||
{
|
||||
if (rgba == null || output == null || width <= 0 || height <= 0 || pixelsPerMeter == 0u || rgba.Length != checked(width * height * 4))
|
||||
throw new ArgumentException("Invalid RGBA PNG input.");
|
||||
byte[] encoded;
|
||||
using (var memory = new MemoryStream())
|
||||
{
|
||||
new ImageWriter().WritePng(rgba, width, height, ColorComponents.RedGreenBlueAlpha, memory);
|
||||
encoded = memory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
const int ihdrEndOffset = 8 + 4 + 4 + 13 + 4;
|
||||
using (var outputMemory = new MemoryStream())
|
||||
{
|
||||
outputMemory.Write(encoded, 0, ihdrEndOffset);
|
||||
var phys = new byte[9];
|
||||
WriteUInt32BigEndian(phys, 0, pixelsPerMeter);
|
||||
WriteUInt32BigEndian(phys, 4, pixelsPerMeter);
|
||||
phys[8] = 1;
|
||||
WriteChunk(outputMemory, PhysType, phys);
|
||||
outputMemory.Write(encoded, ihdrEndOffset, encoded.Length - ihdrEndOffset);
|
||||
encoded = outputMemory.ToArray();
|
||||
}
|
||||
Validate(encoded);
|
||||
output.Write(encoded, 0, encoded.Length);
|
||||
}
|
||||
|
||||
private static void Validate(byte[] png)
|
||||
{
|
||||
if (png == null || png.Length < 45) throw new InvalidDataException("PNG is truncated.");
|
||||
for (int i = 0; i < Signature.Length; i++) if (png[i] != Signature[i]) throw new InvalidDataException("PNG signature is invalid.");
|
||||
int offset = Signature.Length, ihdr = 0, iend = 0;
|
||||
while (offset < png.Length)
|
||||
{
|
||||
if (png.Length - offset < 12) throw new InvalidDataException("PNG chunk header is truncated.");
|
||||
uint length = ReadUInt32BigEndian(png, offset);
|
||||
long crcOffset = (long)offset + 8L + length;
|
||||
if (crcOffset + 4L > png.Length) throw new InvalidDataException("PNG chunk is truncated.");
|
||||
int typeOffset = offset + 4;
|
||||
uint expected = ReadUInt32BigEndian(png, (int)crcOffset);
|
||||
uint actual = ComputeCrc32(png, typeOffset, checked((int)length + 4));
|
||||
if (expected != actual) throw new InvalidDataException("PNG chunk CRC is invalid.");
|
||||
bool isIhdr = IsType(png, typeOffset, 73, 72, 68, 82);
|
||||
bool isIend = IsType(png, typeOffset, 73, 69, 78, 68);
|
||||
if (offset == Signature.Length && !isIhdr) throw new InvalidDataException("PNG must start with IHDR.");
|
||||
if (isIhdr) ihdr++;
|
||||
if (isIend) { iend++; if (crcOffset + 4L != png.Length) throw new InvalidDataException("PNG data follows IEND."); }
|
||||
offset = checked((int)crcOffset + 4);
|
||||
}
|
||||
if (ihdr != 1 || iend != 1) throw new InvalidDataException("PNG must contain exactly one IHDR and IEND.");
|
||||
}
|
||||
private static bool IsType(byte[] bytes, int offset, byte a, byte b, byte c, byte d) { return bytes[offset] == a && bytes[offset + 1] == b && bytes[offset + 2] == c && bytes[offset + 3] == d; }
|
||||
private static uint ReadUInt32BigEndian(byte[] bytes, int offset) { return ((uint)bytes[offset] << 24) | ((uint)bytes[offset + 1] << 16) | ((uint)bytes[offset + 2] << 8) | bytes[offset + 3]; }
|
||||
private static void WriteUInt32BigEndian(byte[] bytes, int offset, uint value) { bytes[offset] = (byte)(value >> 24); bytes[offset + 1] = (byte)(value >> 16); bytes[offset + 2] = (byte)(value >> 8); bytes[offset + 3] = (byte)value; }
|
||||
private static void WriteChunk(Stream output, byte[] type, byte[] data)
|
||||
{
|
||||
var length = new byte[4]; WriteUInt32BigEndian(length, 0, (uint)data.Length); output.Write(length, 0, 4); output.Write(type, 0, 4); output.Write(data, 0, data.Length);
|
||||
var crcBytes = new byte[4]; WriteUInt32BigEndian(crcBytes, 0, ComputeCrc32(type, 0, type.Length, data)); output.Write(crcBytes, 0, 4);
|
||||
}
|
||||
private static uint ComputeCrc32(byte[] data, int offset, int count) { return ComputeCrc32(data, offset, count, null); }
|
||||
private static uint ComputeCrc32(byte[] first, int offset, int count, byte[] second)
|
||||
{
|
||||
uint crc = 0xffffffffu;
|
||||
for (int i = 0; i < count; i++) crc = UpdateCrc(crc, first[offset + i]);
|
||||
if (second != null) for (int i = 0; i < second.Length; i++) crc = UpdateCrc(crc, second[i]);
|
||||
return crc ^ 0xffffffffu;
|
||||
}
|
||||
private static uint UpdateCrc(uint crc, byte value)
|
||||
{
|
||||
crc ^= value;
|
||||
for (int bit = 0; bit < 8; bit++) crc = (crc & 1u) == 0u ? crc >> 1 : 0xedb88320u ^ (crc >> 1);
|
||||
return crc;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user