435 lines
14 KiB
C#
435 lines
14 KiB
C#
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;
|
|
}
|