410 lines
16 KiB
C#
410 lines
16 KiB
C#
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
/// <summary>
|
|
/// Helper class để load occupancy grid từ PGM file (ROS map format)
|
|
/// PGM format: Portable Gray Map
|
|
/// </summary>
|
|
internal static class PgmLoader
|
|
{
|
|
/// <summary>
|
|
/// Load occupancy grid từ PGM file và YAML metadata file.
|
|
/// UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP).
|
|
/// This ensures consistency across all map formats (PGM, PNG, JPG) and in-memory grids.
|
|
/// </summary>
|
|
/// <param name="pgmPath">Path to PGM file.</param>
|
|
/// <param name="yamlPath">Optional path to YAML metadata.</param>
|
|
/// <param name="logger">Optional logger.</param>
|
|
public static OccupancyGrid? LoadFromPgm(
|
|
string pgmPath,
|
|
string? yamlPath = null,
|
|
ILogger? logger = null)
|
|
{
|
|
try
|
|
{
|
|
if (!File.Exists(pgmPath))
|
|
{
|
|
logger?.LogError("PgmLoader: PGM file not found: {PgmPath}", pgmPath);
|
|
return null;
|
|
}
|
|
|
|
// Try to find YAML file if not provided
|
|
if (string.IsNullOrEmpty(yamlPath))
|
|
{
|
|
var yamlPathCandidate = Path.ChangeExtension(pgmPath, ".yaml");
|
|
if (File.Exists(yamlPathCandidate))
|
|
{
|
|
yamlPath = yamlPathCandidate;
|
|
}
|
|
}
|
|
|
|
// Read YAML metadata if available
|
|
double resolution = 0.05; // Default resolution
|
|
Pose origin = new()
|
|
{
|
|
Position = new Vector3(0, 0, 0),
|
|
Orientation = new Quaternion(0, 0, 0, 1)
|
|
};
|
|
|
|
if (!string.IsNullOrEmpty(yamlPath) && File.Exists(yamlPath))
|
|
{
|
|
var metadata = ParseYamlMetadata(yamlPath, logger);
|
|
resolution = metadata.resolution;
|
|
origin = metadata.origin;
|
|
}
|
|
|
|
// Read PGM file
|
|
// Strategy: Read header using StreamReader to parse, then read file again byte-by-byte to find exact binary start position
|
|
string magic = "";
|
|
int width = 0, height = 0, maxValue = 0;
|
|
long binaryDataStartPosition = 0;
|
|
|
|
// First pass: Read header using StreamReader to parse values
|
|
using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
|
|
{
|
|
using var reader = new StreamReader(fileStream, leaveOpen: true);
|
|
|
|
// Read magic number (P5 or P2)
|
|
magic = reader.ReadLine() ?? "";
|
|
if (magic != "P5" && magic != "P2")
|
|
{
|
|
logger?.LogError("PgmLoader: Invalid PGM format. Expected P5 or P2, got: {Magic}", magic);
|
|
return null;
|
|
}
|
|
|
|
// Skip comments
|
|
string? line;
|
|
while ((line = reader.ReadLine()) != null && line.StartsWith('#'))
|
|
{
|
|
// Skip comment lines
|
|
}
|
|
|
|
// Read dimensions
|
|
var dimensions = line?.Split(' ', StringSplitOptions.RemoveEmptyEntries);
|
|
if (dimensions == null || dimensions.Length != 2)
|
|
{
|
|
logger?.LogError("PgmLoader: Invalid dimensions line: {Line}", line);
|
|
return null;
|
|
}
|
|
|
|
width = int.Parse(dimensions[0]);
|
|
height = int.Parse(dimensions[1]);
|
|
|
|
// Read max value
|
|
var maxValueLine = reader.ReadLine();
|
|
if (string.IsNullOrEmpty(maxValueLine))
|
|
{
|
|
logger?.LogError("PgmLoader: Missing max value");
|
|
return null;
|
|
}
|
|
|
|
maxValue = int.Parse(maxValueLine);
|
|
}
|
|
|
|
// Second pass: Read file from beginning byte-by-byte to find exact binary data start position
|
|
// This is necessary because StreamReader buffering makes Position unreliable
|
|
// Strategy: Read lines and count non-comment lines until we find the 3rd non-comment line (max value)
|
|
using (var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read))
|
|
{
|
|
var lineBuffer = new List<byte>();
|
|
var nonCommentLinesFound = 0;
|
|
|
|
while (true)
|
|
{
|
|
var buffer = new byte[1];
|
|
var bytesRead = fileStream.Read(buffer, 0, 1);
|
|
if (bytesRead == 0)
|
|
{
|
|
break; // End of file
|
|
}
|
|
|
|
var currentChar = (char)buffer[0];
|
|
|
|
// Check for newline (handle both \n and \r\n)
|
|
if (currentChar == '\n')
|
|
{
|
|
// Process line
|
|
if (lineBuffer.Count > 0)
|
|
{
|
|
var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
|
|
|
|
if (!line.StartsWith('#'))
|
|
{
|
|
nonCommentLinesFound++;
|
|
|
|
if (nonCommentLinesFound == 3)
|
|
{
|
|
// Max value line - binary data starts after this newline
|
|
binaryDataStartPosition = fileStream.Position;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
lineBuffer.Clear();
|
|
}
|
|
else if (currentChar == '\r')
|
|
{
|
|
// Handle \r\n - read next byte
|
|
var nextBytesRead = fileStream.Read(buffer, 0, 1);
|
|
if (nextBytesRead > 0 && buffer[0] == '\n')
|
|
{
|
|
// Process line
|
|
if (lineBuffer.Count > 0)
|
|
{
|
|
var line = System.Text.Encoding.ASCII.GetString([.. lineBuffer]).TrimStart();
|
|
|
|
if (!line.StartsWith('#'))
|
|
{
|
|
nonCommentLinesFound++;
|
|
|
|
if (nonCommentLinesFound == 3)
|
|
{
|
|
// Max value line - binary data starts after this \r\n
|
|
binaryDataStartPosition = fileStream.Position;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
lineBuffer.Clear();
|
|
}
|
|
else
|
|
{
|
|
// Just \r without \n, add to line buffer
|
|
lineBuffer.Add((byte)currentChar);
|
|
if (nextBytesRead > 0)
|
|
{
|
|
lineBuffer.Add(buffer[0]);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Add character to line buffer
|
|
lineBuffer.Add(buffer[0]);
|
|
}
|
|
}
|
|
|
|
if (binaryDataStartPosition == 0)
|
|
{
|
|
logger?.LogError("PgmLoader: Failed to find max value line end position. Found {LinesFound} non-comment lines.", nonCommentLinesFound);
|
|
return null;
|
|
}
|
|
}
|
|
|
|
// Create occupancy grid
|
|
var occupancyGrid = new OccupancyGrid(resolution, width, height, origin);
|
|
|
|
// Second pass: Read pixel data
|
|
if (magic == "P5")
|
|
{
|
|
// Binary format - read directly from file
|
|
using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
|
|
fileStream.Position = binaryDataStartPosition;
|
|
|
|
var buffer = new byte[width * height];
|
|
var totalBytesRead = 0;
|
|
|
|
// Read in chunks to handle large files
|
|
while (totalBytesRead < buffer.Length)
|
|
{
|
|
var bytesRead = fileStream.Read(buffer, totalBytesRead, buffer.Length - totalBytesRead);
|
|
if (bytesRead == 0)
|
|
{
|
|
// End of file reached
|
|
break;
|
|
}
|
|
totalBytesRead += bytesRead;
|
|
}
|
|
|
|
if (totalBytesRead != buffer.Length)
|
|
{
|
|
logger?.LogError("PgmLoader: Unexpected end of file. Expected {Expected} bytes, got {Actual}. File may be corrupted or incomplete.", buffer.Length, totalBytesRead);
|
|
return null;
|
|
}
|
|
|
|
// Convert to occupancy values
|
|
// UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
|
|
// Both PGM file and output grid use same convention → direct copy, no Y-flip needed
|
|
for (int y = 0; y < height; y++)
|
|
{
|
|
for (int x = 0; x < width; x++)
|
|
{
|
|
int index = y * width + x;
|
|
var pixelValue = buffer[index];
|
|
var occupancyValue = ConvertPixelToOccupancy(pixelValue, maxValue);
|
|
occupancyGrid.Data[index] = occupancyValue;
|
|
}
|
|
}
|
|
}
|
|
else // P2 - ASCII format
|
|
{
|
|
// ASCII format - read from file starting at binaryDataStartPosition
|
|
using var fileStream = new FileStream(pgmPath, FileMode.Open, FileAccess.Read, FileShare.Read);
|
|
fileStream.Position = binaryDataStartPosition;
|
|
using var reader = new StreamReader(fileStream);
|
|
|
|
// Read ASCII values
|
|
var data = new List<int>();
|
|
string? line;
|
|
while ((line = reader.ReadLine()) != null)
|
|
{
|
|
var values = line.Split(' ', StringSplitOptions.RemoveEmptyEntries);
|
|
foreach (var value in values)
|
|
{
|
|
if (int.TryParse(value, out var intValue))
|
|
{
|
|
data.Add(intValue);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (data.Count != width * height)
|
|
{
|
|
logger?.LogError("PgmLoader: Invalid data count. Expected {Expected}, got {Actual}", width * height, data.Count);
|
|
return null;
|
|
}
|
|
|
|
// Convert to occupancy values (same Y convention as P5 branch)
|
|
// UNIFIED CONVENTION: PGM file uses ROS convention (row 0 = world BOTTOM)
|
|
for (int y = 0; y < height; y++)
|
|
{
|
|
for (int x = 0; x < width; x++)
|
|
{
|
|
int index = y * width + x;
|
|
var pixelValue = data[index];
|
|
var occupancyValue = ConvertPixelToOccupancy((byte)pixelValue, maxValue);
|
|
occupancyGrid.Data[index] = occupancyValue;
|
|
}
|
|
}
|
|
}
|
|
|
|
return occupancyGrid;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogError(ex, "PgmLoader: Failed to load PGM file: {PgmPath}", pgmPath);
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Convert PGM pixel value to occupancy grid value
|
|
/// - 0 (black) = occupied (100)
|
|
/// - 254 (white) = free (0)
|
|
/// - 205 (gray) = unknown (-1)
|
|
/// </summary>
|
|
private static sbyte ConvertPixelToOccupancy(int pixelValue, int maxValue)
|
|
{
|
|
// ROS map_server convention:
|
|
// - 0 = occupied
|
|
// - 254 = free
|
|
// - 205 = unknown
|
|
// Scale to 0-255 range
|
|
var normalizedValue = pixelValue * 255 / maxValue;
|
|
|
|
if (normalizedValue == 0)
|
|
{
|
|
return 100; // Occupied
|
|
}
|
|
else if (normalizedValue == 254 || normalizedValue == 255)
|
|
{
|
|
return 0; // Free
|
|
}
|
|
else if (normalizedValue >= 200 && normalizedValue <= 210)
|
|
{
|
|
return -1; // Unknown
|
|
}
|
|
else
|
|
{
|
|
// Interpolate: 0-200 = occupied (100-0), 210-254 = free (0)
|
|
if (normalizedValue < 200)
|
|
{
|
|
return (sbyte)Math.Clamp(100 - (normalizedValue * 100 / 200), 0, 100);
|
|
}
|
|
else
|
|
{
|
|
return 0; // Free
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parse YAML metadata file (ROS map format)
|
|
/// </summary>
|
|
private static (double resolution, Pose origin) ParseYamlMetadata(string yamlPath, ILogger? logger)
|
|
{
|
|
double resolution = 0.05;
|
|
Pose origin = new()
|
|
{
|
|
Position = new Vector3(0, 0, 0),
|
|
Orientation = new Quaternion(0, 0, 0, 1)
|
|
};
|
|
|
|
try
|
|
{
|
|
var lines = File.ReadAllLines(yamlPath);
|
|
foreach (var line in lines)
|
|
{
|
|
var trimmed = line.Trim();
|
|
if (trimmed.StartsWith('#') || string.IsNullOrWhiteSpace(trimmed))
|
|
continue;
|
|
|
|
// Parse key: value
|
|
var colonIndex = trimmed.IndexOf(':');
|
|
if (colonIndex < 0)
|
|
continue;
|
|
|
|
var key = trimmed[..colonIndex].Trim();
|
|
var value = trimmed[(colonIndex + 1)..].Trim();
|
|
|
|
switch (key.ToLowerInvariant())
|
|
{
|
|
case "resolution":
|
|
if (double.TryParse(value, out var res))
|
|
{
|
|
resolution = res;
|
|
}
|
|
break;
|
|
|
|
case "origin":
|
|
// Format: [x, y, yaw] or [x, y, 0]
|
|
value = value.TrimStart('[').TrimEnd(']');
|
|
var coords = value.Split(',');
|
|
if (coords.Length >= 2)
|
|
{
|
|
if (double.TryParse(coords[0].Trim(), out var x) &&
|
|
double.TryParse(coords[1].Trim(), out var y))
|
|
{
|
|
origin.Position = new Vector3(x, y, 0);
|
|
}
|
|
}
|
|
if (coords.Length >= 3)
|
|
{
|
|
if (double.TryParse(coords[2].Trim(), out var yaw))
|
|
{
|
|
// Convert yaw (radians) to quaternion
|
|
var halfYaw = yaw / 2.0;
|
|
origin.Orientation = new Quaternion(0, 0, Math.Sin(halfYaw), Math.Cos(halfYaw));
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogWarning(ex, "PgmLoader: Failed to parse YAML metadata: {YamlPath}", yamlPath);
|
|
}
|
|
|
|
return (resolution, origin);
|
|
}
|
|
}
|
|
|