675 lines
26 KiB
C#
675 lines
26 KiB
C#
using CartographerSharp.Sensor;
|
|
using Microsoft.Extensions.Options;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.Shared.Geometry;
|
|
using System.Collections.Concurrent;
|
|
using System.Diagnostics;
|
|
using RobotNet10.Shared.Numbers;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
|
|
/// <summary>
|
|
/// Sensor pipeline: subscribes to Lidar/IMU/Odometry, queues data, and forwards to caller via actions.
|
|
/// Does not reference ITrajectoryBuilder or sample point cloud; CartographerService provides AddRangeData, AddImuData, AddOdometryData.
|
|
/// </summary>
|
|
internal sealed class SensorPipeline(
|
|
IDeviceProvider _deviceProvider,
|
|
IOdometryEstimator _odometryEstimator,
|
|
IOptions<CartographerConfiguration> configuration,
|
|
ILogger _logger,
|
|
Action<string, RangeDataPayload> _addRangeData,
|
|
Action<string, ImuData> _addImuData,
|
|
Action<string, OdometryData> _addOdometryData) : IDisposable
|
|
{
|
|
#region Fields
|
|
|
|
private readonly CartographerConfiguration _config = configuration.Value;
|
|
private readonly Lock _lock = new();
|
|
|
|
private readonly List<ILidar> _subscribedLidars = [];
|
|
private IInertialMeasurementUnit? _subscribedImu;
|
|
private bool _odometrySubscribed;
|
|
|
|
private volatile bool _active;
|
|
|
|
private Dictionary<string, LidarSensorConfiguration> _lidarConfigs = [];
|
|
private Dictionary<string, Transform> _lidarTransforms = [];
|
|
private Transform? _imuTransform;
|
|
|
|
private const int NotProcessing = 0;
|
|
private const int Processing = 1;
|
|
|
|
// Per-device processing flags: each lidar has its own flag
|
|
private readonly ConcurrentDictionary<string, int> _isProcessingLidar = new();
|
|
private long _lastValidEnqueueTimeTicks; // Use Volatile.Read/Write for thread-safe access
|
|
private const int LidarRoundTimeoutMs = 300;
|
|
|
|
private volatile List<string> _lidarDeviceOrder = [];
|
|
|
|
private struct LidarScanQueueItem
|
|
{
|
|
public string DeviceId { get; set; }
|
|
public LidarScanDataEventArgs ScanData { get; set; }
|
|
}
|
|
|
|
private readonly ConcurrentQueue<LidarScanQueueItem> _scanDataQueue = new();
|
|
|
|
// Transformed lidar data queue item (output of transform thread, input to processing thread)
|
|
private struct TransformedLidarQueueItem
|
|
{
|
|
public string DeviceId { get; set; }
|
|
public RangeDataPayload Payload { get; set; }
|
|
}
|
|
|
|
private readonly ConcurrentQueue<TransformedLidarQueueItem> _transformedQueue = new();
|
|
|
|
private struct ImuDataQueueItem
|
|
{
|
|
public string DeviceId { get; set; }
|
|
public Vector3 LinearAcceleration { get; set; }
|
|
public Vector3 AngularVelocity { get; set; }
|
|
public DateTime Timestamp { get; set; }
|
|
}
|
|
|
|
private readonly ConcurrentQueue<ImuDataQueueItem> _imuDataQueue = new();
|
|
|
|
private Thread? _lidarTransformThread;
|
|
private volatile bool _lidarTransformThreadRunning;
|
|
|
|
private Thread? _lidarProcessingThread;
|
|
private volatile bool _lidarProcessingThreadRunning;
|
|
|
|
private Thread? _imuProcessingThread;
|
|
private volatile bool _imuThreadRunning;
|
|
|
|
private Thread? _odometryProcessingThread;
|
|
private volatile bool _odometryThreadRunning;
|
|
|
|
private volatile bool _disposed;
|
|
|
|
// ManualResetEventSlim for efficient blocking when queues are empty
|
|
private readonly ManualResetEventSlim _lidarDataAvailable = new(false);
|
|
private readonly ManualResetEventSlim _transformedDataAvailable = new(false);
|
|
private readonly ManualResetEventSlim _imuDataAvailable = new(false);
|
|
|
|
// Queue capacity limits (Fix 9)
|
|
private const int MaxScanQueueSize = 5;
|
|
private const int MaxImuQueueSize = 50;
|
|
|
|
#endregion
|
|
|
|
#region Initialization
|
|
|
|
public async Task InitializeAsync()
|
|
{
|
|
await DiscoverAndSubscribeLidarsAsync();
|
|
await SubscribeToImuAsync();
|
|
SubscribeToOdometry();
|
|
}
|
|
|
|
public void ResumeSubscriptions()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_active)
|
|
{
|
|
_logger.LogDebug("SensorPipeline.ResumeSubscriptions: Already active, skipping");
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("SensorPipeline.ResumeSubscriptions: Resuming {LidarCount} lidars", _subscribedLidars.Count);
|
|
|
|
foreach (var lidar in _subscribedLidars)
|
|
{
|
|
lidar.ScanDataReceived += OnLidarScanDataReceived;
|
|
}
|
|
_subscribedImu?.ImuDataChanged += OnImuDataChanged;
|
|
|
|
if (_config.UseOdometry && _odometrySubscribed)
|
|
StartOdometryProcessingThread();
|
|
StartLidarProcessingThread();
|
|
if (_subscribedImu != null)
|
|
StartImuProcessingThread();
|
|
|
|
_active = true;
|
|
_logger.LogInformation("SensorPipeline.ResumeSubscriptions: Sensors resumed, active=true");
|
|
}
|
|
}
|
|
|
|
public void PauseSubscriptions()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_active)
|
|
{
|
|
_logger.LogDebug("SensorPipeline.PauseSubscriptions: Already inactive, skipping");
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("SensorPipeline.PauseSubscriptions: Pausing sensors");
|
|
|
|
foreach (var lidar in _subscribedLidars)
|
|
{
|
|
lidar.ScanDataReceived -= OnLidarScanDataReceived;
|
|
}
|
|
|
|
_subscribedImu?.ImuDataChanged -= OnImuDataChanged;
|
|
|
|
if (_odometrySubscribed)
|
|
StopOdometryProcessingThread();
|
|
StopLidarProcessingThread();
|
|
if (_subscribedImu != null)
|
|
StopImuProcessingThread();
|
|
|
|
_active = false;
|
|
_logger.LogInformation("SensorPipeline.PauseSubscriptions: Sensors paused, active=false");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Subscriptions
|
|
|
|
private async Task DiscoverAndSubscribeLidarsAsync()
|
|
{
|
|
var allLidars = await _deviceProvider.GetDevicesByTypeAsync(DeviceType.Lidar);
|
|
var lidarList = allLidars.OfType<ILidar>().ToList();
|
|
if (lidarList.Count == 0)
|
|
throw new InvalidOperationException("No Lidars found in device provider");
|
|
|
|
var lidarConfigsDict = new Dictionary<string, LidarSensorConfiguration>();
|
|
var lidarTransformsDict = new Dictionary<string, Transform>();
|
|
foreach (var lidar in lidarList)
|
|
{
|
|
var deviceId = (lidar as DeviceBase)?.DeviceId;
|
|
if (string.IsNullOrEmpty(deviceId))
|
|
continue;
|
|
var sensorConfig = _config.Sensors.Lidars.FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
|
|
if (sensorConfig != null)
|
|
{
|
|
lidarConfigsDict[deviceId] = sensorConfig;
|
|
lidarTransformsDict[deviceId] = sensorConfig.Transform;
|
|
_subscribedLidars.Add(lidar);
|
|
}
|
|
else
|
|
{
|
|
// FIXED: Skip lidars not configured in Cartographer.Sensors.Lidars
|
|
// This prevents unintended lidars (e.g., those used only for Detection)
|
|
// from being subscribed to CartographerSharp SLAM pipeline
|
|
_logger.LogDebug("SensorPipeline: Lidar {DeviceId} found but not enabled in Cartographer.Sensors.Lidars config, skipping", deviceId);
|
|
}
|
|
}
|
|
if (_subscribedLidars.Count == 0)
|
|
throw new InvalidOperationException("No Lidars to subscribe to");
|
|
_lidarConfigs = lidarConfigsDict;
|
|
_lidarTransforms = lidarTransformsDict;
|
|
|
|
// Initialize lidar device order and per-device processing flags
|
|
_lidarDeviceOrder = [.. _subscribedLidars
|
|
.Select(lidar => (lidar as DeviceBase)?.DeviceId)
|
|
.Where(id => !string.IsNullOrEmpty(id))
|
|
.Cast<string>()];
|
|
|
|
foreach (var lidarId in _lidarDeviceOrder)
|
|
{
|
|
_isProcessingLidar[lidarId] = NotProcessing;
|
|
}
|
|
}
|
|
|
|
private async Task SubscribeToImuAsync()
|
|
{
|
|
if (!_config.Sensors.Imu.Enabled || string.IsNullOrEmpty(_config.Sensors.Imu.DeviceId))
|
|
return;
|
|
_imuTransform = _config.Sensors.Imu.Transform;
|
|
var imu = await _deviceProvider.GetDeviceAsync(_config.Sensors.Imu.DeviceId);
|
|
if (imu is IInertialMeasurementUnit imuDevice)
|
|
_subscribedImu = imuDevice;
|
|
else
|
|
_logger.LogWarning("SensorPipeline: IMU device {DeviceId} not found or not an IMU", _config.Sensors.Imu.DeviceId);
|
|
}
|
|
|
|
private void SubscribeToOdometry()
|
|
{
|
|
if (!_config.UseOdometry || _odometrySubscribed)
|
|
return;
|
|
_odometrySubscribed = true;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Lidar Processing
|
|
|
|
private void StartLidarProcessingThread()
|
|
{
|
|
if (_lidarTransformThreadRunning || _lidarProcessingThreadRunning)
|
|
return;
|
|
|
|
// Start transform thread
|
|
_lidarTransformThreadRunning = true;
|
|
_lidarTransformThread = new Thread(LidarTransformThreadProc) { Name = "LidarTransform", Priority = ThreadPriority.Highest, IsBackground = true };
|
|
_lidarTransformThread.Start();
|
|
|
|
// Start processing thread
|
|
_lidarProcessingThreadRunning = true;
|
|
_lidarProcessingThread = new Thread(LidarProcessingThreadProc) { Name = "LidarProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
|
_lidarProcessingThread.Start();
|
|
}
|
|
|
|
private void StopLidarProcessingThread()
|
|
{
|
|
// Stop transform thread
|
|
if (_lidarTransformThreadRunning)
|
|
{
|
|
_lidarTransformThreadRunning = false;
|
|
_lidarDataAvailable.Set(); // Wake thread to check flag
|
|
if (_lidarTransformThread != null)
|
|
{
|
|
_lidarTransformThread.Join(2000);
|
|
if (_lidarTransformThread.IsAlive)
|
|
_logger.LogWarning("SensorPipeline: Lidar transform thread did not stop in time");
|
|
_lidarTransformThread = null;
|
|
}
|
|
}
|
|
|
|
// Stop processing thread
|
|
if (_lidarProcessingThreadRunning)
|
|
{
|
|
_lidarProcessingThreadRunning = false;
|
|
_transformedDataAvailable.Set(); // Wake thread to check flag
|
|
if (_lidarProcessingThread != null)
|
|
{
|
|
_lidarProcessingThread.Join(2000);
|
|
if (_lidarProcessingThread.IsAlive)
|
|
_logger.LogWarning("SensorPipeline: Lidar processing thread did not stop in time");
|
|
_lidarProcessingThread = null;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform thread: dequeues raw scan data, transforms to RangeDataPayload, enqueues to transformed queue.
|
|
/// Does NOT check _isProcessingLidar flags - just processes everything in _scanDataQueue.
|
|
/// </summary>
|
|
private void LidarTransformThreadProc()
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
while (_lidarTransformThreadRunning)
|
|
{
|
|
_lidarDataAvailable.Wait(50); // timeout to check _lidarTransformThreadRunning
|
|
_lidarDataAvailable.Reset();
|
|
|
|
if (_scanDataQueue.IsEmpty)
|
|
continue;
|
|
|
|
while (_scanDataQueue.TryDequeue(out var queueItem))
|
|
{
|
|
if (_disposed || !_active ||
|
|
!_lidarConfigs.TryGetValue(queueItem.DeviceId, out var lidarConfig) ||
|
|
!_lidarTransforms.TryGetValue(queueItem.DeviceId, out var lidarTransform))
|
|
continue;
|
|
|
|
try
|
|
{
|
|
var (baseLink, sensorFrame, actualAngleMin, actualAngleMax, angleIncrement) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
|
|
queueItem.ScanData.Timestamp, queueItem.ScanData.MeasurementData,
|
|
lidarTransform, lidarConfig.AngleMin, lidarConfig.AngleMax);
|
|
|
|
_transformedQueue.Enqueue(new TransformedLidarQueueItem
|
|
{
|
|
DeviceId = queueItem.DeviceId,
|
|
Payload = new RangeDataPayload
|
|
{
|
|
BaseLink = baseLink,
|
|
SensorFrame = sensorFrame,
|
|
ActualAngleMin = actualAngleMin,
|
|
ActualAngleMax = actualAngleMax,
|
|
ActualAngleIncrement = angleIncrement
|
|
}
|
|
});
|
|
_transformedDataAvailable.Set();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error transforming Lidar from {DeviceId}", queueItem.DeviceId);
|
|
}
|
|
}
|
|
}
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Processing thread: dequeues transformed data, calls _addRangeData, handles round tracking and timeout.
|
|
/// </summary>
|
|
private void LidarProcessingThreadProc()
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
var processedLidarsInRound = new HashSet<string>();
|
|
var expectedLidarCount = _lidarDeviceOrder.Count;
|
|
|
|
while (_lidarProcessingThreadRunning)
|
|
{
|
|
_transformedDataAvailable.Wait(50); // timeout to check _lidarProcessingThreadRunning
|
|
_transformedDataAvailable.Reset();
|
|
|
|
if (_transformedQueue.IsEmpty)
|
|
{
|
|
// Check timeout if we're in the middle of a round (at least one lidar processed)
|
|
if (processedLidarsInRound.Count > 0)
|
|
{
|
|
var lastEnqueueTicks = Volatile.Read(ref _lastValidEnqueueTimeTicks);
|
|
var elapsedMs = (DateTime.UtcNow.Ticks - lastEnqueueTicks) / TimeSpan.TicksPerMillisecond;
|
|
if (elapsedMs >= LidarRoundTimeoutMs)
|
|
{
|
|
var missingLidars = _lidarDeviceOrder.Except(processedLidarsInRound).ToList();
|
|
_logger.LogError(
|
|
"SensorPipeline: Lidar round timeout after {ElapsedMs}ms. Processed {ProcessedCount}/{ExpectedCount} lidars. Processed: [{ProcessedList}]. Missing: [{MissingList}]",
|
|
elapsedMs, processedLidarsInRound.Count, expectedLidarCount,
|
|
string.Join(", ", processedLidarsInRound),
|
|
string.Join(", ", missingLidars));
|
|
throw new TimeoutException($"SensorPipeline: Lidar round timeout - processed {processedLidarsInRound.Count}/{expectedLidarCount} lidars, missing: [{string.Join(", ", missingLidars)}]");
|
|
}
|
|
}
|
|
continue;
|
|
}
|
|
|
|
while (_transformedQueue.TryDequeue(out var queueItem))
|
|
{
|
|
if (_disposed || !_active)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
_addRangeData(queueItem.DeviceId, queueItem.Payload);
|
|
|
|
// Track processed lidar in this round
|
|
processedLidarsInRound.Add(queueItem.DeviceId);
|
|
|
|
// Check if all lidars have been processed in this round
|
|
if (processedLidarsInRound.Count >= expectedLidarCount)
|
|
{
|
|
// Reset all per-device flags to allow new round
|
|
foreach (var lidarId in _lidarDeviceOrder)
|
|
{
|
|
_isProcessingLidar[lidarId] = NotProcessing;
|
|
}
|
|
processedLidarsInRound.Clear();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error processing Lidar from {DeviceId}", queueItem.DeviceId);
|
|
}
|
|
}
|
|
}
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region IMU Processing
|
|
|
|
private void StartImuProcessingThread()
|
|
{
|
|
if (_imuThreadRunning)
|
|
return;
|
|
_imuThreadRunning = true;
|
|
_imuProcessingThread = new Thread(ImuProcessingThreadProc) { Name = "ImuProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
|
_imuProcessingThread.Start();
|
|
}
|
|
|
|
private void StopImuProcessingThread()
|
|
{
|
|
if (!_imuThreadRunning)
|
|
return;
|
|
_imuThreadRunning = false;
|
|
_imuDataAvailable.Set(); // Wake thread to check flag
|
|
if (_imuProcessingThread != null)
|
|
{
|
|
_imuProcessingThread.Join(2000);
|
|
if (_imuProcessingThread.IsAlive)
|
|
_logger.LogWarning("SensorPipeline: IMU processing thread did not stop in time");
|
|
_imuProcessingThread = null;
|
|
}
|
|
}
|
|
|
|
private void ImuProcessingThreadProc()
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
var imuUpdateIntervalMs = _config.Sensors.ImuUpdateIntervalMs;
|
|
var imuMinIntervalTicks = imuUpdateIntervalMs * TimeSpan.TicksPerMillisecond;
|
|
var lastProcessTimeTicks = 0L;
|
|
while (_imuThreadRunning)
|
|
{
|
|
_imuDataAvailable.Wait(50); // timeout to check _imuThreadRunning
|
|
_imuDataAvailable.Reset();
|
|
|
|
while (_imuDataQueue.TryDequeue(out var queueItem))
|
|
{
|
|
if (_disposed || !_active || _imuTransform == null)
|
|
continue;
|
|
|
|
// Rate limiting: skip if too soon since last process (when interval > 0)
|
|
var currentTicks = queueItem.Timestamp.Ticks;
|
|
if (imuMinIntervalTicks > 0 && lastProcessTimeTicks > 0 &&
|
|
currentTicks - lastProcessTimeTicks < imuMinIntervalTicks)
|
|
continue;
|
|
|
|
try
|
|
{
|
|
var imuData = SensorDataTransformHelper.ToImuData(
|
|
queueItem.LinearAcceleration, queueItem.AngularVelocity, queueItem.Timestamp, _imuTransform.Value);
|
|
_addImuData(queueItem.DeviceId, imuData);
|
|
lastProcessTimeTicks = currentTicks;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error processing IMU from {DeviceId}", queueItem.DeviceId);
|
|
}
|
|
}
|
|
}
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Odometry Processing
|
|
|
|
private void StartOdometryProcessingThread()
|
|
{
|
|
if (_odometryThreadRunning)
|
|
return;
|
|
_odometryThreadRunning = true;
|
|
_odometryProcessingThread = new Thread(OdometryProcessingThreadProc) { Name = "OdometryProcessor", Priority = ThreadPriority.Highest, IsBackground = true };
|
|
_odometryProcessingThread.Start();
|
|
}
|
|
|
|
private void StopOdometryProcessingThread()
|
|
{
|
|
if (!_odometryThreadRunning)
|
|
return;
|
|
_odometryThreadRunning = false;
|
|
if (_odometryProcessingThread != null)
|
|
{
|
|
_odometryProcessingThread.Join(1000);
|
|
if (_odometryProcessingThread.IsAlive)
|
|
_logger.LogWarning("SensorPipeline: Odometry processing thread did not stop in time");
|
|
_odometryProcessingThread = null;
|
|
}
|
|
}
|
|
|
|
private void OdometryProcessingThreadProc()
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
var spinWait = new SpinWait();
|
|
var stopwatch = Stopwatch.StartNew();
|
|
var odometryUpdateIntervalMs = _config.Sensors.OdometryUpdateIntervalMs;
|
|
var targetTicksPerInterval = odometryUpdateIntervalMs * Stopwatch.Frequency / 1000;
|
|
var lastProcessTimeTicks = stopwatch.ElapsedTicks;
|
|
while (_odometryThreadRunning)
|
|
{
|
|
var currentTicks = stopwatch.ElapsedTicks;
|
|
var remaining = targetTicksPerInterval - (currentTicks - lastProcessTimeTicks);
|
|
if (remaining > Stopwatch.Frequency / 100) // > 10ms remaining
|
|
Thread.Sleep(1);
|
|
else
|
|
spinWait.SpinOnce();
|
|
|
|
currentTicks = stopwatch.ElapsedTicks;
|
|
if (currentTicks - lastProcessTimeTicks >= targetTicksPerInterval)
|
|
{
|
|
if (!_disposed && _active)
|
|
{
|
|
try
|
|
{
|
|
var currentPose = _odometryEstimator.CurrentPose;
|
|
var currentTimeTicks = DateTime.UtcNow.Ticks;
|
|
|
|
// Extract theta from quaternion
|
|
var currentTheta = currentPose.Orientation.ToYawRadian();
|
|
var currentX = currentPose.Position.X;
|
|
var currentY = currentPose.Position.Y;
|
|
|
|
var odometryData = SensorDataTransformHelper.ToOdometryData(currentPose, currentTimeTicks);
|
|
_addOdometryData("odometry", odometryData);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error processing odometry at {Ticks}", DateTime.UtcNow.Ticks);
|
|
}
|
|
}
|
|
lastProcessTimeTicks = currentTicks;
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Event Handlers
|
|
|
|
private void OnLidarScanDataReceived(object? sender, LidarScanDataEventArgs e)
|
|
{
|
|
if (_disposed || !_active || sender is not DeviceBase device)
|
|
return;
|
|
if (_scanDataQueue.Count >= MaxScanQueueSize)
|
|
return; // Drop: processing can't keep up
|
|
|
|
var deviceId = device.DeviceId;
|
|
|
|
// Check if this lidar already has data in the current round
|
|
if (_isProcessingLidar.TryGetValue(deviceId, out var status) && status == Processing)
|
|
return; // Drop: this lidar already has data in current round
|
|
|
|
// Set flag to indicate this lidar has data in current round
|
|
_isProcessingLidar[deviceId] = Processing;
|
|
|
|
try
|
|
{
|
|
_scanDataQueue.Enqueue(new LidarScanQueueItem { DeviceId = deviceId, ScanData = e });
|
|
Volatile.Write(ref _lastValidEnqueueTimeTicks, DateTime.UtcNow.Ticks);
|
|
_lidarDataAvailable.Set();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
// Reset flag on error so this lidar can try again
|
|
_isProcessingLidar[deviceId] = NotProcessing;
|
|
_logger.LogError(ex, "SensorPipeline: Error enqueueing Lidar from {DeviceId}", deviceId);
|
|
}
|
|
}
|
|
|
|
private void OnImuDataChanged(object? sender, ImuDataChangedEventArgs e)
|
|
{
|
|
if (sender is not DeviceBase device || _disposed || string.IsNullOrEmpty(device.DeviceId) || _imuTransform == null || !_active)
|
|
return;
|
|
if (_imuDataQueue.Count >= MaxImuQueueSize)
|
|
return; // Drop: processing can't keep up
|
|
try
|
|
{
|
|
_imuDataQueue.Enqueue(new ImuDataQueueItem
|
|
{
|
|
DeviceId = device.DeviceId,
|
|
LinearAcceleration = e.Acceleration.Accel.Linear,
|
|
AngularVelocity = e.AngularVelocity.Vector,
|
|
Timestamp = e.Timestamp
|
|
});
|
|
_imuDataAvailable.Set();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error enqueueing IMU from {DeviceId}", device.DeviceId);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Processing Status
|
|
|
|
/// <summary>
|
|
/// Wait for all queued data to be processed, with timeout.
|
|
/// Returns true if all queues are drained within the timeout.
|
|
/// </summary>
|
|
public bool WaitForProcessingComplete(int timeoutMs = 200)
|
|
{
|
|
var sw = Stopwatch.StartNew();
|
|
var spin = new SpinWait();
|
|
while (sw.ElapsedMilliseconds < timeoutMs)
|
|
{
|
|
// Check if all queues are empty and no lidar is being processed
|
|
var allLidarsNotProcessing = _isProcessingLidar.Values.All(status => status == NotProcessing);
|
|
if (_scanDataQueue.IsEmpty && _transformedQueue.IsEmpty && _imuDataQueue.IsEmpty && allLidarsNotProcessing)
|
|
return true;
|
|
spin.SpinOnce();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region IDisposable
|
|
|
|
public void Dispose()
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
_disposed = true;
|
|
try
|
|
{
|
|
PauseSubscriptions();
|
|
foreach (var lidar in _subscribedLidars)
|
|
lidar.ScanDataReceived -= OnLidarScanDataReceived;
|
|
_subscribedLidars.Clear();
|
|
_subscribedImu?.ImuDataChanged -= OnImuDataChanged;
|
|
_subscribedImu = null;
|
|
if (_odometrySubscribed)
|
|
{
|
|
StopOdometryProcessingThread();
|
|
_odometrySubscribed = false;
|
|
}
|
|
StopLidarProcessingThread();
|
|
StopImuProcessingThread();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SensorPipeline: Error during Dispose");
|
|
}
|
|
finally
|
|
{
|
|
_lidarDataAvailable.Dispose();
|
|
_transformedDataAvailable.Dispose();
|
|
_imuDataAvailable.Dispose();
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
}
|