Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Xloc/XlocAsyncDispatcher.cs

@@ -0,0 +1,199 @@
+using System.Threading.Channels;
+
+namespace RobotNet10.RobotApp.Xloc;
+
+/// <summary>
+/// Single-thread async dispatcher for xloc sensor data — mirrors ROS ros::spin() model.
+/// All sensor types (IMU, Odom, Scan) are dispatched sequentially by ONE worker thread,
+/// eliminating lock contention on the native xloc library.
+///
+/// ROS model: ros::spin() processes all callbacks in a single thread, no mutex needed.
+/// This dispatcher replicates that pattern: one queue, one worker, zero lock contention.
+/// </summary>
+public class XlocAsyncDispatcher : IDisposable
+{
+    private readonly XlocClient _xlocClient;
+    private readonly ILogger? _logger;
+
+    // Single unified queue for ALL sensor types — like ROS callback queue
+    private readonly Channel<SensorDispatchEvent> _dispatchQueue;
+    private readonly Task _workerTask;
+    private readonly CancellationTokenSource _cts;
+    private bool _disposed = false;
+
+    private const int DefaultQueueCapacity = 300;
+
+    private readonly int _queueCapacity;
+    private long _dropCount;
+
+    /// <summary>
+    /// Sensor dispatch event wrapper
+    /// </summary>
+    private record SensorDispatchEvent(
+        string SensorType,
+        string SensorId,
+        Action DispatchAction,
+        long EnqueueTimestamp);
+
+    public XlocAsyncDispatcher(
+        XlocClient xlocClient,
+        int queueCapacity = DefaultQueueCapacity,
+        ILogger? logger = null)
+    {
+        _xlocClient = xlocClient ?? throw new ArgumentNullException(nameof(xlocClient));
+        _logger = logger;
+        _queueCapacity = queueCapacity;
+
+        _dispatchQueue = Channel.CreateBounded<SensorDispatchEvent>(new BoundedChannelOptions(queueCapacity)
+        {
+            FullMode = BoundedChannelFullMode.DropOldest
+        });
+
+        _cts = new CancellationTokenSource();
+        _workerTask = RunWorker(_cts.Token);
+    }
+
+    public ValueTask EnqueueOdometryAsync(Action dispatchAction, string sensorId = "odom")
+    {
+        ThrowIfDisposed();
+        return EnqueueAsync("odometry", sensorId, dispatchAction);
+    }
+
+    public ValueTask EnqueueImuAsync(Action dispatchAction, string sensorId = "imu")
+    {
+        ThrowIfDisposed();
+        return EnqueueAsync("imu", sensorId, dispatchAction);
+    }
+
+    public ValueTask EnqueueLaserScanAsync(Action dispatchAction, string sensorId)
+    {
+        ThrowIfDisposed();
+        return EnqueueAsync("laserscan", sensorId, dispatchAction);
+    }
+
+    private ValueTask EnqueueAsync(string sensorType, string sensorId, Action dispatchAction)
+    {
+        var enqueueTs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
+        var @event = new SensorDispatchEvent(sensorType, sensorId, dispatchAction, enqueueTs);
+
+        if (_dispatchQueue.Reader.Count >= _queueCapacity)
+        {
+            Interlocked.Increment(ref _dropCount);
+        }
+
+        try
+        {
+            return _dispatchQueue.Writer.WriteAsync(@event, _cts.Token);
+        }
+        catch (ChannelClosedException)
+        {
+            throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
+        }
+    }
+
+    private async Task RunWorker(CancellationToken ct)
+    {
+        _logger?.LogInformation("[XLOC-ASYNC] Single dispatch worker started (ROS spin model)");
+
+        try
+        {
+            await foreach (var @event in _dispatchQueue.Reader.ReadAllAsync(ct))
+            {
+                ExecuteDispatchEvent(@event);
+            }
+        }
+        catch (OperationCanceledException)
+        {
+            _logger?.LogInformation("[XLOC-ASYNC] Dispatch worker cancelled");
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "[XLOC-ASYNC-FATAL] Dispatch worker crashed: {Message}", ex.Message);
+        }
+        finally
+        {
+            _logger?.LogInformation("[XLOC-ASYNC] Dispatch worker stopped");
+        }
+    }
+
+    private void ExecuteDispatchEvent(SensorDispatchEvent @event)
+    {
+        var queueWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - @event.EnqueueTimestamp;
+
+        try
+        {
+            var dispatchStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
+
+            @event.DispatchAction();
+
+            var dispatchElapsedMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - dispatchStart;
+
+            _logger?.LogTrace(
+                "[XLOC-ASYNC] {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
+                @event.SensorType,
+                @event.SensorId,
+                queueWaitMs,
+                dispatchElapsedMs);
+
+            if (queueWaitMs >= 10 || dispatchElapsedMs >= 10)
+            {
+                // _logger?.LogWarning(
+                //     "[XLOC-DIAG] AsyncDispatcher {SensorType} sensor={SensorId} queue_wait={QueueWaitMs}ms dispatch={DispatchMs}ms",
+                //     @event.SensorType,
+                //     @event.SensorId,
+                //     queueWaitMs,
+                //     dispatchElapsedMs);
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex,
+                "[XLOC-ASYNC-ERROR] Failed to dispatch {SensorType} sensor={SensorId}: {Message}",
+                @event.SensorType,
+                @event.SensorId,
+                ex.Message);
+        }
+    }
+
+    public void Dispose()
+    {
+        Dispose(true);
+        GC.SuppressFinalize(this);
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (_disposed)
+            return;
+
+        _logger?.LogInformation("[XLOC-ASYNC] Shutting down dispatcher");
+
+        _dispatchQueue.Writer.TryComplete();
+        _cts.Cancel();
+
+        try
+        {
+            _workerTask.Wait(TimeSpan.FromSeconds(5));
+        }
+        catch (AggregateException ex)
+        {
+            _logger?.LogWarning(ex, "[XLOC-ASYNC] Worker task did not complete gracefully");
+        }
+
+        _cts.Dispose();
+
+        _disposed = true;
+        _logger?.LogInformation("[XLOC-ASYNC] Dispatcher shut down");
+    }
+
+    private void ThrowIfDisposed()
+    {
+        if (_disposed)
+            throw new InvalidOperationException("XlocAsyncDispatcher has been disposed.");
+    }
+
+    ~XlocAsyncDispatcher()
+    {
+        Dispose(false);
+    }
+}