244 lines
9.6 KiB
C#
244 lines
9.6 KiB
C#
using RobotNet10.Shared.Localization;
|
|
using SkiaSharp;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Helper class for saving OccupancyGrid to various file formats.
|
|
/// Consolidates file saving logic used by both CartographerService and MapSaveProcessor.
|
|
/// </summary>
|
|
public static class OccupancyGridFileHelper
|
|
{
|
|
#region Public API
|
|
|
|
/// <summary>
|
|
/// Save all map file formats (PGM, YAML, PNG, JPG) to the specified directory.
|
|
/// </summary>
|
|
public static async Task SaveAllFormatsAsync(
|
|
OccupancyGrid occupancyGrid,
|
|
string mapPath,
|
|
CancellationToken cancellationToken = default,
|
|
ILogger? logger = null)
|
|
{
|
|
var pgmPath = Path.Combine(mapPath, "map.pgm");
|
|
var yamlPath = Path.Combine(mapPath, "map.yaml");
|
|
var pngPath = Path.Combine(mapPath, "map.png");
|
|
var jpgPath = Path.Combine(mapPath, "map.jpg");
|
|
|
|
SaveAsPgm(occupancyGrid, pgmPath, logger);
|
|
SaveAsYaml(occupancyGrid, yamlPath, "map.png", logger);
|
|
await Task.Run(() => SaveAsPng(occupancyGrid, pngPath, cancellationToken, logger), cancellationToken);
|
|
await Task.Run(() => SaveAsJpg(occupancyGrid, jpgPath, cancellationToken, logger), cancellationToken);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Save occupancy grid as PGM file (binary format P5)
|
|
/// </summary>
|
|
public static void SaveAsPgm(OccupancyGrid occupancyGrid, string pgmPath, ILogger? logger = null)
|
|
{
|
|
try
|
|
{
|
|
using var fileStream = new FileStream(pgmPath, FileMode.Create, FileAccess.Write);
|
|
using var writer = new StreamWriter(fileStream);
|
|
|
|
// Write PGM header
|
|
writer.WriteLine("P5"); // Binary format
|
|
writer.WriteLine($"{occupancyGrid.Width} {occupancyGrid.Height}");
|
|
writer.WriteLine("255"); // Max value
|
|
|
|
// Flush header before writing binary data
|
|
writer.Flush();
|
|
|
|
// Write pixel data directly without Y-flip.
|
|
// Both OccupancyGrid and PGM file use same convention for consistency with PgmLoader.
|
|
var buffer = new byte[occupancyGrid.Width * occupancyGrid.Height];
|
|
for (int y = 0; y < occupancyGrid.Height; y++)
|
|
{
|
|
for (int x = 0; x < occupancyGrid.Width; x++)
|
|
{
|
|
int index = y * occupancyGrid.Width + x;
|
|
var occupancyValue = occupancyGrid.Data[index];
|
|
|
|
if (occupancyValue == -1)
|
|
{
|
|
buffer[index] = 205; // Unknown (gray)
|
|
}
|
|
else
|
|
{
|
|
// Convert occupancy (0-100) to PGM (0-255)
|
|
// occupancy 0 (free) -> PGM 254 (white)
|
|
// occupancy 100 (occupied) -> PGM 0 (black)
|
|
buffer[index] = (byte)(254 - (occupancyValue * 254 / 100));
|
|
}
|
|
}
|
|
}
|
|
|
|
fileStream.Write(buffer, 0, buffer.Length);
|
|
fileStream.Flush();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save PGM file: {Path}", pgmPath);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Save occupancy grid as YAML file (ROS map format)
|
|
/// </summary>
|
|
public static void SaveAsYaml(OccupancyGrid occupancyGrid, string yamlPath, string imageFilename, ILogger? logger = null)
|
|
{
|
|
try
|
|
{
|
|
var originX = occupancyGrid.Origin.Position.X;
|
|
var originY = occupancyGrid.Origin.Position.Y;
|
|
|
|
// Write YAML file (ROS map format)
|
|
var yamlContent = $"image: {imageFilename}\n" +
|
|
$"resolution: {occupancyGrid.Resolution:F10}\n" +
|
|
$"origin: [{originX:F10}, {originY:F10}, 0.0]\n" +
|
|
$"negate: 0\n" +
|
|
$"occupied_thresh: 0.65\n" +
|
|
$"free_thresh: 0.196\n";
|
|
|
|
File.WriteAllText(yamlPath, yamlContent);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save YAML file: {Path}", yamlPath);
|
|
throw;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Save occupancy grid as PNG file
|
|
/// </summary>
|
|
public static void SaveAsPng(OccupancyGrid occupancyGrid, string pngPath, CancellationToken cancellationToken = default, ILogger? logger = null)
|
|
{
|
|
RenderAndSaveImage(occupancyGrid, pngPath, SKEncodedImageFormat.Png, 100, cancellationToken, logger);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Save occupancy grid as JPG file
|
|
/// </summary>
|
|
public static void SaveAsJpg(OccupancyGrid occupancyGrid, string jpgPath, CancellationToken cancellationToken = default, ILogger? logger = null)
|
|
{
|
|
RenderAndSaveImage(occupancyGrid, jpgPath, SKEncodedImageFormat.Jpeg, 95, cancellationToken, logger);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Image Rendering
|
|
|
|
/// <summary>
|
|
/// Render occupancy grid to an image and save in the specified format.
|
|
/// Shared implementation for both PNG and JPG output.
|
|
/// </summary>
|
|
private static void RenderAndSaveImage(
|
|
OccupancyGrid occupancyGrid,
|
|
string outputPath,
|
|
SKEncodedImageFormat format,
|
|
int quality,
|
|
CancellationToken cancellationToken,
|
|
ILogger? logger)
|
|
{
|
|
var formatName = format == SKEncodedImageFormat.Png ? "PNG" : "JPG";
|
|
try
|
|
{
|
|
if (occupancyGrid == null || occupancyGrid.Width <= 0 || occupancyGrid.Height <= 0)
|
|
{
|
|
logger?.LogWarning("OccupancyGridFileHelper: Cannot save {Format} - invalid occupancy grid", formatName);
|
|
return;
|
|
}
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var width = occupancyGrid.Width;
|
|
var height = occupancyGrid.Height;
|
|
|
|
// Create SKBitmap with RGBA_8888 format
|
|
using var bitmap = new SKBitmap(width, height, SKColorType.Rgba8888, SKAlphaType.Opaque);
|
|
|
|
// Get pixel buffer pointer for direct memory access
|
|
var pixelsPtr = bitmap.GetPixels();
|
|
if (pixelsPtr == IntPtr.Zero)
|
|
{
|
|
throw new InvalidOperationException("Failed to get pixel buffer from bitmap");
|
|
}
|
|
|
|
// Convert occupancy grid data to image pixels
|
|
// Flip Y: OccupancyGrid uses ROS convention (row 0 = world bottom, Y up)
|
|
// but PNG/JPG image convention is row 0 = top, Y down.
|
|
unsafe
|
|
{
|
|
var pixels = (byte*)pixelsPtr.ToPointer();
|
|
|
|
for (int y = 0; y < height; y++)
|
|
{
|
|
for (int x = 0; x < width; x++)
|
|
{
|
|
// Read from flipped Y in grid (bottom-up) to write top-down in image
|
|
var srcIndex = (height - 1 - y) * width + x;
|
|
var occupancyValue = occupancyGrid.Data[srcIndex];
|
|
|
|
byte intensity;
|
|
if (occupancyValue == -1)
|
|
{
|
|
intensity = 205; // Unknown (gray)
|
|
}
|
|
else if (occupancyValue == 0)
|
|
{
|
|
intensity = 254; // Free space (white)
|
|
}
|
|
else
|
|
{
|
|
// Occupied space: Convert occupancy (0-100) to pixel intensity (0-255)
|
|
var intensityValue = 254.0 - (occupancyValue * 254.0 / 100.0);
|
|
intensity = (byte)Math.Clamp((int)Math.Round(intensityValue), 0, 254);
|
|
}
|
|
|
|
// Write RGBA bytes directly (destination index uses image row y)
|
|
var dstIndex = y * width + x;
|
|
var pixelOffset = dstIndex * 4;
|
|
pixels[pixelOffset] = intensity; // R
|
|
pixels[pixelOffset + 1] = intensity; // G
|
|
pixels[pixelOffset + 2] = intensity; // B
|
|
pixels[pixelOffset + 3] = 255; // A (opaque)
|
|
}
|
|
}
|
|
}
|
|
|
|
// Encode and save
|
|
using var image = SKImage.FromBitmap(bitmap) ?? throw new InvalidOperationException("Failed to create SKImage from bitmap");
|
|
using var data = image.Encode(format, quality) ?? throw new InvalidOperationException($"Failed to encode {formatName} image");
|
|
|
|
var directory = Path.GetDirectoryName(outputPath);
|
|
if (!string.IsNullOrEmpty(directory))
|
|
{
|
|
Directory.CreateDirectory(directory);
|
|
}
|
|
|
|
if (File.Exists(outputPath))
|
|
{
|
|
File.Delete(outputPath);
|
|
}
|
|
|
|
using var stream = File.Create(outputPath);
|
|
data.SaveTo(stream);
|
|
stream.Flush();
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
logger?.LogWarning("OccupancyGridFileHelper: {Format} save cancelled: {Path}", formatName, outputPath);
|
|
throw;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogError(ex, "OccupancyGridFileHelper: Failed to save {Format} file: {Path}", formatName, outputPath);
|
|
// Don't throw - image files are optional
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
}
|