QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

243
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/OccupancyGridFileHelper.cs Normal file
View File

@@ -0,0 +1,243 @@
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
}