307 lines
11 KiB
C#
307 lines
11 KiB
C#
using CartographerSharp.Mapping;
|
|
using RobotNet10.RobotApp.Shared;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer;
|
|
using RobotNet10.Shared.Localization;
|
|
using System.Text.Json;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Manages occupancy grid loading, updating, and caching
|
|
/// Handles throttled updates during ScanMapping using counter-based logic
|
|
/// </summary>
|
|
public class OccupancyGridManager(CartographerConfiguration _config, ILogger<OccupancyGridManager> _logger)
|
|
{
|
|
private readonly Lock _lock = new();
|
|
|
|
private OccupancyGrid? _occupancyGrid;
|
|
private OccupancyGrid? _occupancyGridMcl;
|
|
private DateTime _lastUpdatedOccupancyGrid = DateTime.MinValue;
|
|
private volatile int _generationInProgress = 0; // 1 = generation running
|
|
|
|
// Version tracking: PoseGraph node insertion version at last successful generation.
|
|
// Compared against the live PoseGraph version to skip redundant regenerations.
|
|
private int _lastGeneratedVersion = -1;
|
|
|
|
// Configuration: time-based throttling (~3 seconds between updates)
|
|
private readonly System.Diagnostics.Stopwatch _updateStopwatch = System.Diagnostics.Stopwatch.StartNew();
|
|
private const double UpdateIntervalSeconds = 3.0;
|
|
|
|
// Insertion tracking: flag to track if at least one scan was inserted into submap since last update
|
|
// Used in ScanMapping mode to ensure we only update when there's new data
|
|
private volatile bool _hasInsertionSinceLastUpdate = false;
|
|
|
|
public OccupancyGrid? OccupancyGrid
|
|
{
|
|
get
|
|
{
|
|
lock (_lock) { return _occupancyGrid; }
|
|
}
|
|
}
|
|
|
|
public OccupancyGrid? OccupancyGridMcl
|
|
{
|
|
get
|
|
{
|
|
lock (_lock) { return _occupancyGridMcl; }
|
|
}
|
|
}
|
|
|
|
public DateTime LastUpdated
|
|
{
|
|
get
|
|
{
|
|
lock (_lock) { return _lastUpdatedOccupancyGrid; }
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get occupancy grid, optionally checking if updated since a given time
|
|
/// </summary>
|
|
public OccupancyGrid? GetGrid(DateTime since = default)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (since == DateTime.MinValue)
|
|
return _occupancyGrid;
|
|
return _lastUpdatedOccupancyGrid > since ? _occupancyGrid : null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Signal that a scan was inserted into a submap (InsertionResult.HasValue = true).
|
|
/// Called from ProcessWithTrajectoryBuilder when AddSensorData returns insertion result.
|
|
/// This flag is required for ShouldUpdateGrid to return true.
|
|
/// </summary>
|
|
public void SignalInsertion()
|
|
{
|
|
_hasInsertionSinceLastUpdate = true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Check if grid should be updated based on two conditions:
|
|
/// 1. Enough time has elapsed since last update (3 seconds) OR grid is null
|
|
/// 2. At least one scan was inserted into submap since last update (SignalInsertion was called)
|
|
/// Both conditions must be met for update to proceed.
|
|
/// Thread-safe: prevents concurrent generation via _generationInProgress flag.
|
|
/// </summary>
|
|
public bool ShouldUpdateGrid()
|
|
{
|
|
// Fast check: skip if generation is already in progress
|
|
if (Interlocked.CompareExchange(ref _generationInProgress, 0, 0) == 1)
|
|
return false;
|
|
|
|
lock (_lock)
|
|
{
|
|
bool gridIsNull = _occupancyGrid == null;
|
|
bool timeElapsed = _updateStopwatch.Elapsed.TotalSeconds >= UpdateIntervalSeconds;
|
|
bool hasInsertion = _hasInsertionSinceLastUpdate;
|
|
|
|
// Require BOTH: (time elapsed OR grid null) AND has insertion
|
|
if ((gridIsNull || timeElapsed) && hasInsertion)
|
|
{
|
|
_logger.LogDebug(
|
|
"OccupancyGridManager: ShouldUpdateGrid=true, gridIsNull={GridIsNull}, elapsed={Elapsed:F1}s, hasInsertion={HasInsertion}",
|
|
gridIsNull, _updateStopwatch.Elapsed.TotalSeconds, hasInsertion);
|
|
_updateStopwatch.Restart();
|
|
_hasInsertionSinceLastUpdate = false; // Reset insertion flag after update
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update occupancy grid from MapBuilder submaps during ScanMapping
|
|
/// Only generates Display grid for visualization (MCL not used during mapping)
|
|
/// </summary>
|
|
public void UpdateFromMapBuilder(IMapBuilder mapBuilder)
|
|
{
|
|
// Prevent concurrent generation
|
|
if (Interlocked.CompareExchange(ref _generationInProgress, 1, 0) != 0)
|
|
{
|
|
_logger.LogDebug("OccupancyGridManager: Skipping - generation already in progress");
|
|
return;
|
|
}
|
|
|
|
try
|
|
{
|
|
if (mapBuilder == null)
|
|
{
|
|
_logger.LogWarning("OccupancyGridManager: UpdateFromMapBuilder called with null mapBuilder");
|
|
return;
|
|
}
|
|
|
|
var resolution = _config.MapStorage.OccupancyGridResolution;
|
|
var padding = _config.MapStorage.MapPadding;
|
|
var strategy = _config.OccupancyGrid?.MergeStrategy ?? SubmapMergeStrategy.LogOddsSum;
|
|
|
|
var sw = System.Diagnostics.Stopwatch.StartNew();
|
|
|
|
// Generate display grid using configured merge strategy.
|
|
// Pass the last generated version so the generator can skip if no new nodes
|
|
// have been inserted (avoids redundant regeneration of identical data).
|
|
var displayGrid = OccupancyGridGenerator.Generate(
|
|
mapBuilder, resolution, padding,
|
|
Volatile.Read(ref _lastGeneratedVersion),
|
|
out var snapshotVersion,
|
|
_logger, _config.OccupancyGrid);
|
|
|
|
sw.Stop();
|
|
|
|
if (displayGrid == null)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_occupancyGrid = null;
|
|
_occupancyGridMcl = null;
|
|
_lastUpdatedOccupancyGrid = DateTime.MinValue;
|
|
}
|
|
return;
|
|
}
|
|
|
|
// Count cell statistics for debugging
|
|
int freeCells = 0, occupiedCells = 0, unknownCells = 0;
|
|
for (int i = 0; i < displayGrid.Data.Length; i++)
|
|
{
|
|
if (displayGrid.Data[i] < 0) unknownCells++;
|
|
else if (displayGrid.Data[i] == 0) freeCells++;
|
|
else occupiedCells++;
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
_occupancyGrid = displayGrid;
|
|
// Keep existing mclGrid (if any) - don't regenerate during mapping
|
|
_lastUpdatedOccupancyGrid = DateTime.UtcNow;
|
|
}
|
|
|
|
// Cache the version so the next cycle can skip if no new data was added.
|
|
Volatile.Write(ref _lastGeneratedVersion, snapshotVersion);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "OccupancyGridManager: Failed to update grid from MapBuilder");
|
|
}
|
|
finally
|
|
{
|
|
Interlocked.Exchange(ref _generationInProgress, 0);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Load occupancy grid from PGM file for localization
|
|
/// </summary>
|
|
public void LoadFromPgm(string mapName)
|
|
{
|
|
try
|
|
{
|
|
var mapPath = Path.Combine(_config.MapStorage.Directory, mapName);
|
|
var pgmPath = Path.Combine(mapPath, "map.pgm");
|
|
|
|
if (!File.Exists(pgmPath))
|
|
{
|
|
var pgmFiles = Directory.GetFiles(mapPath, "*.pgm", SearchOption.TopDirectoryOnly);
|
|
if (pgmFiles.Length == 0)
|
|
{
|
|
_logger.LogWarning("OccupancyGridManager: No PGM file found for map: {MapName}", mapName);
|
|
Clear();
|
|
return;
|
|
}
|
|
pgmPath = pgmFiles[0];
|
|
}
|
|
|
|
// Load grid once and clone for MCL (Fix 2: avoid loading PGM file twice)
|
|
// UNIFIED CONVENTION: Both use ROS convention (row 0 = world BOTTOM, Y-axis pointing UP)
|
|
var displayGrid = PgmLoader.LoadFromPgm(pgmPath, logger: _logger);
|
|
OccupancyGrid? mclGrid = null;
|
|
if (displayGrid != null)
|
|
{
|
|
mclGrid = new OccupancyGrid
|
|
{
|
|
Resolution = displayGrid.Resolution,
|
|
Width = displayGrid.Width,
|
|
Height = displayGrid.Height,
|
|
Origin = displayGrid.Origin,
|
|
Data = (sbyte[])displayGrid.Data.Clone()
|
|
};
|
|
}
|
|
|
|
// Load origin from map.json if available
|
|
if (displayGrid != null && mclGrid != null)
|
|
{
|
|
var mapJsonPath = Path.Combine(mapPath, "map.json");
|
|
if (File.Exists(mapJsonPath))
|
|
{
|
|
try
|
|
{
|
|
var jsonContent = File.ReadAllText(mapJsonPath);
|
|
var metadata = JsonSerializer.Deserialize<MapInfo>(jsonContent);
|
|
if (metadata?.Origin != null)
|
|
{
|
|
displayGrid.Origin = metadata.Origin;
|
|
mclGrid.Origin = metadata.Origin;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "OccupancyGridManager: Could not read origin from map.json");
|
|
}
|
|
}
|
|
}
|
|
|
|
lock (_lock)
|
|
{
|
|
_occupancyGrid = displayGrid;
|
|
_occupancyGridMcl = mclGrid;
|
|
_lastUpdatedOccupancyGrid = DateTime.UtcNow;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "OccupancyGridManager: Failed to load PGM for map: {MapName}", mapName);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clear grids for new scanning session
|
|
/// </summary>
|
|
public void Clear()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_occupancyGrid = null;
|
|
_occupancyGridMcl = null;
|
|
_lastUpdatedOccupancyGrid = DateTime.MinValue;
|
|
_updateStopwatch.Restart();
|
|
}
|
|
_hasInsertionSinceLastUpdate = false;
|
|
Volatile.Write(ref _lastGeneratedVersion, -1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reset timer and insertion flag for new state (ScanMapping/Localizing)
|
|
/// </summary>
|
|
public void ResetCounter()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_updateStopwatch.Restart();
|
|
}
|
|
_hasInsertionSinceLastUpdate = false;
|
|
Volatile.Write(ref _lastGeneratedVersion, -1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispose resources
|
|
/// </summary>
|
|
public void Dispose()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_occupancyGrid = null;
|
|
_occupancyGridMcl = null;
|
|
}
|
|
}
|
|
}
|