391 lines
13 KiB
C#
391 lines
13 KiB
C#
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);
|
|
}
|
|
}
|