Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/TrajectoryConstraintCache.cs

@@ -0,0 +1,99 @@
+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;
+        }
+    }
+}