Initial commit
This commit is contained in:
@@ -0,0 +1,409 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user