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