Initial commit
This commit is contained in:
@@ -0,0 +1,434 @@
|
||||
using System.Collections.Concurrent;
|
||||
using System.Diagnostics;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
|
||||
namespace Olei.LidarSensor;
|
||||
|
||||
/// <summary>
|
||||
/// High-performance UDP server for receiving and parsing Olei LiDAR data
|
||||
/// Uses Socket API with pre-allocated buffers for zero-allocation receive
|
||||
/// Optimized for low latency and minimal memory allocation
|
||||
/// </summary>
|
||||
public sealed class OleiLidarServer : IDisposable
|
||||
{
|
||||
private readonly int _port;
|
||||
private Socket? _socket;
|
||||
private Thread? _receiveThread;
|
||||
private volatile bool _isRunning;
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
// Pre-allocated receive buffer (reused across all receives)
|
||||
private byte[]? _receiveBuffer;
|
||||
private const int RECEIVE_BUFFER_SIZE = LidarDataPacket.PACKET_SIZE * 16; // Buffer for multiple packets
|
||||
|
||||
// Packet pool for reuse
|
||||
private readonly ConcurrentBag<LidarDataPacket> _packetPool;
|
||||
private const int INITIAL_PACKET_POOL_SIZE = 10;
|
||||
private const int MAX_PACKET_POOL_SIZE = 50;
|
||||
|
||||
// Pool size tracking (atomic counter for thread-safe accurate tracking)
|
||||
private long _packetPoolCount;
|
||||
|
||||
// Statistics
|
||||
private long _totalPacketsReceived;
|
||||
private long _totalPacketsParsed;
|
||||
private long _totalParseErrors;
|
||||
private long _totalPacketsCreated;
|
||||
|
||||
// Scan frequency calculation (optimized - update only once per second)
|
||||
private readonly Stopwatch _frequencyStopwatch = new();
|
||||
private long _packetsReceivedInCurrentSecond;
|
||||
private long _lastFrequencyUpdateMs;
|
||||
|
||||
/// <summary>
|
||||
/// Event raised when a valid LiDAR packet is received and parsed
|
||||
/// </summary>
|
||||
public event EventHandler<LidarDataPacket>? DataReceived;
|
||||
|
||||
/// <summary>
|
||||
/// Event raised when an error occurs during packet reception or parsing
|
||||
/// </summary>
|
||||
public event EventHandler<LidarErrorEventArgs>? ErrorOccurred;
|
||||
|
||||
/// <summary>
|
||||
/// Total number of packets received (including invalid ones)
|
||||
/// </summary>
|
||||
public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);
|
||||
|
||||
/// <summary>
|
||||
/// Total number of packets successfully parsed
|
||||
/// </summary>
|
||||
public long TotalPacketsParsed => Interlocked.Read(ref _totalPacketsParsed);
|
||||
|
||||
/// <summary>
|
||||
/// Total number of parse errors
|
||||
/// </summary>
|
||||
public long TotalParseErrors => Interlocked.Read(ref _totalParseErrors);
|
||||
|
||||
/// <summary>
|
||||
/// Total number of packets created (for monitoring allocation rate)
|
||||
/// </summary>
|
||||
public long TotalPacketsCreated => Interlocked.Read(ref _totalPacketsCreated);
|
||||
|
||||
/// <summary>
|
||||
/// Current number of packets in the pool
|
||||
/// </summary>
|
||||
public long CurrentPoolSize => Interlocked.Read(ref _packetPoolCount);
|
||||
|
||||
/// <summary>
|
||||
/// Scan frequency (Hz)
|
||||
/// Typically 10-20 Hz for Olei LiDAR
|
||||
/// </summary>
|
||||
public double? ScanFrequencyHz { get; private set; }
|
||||
|
||||
/// <summary>
|
||||
/// Check if server is currently running
|
||||
/// </summary>
|
||||
public bool IsRunning => _isRunning;
|
||||
|
||||
/// <summary>
|
||||
/// Create a new Olei LiDAR server
|
||||
/// </summary>
|
||||
/// <param name="port">UDP port to listen on</param>
|
||||
public OleiLidarServer(int port)
|
||||
{
|
||||
_port = port;
|
||||
_packetPool = [];
|
||||
_packetPoolCount = 0;
|
||||
_totalPacketsCreated = 0;
|
||||
|
||||
// Pre-allocate packet pool
|
||||
for (int i = 0; i < INITIAL_PACKET_POOL_SIZE; i++)
|
||||
{
|
||||
_packetPool.Add(new LidarDataPacket());
|
||||
Interlocked.Increment(ref _packetPoolCount);
|
||||
Interlocked.Increment(ref _totalPacketsCreated);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Start receiving LiDAR data
|
||||
/// </summary>
|
||||
public void Start()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (_isRunning)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
// Create socket with optimized settings
|
||||
_socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
|
||||
_socket.Bind(new IPEndPoint(IPAddress.Any, _port));
|
||||
|
||||
// Set socket options for high-performance UDP reception
|
||||
_socket.ReceiveBufferSize = 2 * 1024 * 1024; // 2MB OS buffer
|
||||
_socket.ReceiveTimeout = 0; // Blocking receive (no timeout)
|
||||
|
||||
// Allocate receive buffer once
|
||||
_receiveBuffer = new byte[RECEIVE_BUFFER_SIZE];
|
||||
|
||||
_isRunning = true;
|
||||
|
||||
// Reset frequency calculation
|
||||
_packetsReceivedInCurrentSecond = 0;
|
||||
_lastFrequencyUpdateMs = 0;
|
||||
_frequencyStopwatch.Restart();
|
||||
|
||||
// Start dedicated receive thread with high priority
|
||||
_receiveThread = new Thread(ReceiveLoop)
|
||||
{
|
||||
Name = $"OleiLidar-Receive-{_port}",
|
||||
IsBackground = false,
|
||||
Priority = ThreadPriority.Highest
|
||||
};
|
||||
_receiveThread.Start();
|
||||
|
||||
Console.WriteLine($"[OleiLidarServer] Started on port {_port}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_isRunning = false;
|
||||
OnError(new LidarErrorEventArgs($"Failed to start server: {ex.Message}", ex));
|
||||
throw;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stop receiving LiDAR data
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_isRunning)
|
||||
return;
|
||||
|
||||
_isRunning = false;
|
||||
|
||||
try
|
||||
{
|
||||
_socket?.Close();
|
||||
_socket?.Dispose();
|
||||
_socket = null;
|
||||
|
||||
// Wait for receive thread to exit (with timeout)
|
||||
_receiveThread?.Join(TimeSpan.FromSeconds(2));
|
||||
_receiveThread = null;
|
||||
|
||||
// Clean up receive buffer
|
||||
_receiveBuffer = null;
|
||||
|
||||
_frequencyStopwatch.Stop();
|
||||
|
||||
Console.WriteLine($"[OleiLidarServer] Stopped on port {_port}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
OnError(new LidarErrorEventArgs($"Error stopping server: {ex.Message}", ex));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Main receive loop running in dedicated thread
|
||||
/// Uses blocking Socket.ReceiveFrom with pre-allocated buffer for zero-allocation receive
|
||||
/// </summary>
|
||||
private void ReceiveLoop()
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
|
||||
try
|
||||
{
|
||||
EndPoint remoteEndPoint = new IPEndPoint(IPAddress.Any, 0);
|
||||
|
||||
while (_isRunning)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_socket == null || _receiveBuffer == null)
|
||||
break;
|
||||
|
||||
// Blocking receive into pre-allocated buffer (zero allocation!)
|
||||
int receivedBytes = _socket.ReceiveFrom(_receiveBuffer, ref remoteEndPoint);
|
||||
|
||||
if (receivedBytes <= 0)
|
||||
continue;
|
||||
|
||||
// Process ALL packets in received buffer
|
||||
int offset = 0;
|
||||
while (offset + LidarDataPacket.PACKET_SIZE <= receivedBytes)
|
||||
{
|
||||
// Create span directly from pre-allocated buffer (zero-copy)
|
||||
ReadOnlySpan<byte> packetData = _receiveBuffer.AsSpan(offset, LidarDataPacket.PACKET_SIZE);
|
||||
|
||||
// Quick validation before processing
|
||||
if (LidarPacketParser.IsValidPacket(packetData))
|
||||
{
|
||||
Interlocked.Increment(ref _totalPacketsReceived);
|
||||
ProcessPacketSpan(packetData);
|
||||
}
|
||||
|
||||
offset += LidarDataPacket.PACKET_SIZE;
|
||||
}
|
||||
|
||||
// Note: Leftover bytes warning removed from hot path for performance
|
||||
}
|
||||
catch (SocketException ex) when (ex.SocketErrorCode == SocketError.Interrupted)
|
||||
{
|
||||
// Socket was closed, exit loop
|
||||
break;
|
||||
}
|
||||
catch (ObjectDisposedException)
|
||||
{
|
||||
// Socket was disposed, exit loop
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
OnError(new LidarErrorEventArgs($"Error receiving packet: {ex.Message}", ex));
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Process received packet from ReadOnlySpan (zero-copy)
|
||||
/// </summary>
|
||||
private void ProcessPacketSpan(ReadOnlySpan<byte> data)
|
||||
{
|
||||
LidarDataPacket? packet = null;
|
||||
|
||||
try
|
||||
{
|
||||
// Get packet from pool or create new one
|
||||
if (_packetPool.TryTake(out packet))
|
||||
{
|
||||
Interlocked.Decrement(ref _packetPoolCount);
|
||||
}
|
||||
else
|
||||
{
|
||||
packet = new LidarDataPacket();
|
||||
Interlocked.Increment(ref _totalPacketsCreated);
|
||||
}
|
||||
|
||||
// Parse packet (already validated)
|
||||
if (LidarPacketParser.TryParse(data, packet))
|
||||
{
|
||||
Interlocked.Increment(ref _totalPacketsParsed);
|
||||
|
||||
// Update scan frequency (optimized - only check timestamp, no lock)
|
||||
UpdateScanFrequencyOptimized();
|
||||
|
||||
// Raise event and automatically return packet to pool after processing
|
||||
OnDataReceived(packet);
|
||||
}
|
||||
else
|
||||
{
|
||||
Interlocked.Increment(ref _totalParseErrors);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Interlocked.Increment(ref _totalParseErrors);
|
||||
OnError(new LidarErrorEventArgs($"Error processing packet: {ex.Message}", ex));
|
||||
}
|
||||
finally
|
||||
{
|
||||
// Always return packet to pool, even if event handler throws exception
|
||||
if (packet != null)
|
||||
{
|
||||
ReturnPacketToPool(packet);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update scan frequency based on packets received per second
|
||||
/// Optimized version: only takes lock when actually updating (once per second)
|
||||
/// </summary>
|
||||
private void UpdateScanFrequencyOptimized()
|
||||
{
|
||||
// Increment packet count (lock-free)
|
||||
Interlocked.Increment(ref _packetsReceivedInCurrentSecond);
|
||||
|
||||
// Check if 1 second has elapsed (lock-free check)
|
||||
long elapsedMs = _frequencyStopwatch.ElapsedMilliseconds;
|
||||
if (elapsedMs - Interlocked.Read(ref _lastFrequencyUpdateMs) >= 1000)
|
||||
{
|
||||
// Try to update (only one thread will succeed)
|
||||
long previousUpdateMs = Interlocked.CompareExchange(ref _lastFrequencyUpdateMs, elapsedMs, Interlocked.Read(ref _lastFrequencyUpdateMs));
|
||||
|
||||
// Check if we won the race to update
|
||||
if (elapsedMs - previousUpdateMs >= 1000)
|
||||
{
|
||||
// Calculate frequency
|
||||
long packetCount = Interlocked.Exchange(ref _packetsReceivedInCurrentSecond, 0);
|
||||
double elapsedSeconds = (elapsedMs - previousUpdateMs) / 1000.0;
|
||||
ScanFrequencyHz = packetCount / elapsedSeconds;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Return packet to pool for reuse
|
||||
/// This is called automatically after event handlers complete
|
||||
/// Uses atomic operations to prevent race conditions and ensure accurate pool size
|
||||
/// </summary>
|
||||
private void ReturnPacketToPool(LidarDataPacket packet)
|
||||
{
|
||||
if (packet == null)
|
||||
return;
|
||||
|
||||
// Atomically try to increment the pool count
|
||||
long newCount = Interlocked.Increment(ref _packetPoolCount);
|
||||
|
||||
if (newCount <= MAX_PACKET_POOL_SIZE)
|
||||
{
|
||||
// Successfully reserved a slot in the pool
|
||||
_packetPool.Add(packet);
|
||||
}
|
||||
else
|
||||
{
|
||||
// Pool is full - decrement counter back and let packet be GC'd
|
||||
Interlocked.Decrement(ref _packetPoolCount);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Raise DataReceived event
|
||||
/// Packet is automatically returned to pool after all handlers complete
|
||||
/// </summary>
|
||||
private void OnDataReceived(LidarDataPacket e)
|
||||
{
|
||||
DataReceived?.Invoke(this, e);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Raise ErrorOccurred event
|
||||
/// </summary>
|
||||
private void OnError(LidarErrorEventArgs e)
|
||||
{
|
||||
ErrorOccurred?.Invoke(this, e);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Reset statistics counters
|
||||
/// Note: Pool size and created count are preserved (shows cumulative allocations)
|
||||
/// </summary>
|
||||
public void ResetStatistics()
|
||||
{
|
||||
Interlocked.Exchange(ref _totalPacketsReceived, 0);
|
||||
Interlocked.Exchange(ref _totalPacketsParsed, 0);
|
||||
Interlocked.Exchange(ref _totalParseErrors, 0);
|
||||
|
||||
// Reset frequency calculation
|
||||
Interlocked.Exchange(ref _packetsReceivedInCurrentSecond, 0);
|
||||
Interlocked.Exchange(ref _lastFrequencyUpdateMs, 0);
|
||||
_frequencyStopwatch.Restart();
|
||||
ScanFrequencyHz = null;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get statistics summary with pool metrics
|
||||
/// </summary>
|
||||
public string GetStatistics()
|
||||
{
|
||||
long received = TotalPacketsReceived;
|
||||
long parsed = TotalPacketsParsed;
|
||||
long errors = TotalParseErrors;
|
||||
long created = TotalPacketsCreated;
|
||||
long poolSize = CurrentPoolSize;
|
||||
double successRate = received > 0 ? (parsed * 100.0 / received) : 0;
|
||||
string frequency = ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A";
|
||||
|
||||
return $"Received: {received}, Parsed: {parsed}, Errors: {errors}, " +
|
||||
$"Created: {created}, Pool: {poolSize}/{MAX_PACKET_POOL_SIZE}, " +
|
||||
$"Success: {successRate:F2}%, Frequency: {frequency}";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Dispose resources
|
||||
/// </summary>
|
||||
public void Dispose()
|
||||
{
|
||||
Stop();
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Event args for LiDAR error events
|
||||
/// </summary>
|
||||
public class LidarErrorEventArgs(string message, Exception? exception = null) : EventArgs
|
||||
{
|
||||
public string Message { get; } = message;
|
||||
public Exception? Exception { get; } = exception;
|
||||
}
|
||||
Reference in New Issue
Block a user