563 lines
20 KiB
C#
563 lines
20 KiB
C#
using System.Diagnostics;
|
|
using Olei.LidarSensor;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.Shared;
|
|
using RobotNet10.Shared.Sensor;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.Olei;
|
|
|
|
/// <summary>
|
|
/// Configuration for Olei 2D LiDAR Driver
|
|
/// </summary>
|
|
public class Olei2dLidarDriverConfig
|
|
{
|
|
/// <summary>
|
|
/// UDP port to listen for LiDAR data
|
|
/// Default: 2368 (typical LiDAR port)
|
|
/// </summary>
|
|
public int UdpPort { get; set; } = 2368;
|
|
|
|
/// <summary>
|
|
/// Frame ID for the LiDAR sensor
|
|
/// Used in ROS-style message headers
|
|
/// </summary>
|
|
public string FrameId { get; set; } = "laser";
|
|
|
|
/// <summary>
|
|
/// Whether the LiDAR is mounted inverted (upside down).
|
|
/// When true, scan angles are mirrored (180° - angle) to correct orientation.
|
|
/// </summary>
|
|
public bool Inverted { get; set; } = false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Driver for Olei 2D LiDAR Sensor (LR-1F / LR-1BS)
|
|
/// Implements DeviceBase and ILidar interface
|
|
/// Protocol: UDP/IP v2.1
|
|
/// </summary>
|
|
[Device(DeviceType.Lidar, "Olei", "Olei2dLidarDriver", "1.0.0",
|
|
Description = "2D LiDAR Sensor Communication Data Protocol v2.1")]
|
|
public class Olei2dLidarDriver : DeviceBase, ILidar
|
|
{
|
|
private readonly Olei2dLidarDriverConfig _config = new();
|
|
private OleiLidarServer? _lidarServer;
|
|
|
|
// Cached measurements
|
|
private LaserScan? _currentMeasurementData;
|
|
private DateTime? _lastScanDataTimestamp;
|
|
|
|
// Frequency update tracking (for UI properties)
|
|
private readonly Stopwatch _frequencyUpdateStopwatch = Stopwatch.StartNew();
|
|
|
|
// Scan frequency calculation (actual LaserScan generation rate)
|
|
private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
|
|
private long _scansGeneratedInCurrentSecond = 0;
|
|
private readonly Lock _scanFrequencyLock = new();
|
|
|
|
// Calculated LiDAR specifications from actual data
|
|
private const double _minAngleRad = 0.0;
|
|
private const double _maxAngleRad = 2.0 * Math.PI;
|
|
private double _angularResolutionRad = 0.0; // Calculated from actual packet data
|
|
private const double DEFAULT_ACCURACY_M = 0.02; // 2 cm accuracy (fixed by hardware)
|
|
|
|
// Hardware range specifications (no filtering applied)
|
|
private const double HARDWARE_MIN_RANGE_M = 0.0;
|
|
private const double HARDWARE_MAX_RANGE_M = 30.0; // 30m max range for Olei LiDAR
|
|
|
|
// Packet accumulation for full scan (0° ~ 360°)
|
|
private readonly List<LidarDataBlock> _accumulatedBlocks = [];
|
|
private double _lastPacketStartAngle = -1.0;
|
|
private readonly Lock _accumulationLock = new();
|
|
private uint _scanSequenceNumber = 0;
|
|
private DateTime _currentScanStartTime = DateTime.UtcNow;
|
|
|
|
/// <summary>
|
|
/// Constructor with configuration
|
|
/// </summary>
|
|
public Olei2dLidarDriver(
|
|
string deviceId,
|
|
string deviceName, IConfigurationSection configuration)
|
|
: base(deviceId, deviceName, DeviceType.Lidar)
|
|
{
|
|
configuration.Bind(_config);
|
|
Description = "Olei 2D LiDAR Sensor (LR-1F/LR-1BS) - UDP Protocol v2.1";
|
|
}
|
|
|
|
#region ILidar Implementation
|
|
|
|
/// <summary>
|
|
/// Current measurement data (scan points)
|
|
/// </summary>
|
|
public LaserScan? CurrentMeasurementData => _currentMeasurementData;
|
|
|
|
/// <summary>
|
|
/// Timestamp of the most recent scan data
|
|
/// </summary>
|
|
public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;
|
|
|
|
/// <summary>
|
|
/// Minimum scan angle (radians)
|
|
/// Calculated from actual LiDAR data
|
|
/// </summary>
|
|
public double MinAngleRad => _minAngleRad;
|
|
|
|
/// <summary>
|
|
/// Maximum scan angle (radians)
|
|
/// Calculated from actual LiDAR data
|
|
/// </summary>
|
|
public double MaxAngleRad => _maxAngleRad;
|
|
|
|
/// <summary>
|
|
/// Minimum measurement range (meters)
|
|
/// Hardware specification (no filtering)
|
|
/// </summary>
|
|
public double MinRangeM => HARDWARE_MIN_RANGE_M;
|
|
|
|
/// <summary>
|
|
/// Maximum measurement range (meters)
|
|
/// Hardware specification (no filtering)
|
|
/// </summary>
|
|
public double MaxRangeM => HARDWARE_MAX_RANGE_M;
|
|
|
|
/// <summary>
|
|
/// Angular resolution (radians)
|
|
/// Calculated from actual LiDAR data
|
|
/// </summary>
|
|
public double? AngularResolutionRad => _angularResolutionRad;
|
|
|
|
/// <summary>
|
|
/// Scan frequency (Hz)
|
|
/// Typically 10-20 Hz for Olei LiDAR
|
|
/// </summary>
|
|
public double? ScanFrequencyHz { get; private set; }
|
|
|
|
/// <summary>
|
|
/// Field of View (radians)
|
|
/// </summary>
|
|
public double FieldOfViewRad => MaxAngleRad - MinAngleRad;
|
|
|
|
/// <summary>
|
|
/// Supports intensity measurements
|
|
/// </summary>
|
|
public bool SupportsIntensity => true;
|
|
|
|
/// <summary>
|
|
/// Measurement accuracy (meters)
|
|
/// </summary>
|
|
public double? AccuracyM => DEFAULT_ACCURACY_M;
|
|
|
|
/// <summary>
|
|
/// Event raised when new scan data is received
|
|
/// </summary>
|
|
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
|
|
|
#endregion
|
|
|
|
#region DeviceBase Implementation
|
|
|
|
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
await Task.Run(() =>
|
|
{
|
|
// Initialize LiDAR server
|
|
_lidarServer = new OleiLidarServer(_config.UdpPort);
|
|
|
|
// Subscribe to events
|
|
_lidarServer.DataReceived += OnLidarDataReceived;
|
|
_lidarServer.ErrorOccurred += OnLidarErrorOccurred;
|
|
|
|
SetProperty("UdpPort", _config.UdpPort.ToString());
|
|
SetProperty("FrameId", _config.FrameId);
|
|
SetProperty("MinRange", $"{HARDWARE_MIN_RANGE_M:F2} m");
|
|
SetProperty("MaxRange", $"{HARDWARE_MAX_RANGE_M:F2} m");
|
|
SetProperty("FOV", "N/A"); // Will be calculated from actual data
|
|
SetProperty("AngularResolution", "N/A"); // Will be calculated from actual data
|
|
|
|
}, cancellationToken);
|
|
}
|
|
|
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
if (_lidarServer == null)
|
|
throw new InvalidOperationException("LiDAR server not initialized. Call InitializeAsync first.");
|
|
|
|
// Start LiDAR server
|
|
_lidarServer.Start();
|
|
|
|
SetProperty("ServerStatus", "Running");
|
|
SetProperty("Statistics", _lidarServer.GetStatistics());
|
|
}
|
|
|
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
_lidarServer?.Stop();
|
|
|
|
SetProperty("ServerStatus", "Stopped");
|
|
SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
|
|
}
|
|
|
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Reset statistics
|
|
_lidarServer?.ResetStatistics();
|
|
|
|
// Reset scan frequency tracking
|
|
lock (_scanFrequencyLock)
|
|
{
|
|
Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
|
|
_scanFrequencyStopwatch.Restart();
|
|
ScanFrequencyHz = null;
|
|
}
|
|
|
|
SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
|
|
SetProperty("ScanFrequency", "N/A");
|
|
}
|
|
|
|
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
|
{
|
|
await Task.Delay(500, cancellationToken);
|
|
if (_lidarServer == null || !_lidarServer.IsRunning)
|
|
return false;
|
|
|
|
lock (_accumulationLock)
|
|
{
|
|
return _lastScanDataTimestamp.HasValue;
|
|
}
|
|
}
|
|
|
|
protected override List<PropertyDescription> CreatePropertyDescriptions()
|
|
{
|
|
return
|
|
[
|
|
new PropertyDescription("UdpPort", "UDP Port", "UDP port for receiving LiDAR data"),
|
|
new PropertyDescription("FrameId", "Frame ID", "ROS-style frame identifier"),
|
|
new PropertyDescription("MinRange", "Min Range", "Minimum measurement range"),
|
|
new PropertyDescription("MaxRange", "Max Range", "Maximum measurement range"),
|
|
new PropertyDescription("FOV", "Field of View", "Total scanning angle"),
|
|
new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
|
|
new PropertyDescription("ServerStatus", "Server Status", "UDP server status"),
|
|
new PropertyDescription("Statistics", "Statistics", "Server statistics"),
|
|
new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
|
|
new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
|
|
];
|
|
}
|
|
|
|
protected override void Dispose(bool disposing)
|
|
{
|
|
if (disposing && _lidarServer != null)
|
|
{
|
|
_lidarServer.DataReceived -= OnLidarDataReceived;
|
|
_lidarServer.ErrorOccurred -= OnLidarErrorOccurred;
|
|
_lidarServer.Dispose();
|
|
_lidarServer = null;
|
|
}
|
|
|
|
base.Dispose(disposing);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Event Handlers
|
|
|
|
/// <summary>
|
|
/// Handle data received from LiDAR server
|
|
/// Accumulates packets until full 360° scan is complete
|
|
/// </summary>
|
|
private void OnLidarDataReceived(object? sender, LidarDataPacket e)
|
|
{
|
|
try
|
|
{
|
|
var packet = e;
|
|
|
|
// Validate packet
|
|
if (!packet.Header.IsValidFrame)
|
|
return;
|
|
|
|
// Get first valid block's angle to detect scan wrap
|
|
double packetStartAngle = -1.0;
|
|
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
|
|
{
|
|
if (packet.DataBlocks[i].IsValid)
|
|
{
|
|
packetStartAngle = packet.DataBlocks[i].GetAngleDegrees();
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (packetStartAngle < 0)
|
|
return; // No valid blocks in packet
|
|
|
|
LaserScan? completedScan = null;
|
|
bool updateProperties = false;
|
|
|
|
lock (_accumulationLock)
|
|
{
|
|
// Detect scan completion: angle wrapped back to near 0° after being > 300°
|
|
if (_lastPacketStartAngle > 300.0 && packetStartAngle < 50.0 && _accumulatedBlocks.Count > 0)
|
|
{
|
|
var scan = CreateLaserScanFromAccumulated(packet.Header, _currentScanStartTime);
|
|
completedScan = scan;
|
|
|
|
_currentMeasurementData = scan;
|
|
_lastScanDataTimestamp = scan.Header.Stamp;
|
|
|
|
Interlocked.Increment(ref _scansGeneratedInCurrentSecond);
|
|
UpdateScanFrequency();
|
|
|
|
if (_frequencyUpdateStopwatch.ElapsedMilliseconds > 1000)
|
|
{
|
|
updateProperties = true;
|
|
_frequencyUpdateStopwatch.Restart();
|
|
}
|
|
|
|
_accumulatedBlocks.Clear();
|
|
_currentScanStartTime = DateTime.UtcNow;
|
|
}
|
|
else if (_accumulatedBlocks.Count == 0)
|
|
{
|
|
_currentScanStartTime = DateTime.UtcNow;
|
|
}
|
|
|
|
// Add current packet's blocks to accumulation buffer
|
|
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
|
|
{
|
|
var block = packet.DataBlocks[i];
|
|
if (block.IsValid)
|
|
{
|
|
double angleDeg = block.GetAngleDegrees();
|
|
if (angleDeg >= 360.0)
|
|
angleDeg %= 360.0;
|
|
_accumulatedBlocks.Add(block);
|
|
}
|
|
}
|
|
|
|
_lastPacketStartAngle = packetStartAngle;
|
|
}
|
|
|
|
// Fire event and update properties outside the lock to avoid blocking UDP reception
|
|
if (completedScan is { } publishedScan)
|
|
{
|
|
if (updateProperties)
|
|
{
|
|
SetProperty("LastScanTime", publishedScan.Header.Stamp.ToString("HH:mm:ss.fff"));
|
|
SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
|
|
SetProperty("FOV", $"{FieldOfViewRad * 180 / Math.PI:F1}°");
|
|
SetProperty("AngularResolution", $"{_angularResolutionRad * 180 / Math.PI:F3}°");
|
|
}
|
|
|
|
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(
|
|
publishedScan.Header.Stamp,
|
|
publishedScan
|
|
));
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
OnErrorOccurred(new Exception($"Error processing LiDAR data: {ex.Message}", ex));
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Handle errors from LiDAR server
|
|
/// </summary>
|
|
private void OnLidarErrorOccurred(object? sender, LidarErrorEventArgs e)
|
|
{
|
|
OnErrorOccurred(e.Exception ?? new Exception(e.Message));
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Helper Methods
|
|
|
|
/// <summary>
|
|
/// Update scan frequency based on LaserScan generation rate
|
|
/// Calculates how many scans are generated per second
|
|
/// </summary>
|
|
private void UpdateScanFrequency()
|
|
{
|
|
// Check if 1 second has elapsed
|
|
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
|
|
{
|
|
lock (_scanFrequencyLock)
|
|
{
|
|
// Double-check inside lock to prevent race condition
|
|
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
|
|
{
|
|
// Calculate frequency (scans per second)
|
|
long scanCount = Interlocked.Read(ref _scansGeneratedInCurrentSecond);
|
|
double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
|
|
ScanFrequencyHz = scanCount / elapsedSeconds;
|
|
|
|
// Reset for next measurement period
|
|
Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
|
|
_scanFrequencyStopwatch.Restart();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Create LaserScan from accumulated blocks (full 360° scan)
|
|
/// </summary>
|
|
private LaserScan CreateLaserScanFromAccumulated(LidarHeader lastHeader, DateTime scanStartTime)
|
|
{
|
|
var header = new Header(
|
|
seq: _scanSequenceNumber++,
|
|
stamp: scanStartTime,
|
|
frameId: _config.FrameId
|
|
);
|
|
|
|
// Get distance scale from last header
|
|
byte distanceScale = lastHeader.DistanceScale;
|
|
double distanceScaleToMeters = distanceScale / 1000.0;
|
|
|
|
// Calculate actual angle and range from accumulated data
|
|
double minAngleDeg = double.MaxValue;
|
|
double maxAngleDeg = double.MinValue;
|
|
double minDistanceM = double.MaxValue;
|
|
double maxDistanceM = double.MinValue;
|
|
|
|
// Create sorted list of angles to calculate angular resolution
|
|
var sortedAngles = new List<double>(_accumulatedBlocks.Count);
|
|
|
|
// First pass: Find actual min/max values from data and collect angles
|
|
foreach (var block in _accumulatedBlocks)
|
|
{
|
|
double angleDeg = block.GetAngleDegrees();
|
|
|
|
// Normalize angle to 0-360° range
|
|
if (angleDeg >= 360.0)
|
|
angleDeg %= 360.0;
|
|
|
|
minAngleDeg = Math.Min(minAngleDeg, angleDeg);
|
|
maxAngleDeg = Math.Max(maxAngleDeg, angleDeg);
|
|
sortedAngles.Add(angleDeg);
|
|
|
|
double distanceM = block.DistanceRaw * distanceScaleToMeters;
|
|
if (distanceM > 0) // Only consider valid distances
|
|
{
|
|
minDistanceM = Math.Min(minDistanceM, distanceM);
|
|
maxDistanceM = Math.Max(maxDistanceM, distanceM);
|
|
}
|
|
}
|
|
|
|
// Fallback to defaults if no valid data
|
|
if (minAngleDeg == double.MaxValue || maxAngleDeg == double.MinValue)
|
|
{
|
|
minAngleDeg = 0.0;
|
|
maxAngleDeg = 360.0;
|
|
}
|
|
|
|
if (minDistanceM == double.MaxValue || maxDistanceM == double.MinValue)
|
|
{
|
|
minDistanceM = HARDWARE_MIN_RANGE_M;
|
|
maxDistanceM = HARDWARE_MAX_RANGE_M;
|
|
}
|
|
|
|
// Calculate angular resolution from actual data
|
|
double angularResolutionDeg = 0.225; // Default fallback
|
|
if (sortedAngles.Count > 1)
|
|
{
|
|
sortedAngles.Sort();
|
|
|
|
// Find minimum difference between consecutive angles
|
|
double minAngleDiff = double.MaxValue;
|
|
for (int i = 1; i < sortedAngles.Count; i++)
|
|
{
|
|
double diff = sortedAngles[i] - sortedAngles[i - 1];
|
|
if (diff > 0.01) // Ignore very small differences (noise/duplicates)
|
|
{
|
|
minAngleDiff = Math.Min(minAngleDiff, diff);
|
|
}
|
|
}
|
|
|
|
if (minAngleDiff < double.MaxValue)
|
|
{
|
|
angularResolutionDeg = minAngleDiff;
|
|
}
|
|
}
|
|
|
|
// Update cached angular resolution for property reporting
|
|
_angularResolutionRad = angularResolutionDeg * Math.PI / 180.0;
|
|
|
|
// Convert angles to radians
|
|
double angleMinRad = minAngleDeg * Math.PI / 180.0;
|
|
double angleMaxRad = maxAngleDeg * Math.PI / 180.0;
|
|
|
|
// Calculate expected number of points based on calculated angular resolution
|
|
double angleSpanDeg = maxAngleDeg - minAngleDeg;
|
|
int expectedPoints = (int)Math.Ceiling(angleSpanDeg / angularResolutionDeg) + 1;
|
|
|
|
// Initialize arrays with expected size
|
|
double[] ranges = new double[expectedPoints];
|
|
double[] intensities = new double[expectedPoints];
|
|
|
|
// Initialize all to -1 (no data, JSON-safe)
|
|
Array.Fill(ranges, -1.0);
|
|
Array.Fill(intensities, 0.0);
|
|
|
|
// Second pass: Map accumulated blocks to array indices based on angle
|
|
int validCount = 0;
|
|
foreach (var block in _accumulatedBlocks)
|
|
{
|
|
double angleDeg = _config.Inverted
|
|
? (block.GetAngleDegrees() + 180.0) % 360.0
|
|
: block.GetAngleDegrees();
|
|
|
|
// Normalize angle to 0-360° range
|
|
if (angleDeg < 0)
|
|
angleDeg += 360.0;
|
|
else if (angleDeg >= 360.0)
|
|
angleDeg %= 360.0;
|
|
|
|
// Calculate array index from relative angle position using calculated angular resolution
|
|
double relativeAngle = angleDeg - minAngleDeg;
|
|
int index = (int)Math.Round(relativeAngle / angularResolutionDeg);
|
|
|
|
// Clamp index to valid range
|
|
if (index >= 0 && index < expectedPoints)
|
|
{
|
|
double distanceM = block.DistanceRaw * distanceScaleToMeters;
|
|
if (distanceM <= 0 || distanceM >= HARDWARE_MAX_RANGE_M)
|
|
{
|
|
ranges[index] = -1.0; // No detection / max-range return
|
|
intensities[index] = 0.0;
|
|
continue;
|
|
}
|
|
// Store all distance values (no range filtering)
|
|
ranges[index] = distanceM;
|
|
intensities[index] = block.SignalStrength;
|
|
validCount++;
|
|
}
|
|
}
|
|
|
|
// Calculate angle increment from actual data
|
|
double angleIncrementRad = expectedPoints > 1
|
|
? (angleMaxRad - angleMinRad) / (expectedPoints - 1)
|
|
: angularResolutionDeg * Math.PI / 180.0; // Use calculated resolution
|
|
|
|
// Calculate scan timing (estimate based on rotation rate if available)
|
|
double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
|
|
? 1.0 / ScanFrequencyHz.Value
|
|
: 0.1; // Default 10 Hz
|
|
double timeIncrement = scanTime / expectedPoints;
|
|
|
|
return new LaserScan
|
|
{
|
|
Header = header,
|
|
AngleMin = angleMinRad, // From actual data
|
|
AngleMax = angleMaxRad, // From actual data
|
|
AngleIncrement = angleIncrementRad, // Calculated from data
|
|
TimeIncrement = timeIncrement,
|
|
ScanTime = scanTime,
|
|
RangeMin = HARDWARE_MIN_RANGE_M,
|
|
RangeMax = HARDWARE_MAX_RANGE_M,
|
|
Ranges = ranges,
|
|
Intensities = intensities
|
|
};
|
|
}
|
|
|
|
#endregion
|
|
}
|