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