100 lines
3.1 KiB
C#
100 lines
3.1 KiB
C#
using CartographerSharp.Mapping;
|
|
using Microsoft.Extensions.Logging;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Cache for pose graph constraints of a single trajectory.
|
|
/// Used by CartographerService to avoid repeatedly querying constraints when calculating covariance.
|
|
/// </summary>
|
|
/// <remarks>
|
|
/// Creates a constraint cache with the given refresh interval.
|
|
/// </remarks>
|
|
public sealed class TrajectoryConstraintCache(TimeSpan _updateInterval)
|
|
{
|
|
private readonly Lock _lock = new();
|
|
private List<IPoseGraph.Constraint>? _cachedConstraints;
|
|
private DateTime _lastUpdate = DateTime.MinValue;
|
|
private IMapBuilder? _lastMapBuilder;
|
|
private int _lastTrajectoryId = -1;
|
|
|
|
/// <summary>
|
|
/// Gets constraints for the given trajectory, refreshing from the pose graph when the cache is stale
|
|
/// or when map builder / trajectory id change.
|
|
/// </summary>
|
|
/// <param name="mapBuilder">Current map builder (may be null)</param>
|
|
/// <param name="trajectoryId">Trajectory id to filter constraints</param>
|
|
/// <param name="logger">Optional logger for warnings</param>
|
|
/// <returns>Cached or freshly fetched constraints, or null if mapBuilder is null or fetch failed</returns>
|
|
public List<IPoseGraph.Constraint>? GetOrUpdate(
|
|
IMapBuilder? mapBuilder,
|
|
int trajectoryId,
|
|
ILogger? logger = null)
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
bool needsUpdate = false;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (mapBuilder != _lastMapBuilder || trajectoryId != _lastTrajectoryId)
|
|
{
|
|
needsUpdate = true;
|
|
}
|
|
else if (_cachedConstraints == null || (now - _lastUpdate) > _updateInterval)
|
|
{
|
|
needsUpdate = true;
|
|
}
|
|
}
|
|
|
|
if (!needsUpdate)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _cachedConstraints;
|
|
}
|
|
}
|
|
|
|
if (mapBuilder == null)
|
|
{
|
|
return null;
|
|
}
|
|
|
|
try
|
|
{
|
|
var constraints = mapBuilder.PoseGraph.Constraints().Where(c => c.NodeId.TrajectoryId == trajectoryId).ToList();
|
|
|
|
lock (_lock)
|
|
{
|
|
_cachedConstraints = constraints;
|
|
_lastUpdate = now;
|
|
_lastMapBuilder = mapBuilder;
|
|
_lastTrajectoryId = trajectoryId;
|
|
}
|
|
|
|
return constraints;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logger?.LogWarning(ex, "TrajectoryConstraintCache: Failed to update constraint cache");
|
|
lock (_lock)
|
|
{
|
|
return _cachedConstraints;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clears the cache (e.g. when switching map or trajectory).
|
|
/// </summary>
|
|
public void Invalidate()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_cachedConstraints = null;
|
|
_lastUpdate = DateTime.MinValue;
|
|
_lastMapBuilder = null;
|
|
_lastTrajectoryId = -1;
|
|
}
|
|
}
|
|
}
|