Initial commit
This commit is contained in:
@@ -0,0 +1,390 @@
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Lidar;
|
||||
|
||||
/// <summary>
|
||||
/// Olei 2D LiDAR driver (LR-1BS5 / olelidar ROS protocol, 1240-byte UDP packets).
|
||||
/// </summary>
|
||||
[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
|
||||
public class OleiLidarDriver : DeviceBase, ILidar
|
||||
{
|
||||
private readonly Connect _connect = new();
|
||||
private readonly DecodeLidar _decode = new();
|
||||
|
||||
private readonly double _minRangeM;
|
||||
private readonly double _maxRangeM;
|
||||
private readonly double? _angularResolutionRad;
|
||||
private readonly double? _scanFrequencyHz;
|
||||
private readonly bool _supportsIntensity;
|
||||
private readonly double? _accuracyM;
|
||||
private readonly double _startAngleRad;
|
||||
private readonly double _endAngleRad;
|
||||
private readonly string _frameId;
|
||||
private readonly int _angleMinHundredths;
|
||||
private readonly int _angleMaxHundredths;
|
||||
private readonly int _poly;
|
||||
|
||||
private DateTime? _lastScanDataTimestamp;
|
||||
private LaserScan? _lastLaserScan;
|
||||
private bool _portOpened;
|
||||
|
||||
private readonly Lock _dataLock = new();
|
||||
private CancellationTokenSource? _updateCts;
|
||||
private Task? _updateTask;
|
||||
private uint _scanSequence;
|
||||
|
||||
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
||||
|
||||
public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
|
||||
: base(deviceId, deviceName, DeviceType.Lidar)
|
||||
{
|
||||
_connect.device_IP = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
|
||||
_connect.device_port = connection.GetValue<int?>("DevicePort")
|
||||
?? connection.GetValue<int?>("UdpPort")
|
||||
?? 2368;
|
||||
_connect.local_ip = connection.GetValue<string>("LocalIp") ?? string.Empty;
|
||||
_connect.multiaddr_ip = connection.GetValue<string>("Multiaddr") ?? string.Empty;
|
||||
|
||||
var minAngleDeg = connection.GetValue<double?>("MinAngle") ?? -135.0;
|
||||
var maxAngleDeg = connection.GetValue<double?>("MaxAngle") ?? 135.0;
|
||||
_startAngleRad = minAngleDeg * Math.PI / 180.0;
|
||||
_endAngleRad = maxAngleDeg * Math.PI / 180.0;
|
||||
|
||||
_angleMinHundredths = (int)(minAngleDeg * 100 + 18000);
|
||||
_angleMaxHundredths = (int)(maxAngleDeg * 100 + 18000);
|
||||
|
||||
_minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.2;
|
||||
_maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
|
||||
_scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz");
|
||||
_supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
|
||||
_accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02;
|
||||
_frameId = connection.GetValue<string>("FrameId") ?? "olei_lidar_frame";
|
||||
_poly = connection.GetValue<int?>("Poly") ?? 1;
|
||||
|
||||
var stepDeg = connection.GetValue<double?>("StepDeg") ?? 0.225;
|
||||
_angularResolutionRad = stepDeg * Math.PI / 180.0;
|
||||
|
||||
var decoderConfig = new DecoderConfig
|
||||
{
|
||||
AngleMin = minAngleDeg,
|
||||
AngleMax = maxAngleDeg,
|
||||
RangeMin = _minRangeM,
|
||||
RangeMax = _maxRangeM,
|
||||
Poly = _poly,
|
||||
Inverted = connection.GetValue<bool?>("Inverted") ?? false,
|
||||
StepDeg = stepDeg,
|
||||
FrameId = _frameId
|
||||
};
|
||||
_decode.SetConfig(decoderConfig);
|
||||
|
||||
if (connection.GetValue<bool?>("AutoReconnectEnabled") is bool autoReconnect)
|
||||
AutoReconnectEnabled = autoReconnect;
|
||||
else
|
||||
AutoReconnectEnabled = true;
|
||||
|
||||
if (connection.GetValue<int?>("ReconnectDelayMs") is int reconnectDelay)
|
||||
ReconnectDelayMs = reconnectDelay;
|
||||
|
||||
if (connection.GetValue<int?>("MaxReconnectAttempts") is int maxAttempts)
|
||||
MaxReconnectAttempts = maxAttempts;
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
yield return new PropertyDescription("NumberOfPoints", "Number of Points", "Số điểm scan")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 1,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "0"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("StartAngle", "Start Angle (deg)", "Góc bắt đầu (độ)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 2,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = (_startAngleRad * 180.0 / Math.PI).ToString("F1")
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("EndAngle", "End Angle (deg)", "Góc kết thúc (độ)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 3,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = (_endAngleRad * 180.0 / Math.PI).ToString("F1")
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("MaxRange", "Max Range (m)", "Tầm quét tối đa (m)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 4,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = _maxRangeM.ToString("F1")
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan cuối cùng")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 5,
|
||||
Category = "Trạng thái",
|
||||
DefaultValue = ""
|
||||
};
|
||||
}
|
||||
|
||||
protected override Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_portOpened = _connect.openPort();
|
||||
if (!_portOpened)
|
||||
throw new InvalidOperationException(
|
||||
$"Failed to open Olei UDP port {_connect.device_port} on local {_connect.local_ip}");
|
||||
}
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StartUpdateLoop();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopUpdateLoop();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_lastLaserScan = null;
|
||||
_lastScanDataTimestamp = null;
|
||||
_scanSequence = 0;
|
||||
}
|
||||
UpdateProperties();
|
||||
return Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
if (!_portOpened || !_connect.IsOpen)
|
||||
return false;
|
||||
|
||||
// LiDAR may need a few seconds after motor start before UDP data arrives.
|
||||
for (var i = 0; i < 40; i++)
|
||||
{
|
||||
if (_lastScanDataTimestamp.HasValue)
|
||||
return true;
|
||||
|
||||
if (_connect.TotalPacketsReceived > 0 || _connect.GetPacketCount() > 0)
|
||||
return true;
|
||||
|
||||
await Task.Delay(250, cancellationToken);
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"[WARN] Olei LiDAR connection check failed: packets={_connect.TotalPacketsReceived}, " +
|
||||
$"queued={_connect.GetPacketCount()}, wrongSrc={_connect.DroppedWrongSource}, " +
|
||||
$"wrongSize={_connect.DroppedWrongSize}, device={_connect.device_IP}:{_connect.device_port}");
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
private void StartUpdateLoop()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_updateCts != null)
|
||||
return;
|
||||
|
||||
_updateCts = new CancellationTokenSource();
|
||||
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
|
||||
}
|
||||
}
|
||||
|
||||
private void StopUpdateLoop()
|
||||
{
|
||||
CancellationTokenSource? cts;
|
||||
lock (_dataLock)
|
||||
{
|
||||
cts = _updateCts;
|
||||
_updateCts = null;
|
||||
_updateTask = null;
|
||||
}
|
||||
cts?.Cancel();
|
||||
cts?.Dispose();
|
||||
}
|
||||
|
||||
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
var framePublished = false;
|
||||
while (_connect.TryDequeuePacket() is { } pkt)
|
||||
{
|
||||
if (_decode.PacketCb(pkt))
|
||||
framePublished |= TryPublishScan(pkt.stamp);
|
||||
}
|
||||
|
||||
if (!framePublished)
|
||||
await Task.Delay(1, cancellationToken);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "Olei LiDAR update loop error");
|
||||
await Task.Delay(500, cancellationToken);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryPublishScan(DateTime packetReceiveStamp)
|
||||
{
|
||||
var angles = _decode.scanAngleInVec;
|
||||
var rangesMm = _decode.scanRangeInVec;
|
||||
var intensitiesRaw = _decode.scanIntensityInVec;
|
||||
|
||||
if (angles.Count == 0 ||
|
||||
angles.Count != rangesMm.Count ||
|
||||
angles.Count != intensitiesRaw.Count)
|
||||
return false;
|
||||
|
||||
var rangeList = new List<double>();
|
||||
var intensityList = new List<double>();
|
||||
var angleRadList = new List<double>();
|
||||
|
||||
for (var i = 0; i < angles.Count; i++)
|
||||
{
|
||||
if (i % _poly != 0)
|
||||
continue;
|
||||
|
||||
var angleHundredths = angles[i];
|
||||
if (angleHundredths < _angleMinHundredths || angleHundredths > _angleMaxHundredths)
|
||||
continue;
|
||||
|
||||
var angleDeg = (angleHundredths - 18000) / 100.0;
|
||||
var angleRad = angleDeg * Math.PI / 180.0;
|
||||
angleRadList.Add(angleRad);
|
||||
|
||||
var distanceM = rangesMm[i] * OleiConstants.DistanceResolution;
|
||||
var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
|
||||
rangeList.Add(isValid ? distanceM : double.NaN);
|
||||
intensityList.Add(intensitiesRaw[i]);
|
||||
}
|
||||
|
||||
if (rangeList.Count == 0)
|
||||
return false;
|
||||
|
||||
var frequency = _decode.Frequency > 0.001f
|
||||
? _decode.Frequency
|
||||
: (float)(_scanFrequencyHz ?? 10.0);
|
||||
var scanTime = 1.0 / frequency;
|
||||
var numberOfPoints = rangeList.Count;
|
||||
|
||||
var angleMin = angleRadList[0];
|
||||
var angleMax = angleRadList[^1];
|
||||
var angleIncrement = numberOfPoints > 1
|
||||
? (angleMax - angleMin) / (numberOfPoints - 1)
|
||||
: _decode.StepDeg * Math.PI / 180.0;
|
||||
|
||||
var receiveUtc = packetReceiveStamp.Kind == DateTimeKind.Utc
|
||||
? packetReceiveStamp
|
||||
: packetReceiveStamp.ToUniversalTime();
|
||||
var lidarEndUtc = _decode.LastCompletedScanLidarEndUtc ?? receiveUtc;
|
||||
var scanStamp = lidarEndUtc - TimeSpan.FromSeconds(scanTime);
|
||||
|
||||
var laserScan = new LaserScan
|
||||
{
|
||||
Header = new RobotNet10.Shared.Header
|
||||
{
|
||||
Seq = _scanSequence++,
|
||||
Stamp = scanStamp,
|
||||
FrameId = _frameId
|
||||
},
|
||||
AngleMin = angleMin,
|
||||
AngleMax = angleMax,
|
||||
AngleIncrement = angleIncrement,
|
||||
TimeIncrement = scanTime / numberOfPoints,
|
||||
ScanTime = scanTime,
|
||||
RangeMin = (float)_minRangeM,
|
||||
RangeMax = (float)_maxRangeM,
|
||||
Ranges = rangeList.ToArray(),
|
||||
Intensities = intensityList.ToArray()
|
||||
};
|
||||
|
||||
lock (_dataLock)
|
||||
{
|
||||
_lastLaserScan = laserScan;
|
||||
_lastScanDataTimestamp = scanStamp;
|
||||
}
|
||||
|
||||
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
|
||||
UpdateProperties();
|
||||
return true;
|
||||
}
|
||||
|
||||
private void UpdateProperties()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
var pointCount = _lastLaserScan?.Ranges.Length ?? 0;
|
||||
SetProperty("NumberOfPoints", pointCount.ToString());
|
||||
SetProperty("StartAngle", (_startAngleRad * 180.0 / Math.PI).ToString("F1"));
|
||||
SetProperty("EndAngle", (_endAngleRad * 180.0 / Math.PI).ToString("F1"));
|
||||
SetProperty("MaxRange", _maxRangeM.ToString("F1"));
|
||||
SetProperty("LastScanTime", _lastScanDataTimestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
|
||||
}
|
||||
}
|
||||
|
||||
#region ILidar
|
||||
|
||||
public LaserScan? CurrentMeasurementData
|
||||
{
|
||||
get { lock (_dataLock) { return _lastLaserScan; } }
|
||||
}
|
||||
|
||||
public DateTime? LastScanDataTimestamp
|
||||
{
|
||||
get { lock (_dataLock) { return _lastScanDataTimestamp; } }
|
||||
}
|
||||
|
||||
public double MinAngleRad => _startAngleRad;
|
||||
public double MaxAngleRad => _endAngleRad;
|
||||
public double MinRangeM => _minRangeM;
|
||||
public double MaxRangeM => _maxRangeM;
|
||||
public double? AngularResolutionRad => _angularResolutionRad;
|
||||
public double? ScanFrequencyHz => _decode.Frequency > 0.001f ? _decode.Frequency : _scanFrequencyHz;
|
||||
public double FieldOfViewRad => Math.Abs(_endAngleRad - _startAngleRad);
|
||||
public bool SupportsIntensity => _supportsIntensity;
|
||||
public double? AccuracyM => _accuracyM;
|
||||
|
||||
#endregion
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
StopUpdateLoop();
|
||||
_connect.Dispose();
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user