Initial commit
This commit is contained in:
@@ -0,0 +1,304 @@
|
||||
/*
|
||||
* Copyright 2016 The Cartographer Authors
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*/
|
||||
|
||||
using System.Collections.Concurrent;
|
||||
|
||||
namespace CartographerSharp.Mapping.Internal;
|
||||
|
||||
/// <summary>
|
||||
/// Thread-safe work queue for serializing pose graph operations.
|
||||
/// Similar to Cartographer C++ WorkQueue.
|
||||
///
|
||||
/// Operations are added to the queue non-blocking, and processed
|
||||
/// sequentially by a background thread to ensure thread-safety.
|
||||
/// </summary>
|
||||
public class WorkQueue : IDisposable
|
||||
{
|
||||
private readonly ConcurrentQueue<WorkItem> _queue = new();
|
||||
private readonly Lock _lock = new();
|
||||
private readonly CancellationTokenSource _cancellationTokenSource = new();
|
||||
private readonly AutoResetEvent _newItemEvent = new(false);
|
||||
private readonly ManualResetEvent _optimizationDoneEvent = new(false);
|
||||
private Thread? _processingThread;
|
||||
private bool _running = true;
|
||||
private bool _disposed = false;
|
||||
|
||||
/// <summary>
|
||||
/// Event raised when work queue needs optimization.
|
||||
/// IMPORTANT: Handler MUST call NotifyOptimizationDone() when optimization completes,
|
||||
/// otherwise work queue will be blocked forever.
|
||||
/// </summary>
|
||||
public event EventHandler? OptimizationNeeded;
|
||||
|
||||
/// <summary>
|
||||
/// Gets whether the work queue is empty.
|
||||
/// </summary>
|
||||
public bool IsEmpty => _queue.IsEmpty;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the number of items in the work queue.
|
||||
/// </summary>
|
||||
public int Count => _queue.Count;
|
||||
|
||||
public WorkQueue()
|
||||
{
|
||||
StartProcessing();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Starts background thread to process work queue with high priority.
|
||||
/// </summary>
|
||||
private void StartProcessing()
|
||||
{
|
||||
_processingThread = new Thread(() => ProcessWorkQueue(_cancellationTokenSource.Token))
|
||||
{
|
||||
// IMPORTANT (Linux RT): do NOT run Highest priority here.
|
||||
// This thread should never starve the sensor/scan-matching pipeline.
|
||||
Priority = ThreadPriority.Highest,
|
||||
IsBackground = true,
|
||||
Name = "CartographerWorkQueue"
|
||||
};
|
||||
_processingThread.Start();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Adds a work item to the queue. Non-blocking and thread-safe.
|
||||
/// </summary>
|
||||
public void AddWorkItem(WorkItem workItem)
|
||||
{
|
||||
ObjectDisposedException.ThrowIf(_disposed, this);
|
||||
|
||||
if (!_running)
|
||||
{
|
||||
throw new InvalidOperationException("WorkQueue is not running");
|
||||
}
|
||||
|
||||
_queue.Enqueue(workItem);
|
||||
// Wake processing thread if it is waiting.
|
||||
_newItemEvent.Set();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Notifies work queue that optimization has completed.
|
||||
/// This allows the work queue processing thread to resume.
|
||||
/// Match C++ behavior: After HandleWorkQueue completes (which runs optimization),
|
||||
/// DrainWorkQueue is called again to continue processing.
|
||||
/// </summary>
|
||||
public void NotifyOptimizationDone()
|
||||
{
|
||||
_optimizationDoneEvent.Set();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Processes work items until queue is empty or optimization is needed.
|
||||
/// Called by background thread.
|
||||
/// Match C++ behavior: When optimization is needed, STOP processing and WAIT
|
||||
/// until optimization completes (signaled via NotifyOptimizationDone).
|
||||
/// </summary>
|
||||
private void ProcessWorkQueue(CancellationToken cancellationToken)
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested && _running)
|
||||
{
|
||||
bool processedAny = false;
|
||||
bool optimizationNeeded = false;
|
||||
|
||||
while (_queue.TryDequeue(out var workItem))
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
var result = workItem.Action();
|
||||
|
||||
if (result == WorkItemResult.RunOptimization)
|
||||
{
|
||||
optimizationNeeded = true;
|
||||
// Stop processing to allow optimization to run
|
||||
break;
|
||||
}
|
||||
|
||||
processedAny = true;
|
||||
}
|
||||
|
||||
// Match C++: When optimization needed, STOP work queue and WAIT for optimization to complete
|
||||
if (optimizationNeeded)
|
||||
{
|
||||
// Reset event before invoking (in case it was set previously)
|
||||
_optimizationDoneEvent.Reset();
|
||||
|
||||
// Invoke optimization handler synchronously
|
||||
// CRITICAL: Handler MUST call NotifyOptimizationDone() when done
|
||||
OptimizationNeeded?.Invoke(this, EventArgs.Empty);
|
||||
|
||||
// WAIT for optimization to complete before continuing work queue
|
||||
// This matches C++ behavior where DrainWorkQueue() stops until HandleWorkQueue completes
|
||||
_optimizationDoneEvent.WaitOne();
|
||||
|
||||
// After optimization completes, continue processing work queue
|
||||
continue;
|
||||
}
|
||||
|
||||
// Avoid busy-spinning (especially harmful on RT kernels). If we didn't
|
||||
// process anything and no optimization was requested, wait for new work.
|
||||
if (!processedAny)
|
||||
{
|
||||
// Wakeups happen via AddWorkItem(). Also periodically wake to observe cancellation.
|
||||
// Check cancellation before waiting
|
||||
if (cancellationToken.IsCancellationRequested || !_running)
|
||||
{
|
||||
break;
|
||||
}
|
||||
_newItemEvent.WaitOne(TimeSpan.FromMilliseconds(50));
|
||||
}
|
||||
}
|
||||
}
|
||||
catch
|
||||
{
|
||||
throw;
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Drains the work queue synchronously (for testing or final processing).
|
||||
/// Match C++: Process work items until queue is empty or optimization is needed.
|
||||
///
|
||||
/// If processing thread is alive, waits for queue to empty.
|
||||
/// If processing thread is dead, processes remaining items.
|
||||
/// </summary>
|
||||
public void DrainWorkQueue()
|
||||
{
|
||||
// If processing thread is still alive, just wait for it to drain the queue
|
||||
if (_processingThread?.IsAlive == true)
|
||||
{
|
||||
// Wait for queue to be empty (processing thread will handle it)
|
||||
while (!_queue.IsEmpty)
|
||||
{
|
||||
Thread.Sleep(10);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// Processing thread is dead, we need to drain the queue ourselves
|
||||
bool processWorkQueue = true;
|
||||
while (processWorkQueue)
|
||||
{
|
||||
if (!_queue.TryDequeue(out var workItem))
|
||||
{
|
||||
// Queue is empty
|
||||
return;
|
||||
}
|
||||
|
||||
var result = workItem.Action();
|
||||
// Match C++: Continue processing if kDoNotRunOptimization, stop if kRunOptimization
|
||||
processWorkQueue = result == WorkItemResult.DoNotRunOptimization;
|
||||
|
||||
if (result == WorkItemResult.RunOptimization)
|
||||
{
|
||||
// Signal optimization needed (caller should handle this)
|
||||
OptimizationNeeded?.Invoke(this, EventArgs.Empty);
|
||||
// Stop processing to allow optimization to run
|
||||
// Caller should call DrainWorkQueue() again after optimization
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Waits for queue to be empty (with timeout).
|
||||
/// </summary>
|
||||
public void WaitForQueueToEmptyAsync(TimeSpan timeout)
|
||||
{
|
||||
var startTime = DateTime.UtcNow;
|
||||
|
||||
while (DateTime.UtcNow - startTime < timeout)
|
||||
{
|
||||
if (_queue.IsEmpty)
|
||||
{
|
||||
// Give a small delay to ensure no new items are being added
|
||||
Thread.Sleep(10);
|
||||
|
||||
if (_queue.IsEmpty)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Thread.Sleep(10);
|
||||
}
|
||||
|
||||
var remainingCount = _queue.Count;
|
||||
if (remainingCount > 0)
|
||||
{
|
||||
throw new TimeoutException($"Work queue not empty after timeout. Remaining items: {remainingCount}");
|
||||
}
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_running = false;
|
||||
}
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_running = false;
|
||||
_cancellationTokenSource.Cancel();
|
||||
|
||||
_newItemEvent.Set(); // wake thread so it can exit promptly
|
||||
|
||||
// Wait for thread to finish (with timeout)
|
||||
if (_processingThread != null)
|
||||
{
|
||||
// Check if thread is already finished before joining
|
||||
if (_processingThread.IsAlive)
|
||||
{
|
||||
if (!_processingThread.Join(TimeSpan.FromSeconds(5)))
|
||||
{
|
||||
// Thread didn't finish in time, but continue cleanup
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Clear event handlers to prevent memory leaks
|
||||
OptimizationNeeded = null;
|
||||
|
||||
// Drain remaining work items to prevent memory leaks
|
||||
DrainWorkQueue();
|
||||
|
||||
_newItemEvent.Dispose();
|
||||
_optimizationDoneEvent.Dispose();
|
||||
_cancellationTokenSource.Dispose();
|
||||
_disposed = true;
|
||||
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user