375 lines
14 KiB
C#
375 lines
14 KiB
C#
using Microsoft.AspNetCore.Mvc.Diagnostics;
|
|
using RobotNet10.RobotApp.Client.Shared.Devices;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.Shared.Sensor;
|
|
using System;
|
|
using System.Net;
|
|
using System.Net.Sockets;
|
|
//using static RobotNet10.RobotApp.Drivers.Lidar.OleiLidarDecode;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.Lidar
|
|
{
|
|
[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
|
|
public class OleiLidarDriver : DeviceBase, ILidar
|
|
{
|
|
private readonly object _scanDataLock = new();
|
|
|
|
|
|
// Cached scan data
|
|
//private LidarMeasurementData? _cachedMeasurementData = null;
|
|
|
|
// Device specifications
|
|
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 int _numberOfPoints;
|
|
private readonly double _startAngleRad;
|
|
private readonly double _endAngleRad;
|
|
private DateTime? _lastScanDataTimestamp;
|
|
private LaserScan? _lastLaserScan;
|
|
private oleiPackage? _pkt;
|
|
Connect oleiConnect = new Connect();
|
|
DecodeLidar oleiDecode = new DecodeLidar();
|
|
|
|
DecoderConfig decoderConfig = new DecoderConfig();
|
|
private readonly Lock _dataLock = new();
|
|
private CancellationTokenSource? _updateCts;
|
|
private Task? _updateTask;
|
|
public float maxAngle;
|
|
public float minAngle;
|
|
public UdpClient udp;
|
|
public IPEndPoint check_ip_device;
|
|
bool portOpened;
|
|
|
|
//Event
|
|
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
|
|
|
//Cached data
|
|
//private LidarMeasurementData? _currentMeasurementData;
|
|
public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
|
|
: base(deviceId, deviceName, DeviceType.Lidar)
|
|
{
|
|
// Đọc cấu hình kết nối (IP và Port)
|
|
string deviceIp = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
|
|
int devicePort = connection.GetValue<int?>("DevicePort") ?? 2368;
|
|
string localIp = connection.GetValue<string>("LocalIp") ?? "192.168.254.10";
|
|
|
|
// Cấu hình Connect object với IP và Port từ config
|
|
oleiConnect.device_IP = deviceIp;
|
|
oleiConnect.device_port = devicePort;
|
|
oleiConnect.local_ip = localIp;
|
|
// Đọc cấu hình
|
|
minAngle = connection.GetValue<float?>("MinAngle") ?? -130.0f;
|
|
maxAngle = connection.GetValue<float?>("MaxAngle") ?? 130.0f;
|
|
_maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
|
|
_minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.1; // Default 20cm
|
|
_scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz") ?? 10.0; // Default 10Hz
|
|
_supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
|
|
_accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02; // Default 2cm accuracy
|
|
|
|
// Set decoder config from JSON
|
|
decoderConfig.Inverted = connection.GetValue<bool?>("Inverted") ?? false;
|
|
|
|
// Apply config to decoder
|
|
oleiDecode.SetConfig(decoderConfig);
|
|
|
|
var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
|
|
var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
|
|
var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
|
|
|
|
if (autoReconnectEnabled.HasValue)
|
|
AutoReconnectEnabled = autoReconnectEnabled.Value;
|
|
else
|
|
AutoReconnectEnabled = false; // Không cần reconnect cho simulation
|
|
|
|
if (reconnectDelayMs.HasValue)
|
|
ReconnectDelayMs = reconnectDelayMs.Value;
|
|
|
|
if (maxReconnectAttempts.HasValue)
|
|
MaxReconnectAttempts = maxReconnectAttempts.Value;
|
|
|
|
// Khởi tạo giá trị properties
|
|
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 = "2000"
|
|
};
|
|
|
|
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 = "-180"
|
|
};
|
|
|
|
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 = "180"
|
|
};
|
|
|
|
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 = "20"
|
|
};
|
|
|
|
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 async Task OnInitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Mở cổng UDP - kiểm tra kết quả và throw exception nếu fail
|
|
lock (_dataLock)
|
|
{
|
|
portOpened = oleiConnect.openPort();
|
|
}
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Bắt đầu vòng lặp cập nhật dữ liệu với tần số 1Hz
|
|
StartUpdateLoop();
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Dừng vòng lặp cập nhật
|
|
StopUpdateLoop();
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Reset về giá trị mặc định
|
|
lock (_dataLock)
|
|
{
|
|
_lastLaserScan = null;
|
|
_lastScanDataTimestamp = null;
|
|
_pkt = null;
|
|
}
|
|
UpdateProperties();
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
|
{
|
|
// Simulation luôn connected
|
|
if (!portOpened)
|
|
{
|
|
return await Task.FromResult(false);
|
|
}
|
|
return await Task.FromResult(true);
|
|
}
|
|
|
|
private void StartUpdateLoop()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
if (_updateTask != null && !_updateTask.IsCompleted)
|
|
return;
|
|
|
|
_updateCts = new CancellationTokenSource();
|
|
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
|
|
}
|
|
}
|
|
|
|
private void StopUpdateLoop()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
_updateCts?.Cancel();
|
|
_updateCts?.Dispose();
|
|
_updateCts = null;
|
|
_updateTask = null;
|
|
}
|
|
}
|
|
|
|
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
|
|
{
|
|
//_connect.openPort();
|
|
//Console.WriteLine("[INFO] START READ DATA");
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
// Đọc dữ liệu từ UDP
|
|
lock (_dataLock)
|
|
{
|
|
oleiConnect.readRawdata();
|
|
|
|
if (oleiConnect.GetPacketCount() > 0)
|
|
{
|
|
GenerateScanData();
|
|
}
|
|
}
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
LastError = ex;
|
|
OnErrorOccurred(ex, "Update loop error");
|
|
await Task.Delay(1000, cancellationToken);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void GenerateScanData()
|
|
{
|
|
// Dequeue packet an toàn (thread-safe)
|
|
oleiPackage? pkt = oleiConnect.TryDequeuePacket();
|
|
|
|
if (pkt == null)
|
|
{
|
|
// Không log để tránh spam khi queue rỗng
|
|
return;
|
|
}
|
|
|
|
var timestamp = DateTime.UtcNow;
|
|
|
|
// Decode packet
|
|
oleiDecode.PacketCb(pkt);
|
|
|
|
// Kiểm tra xem có dữ liệu không
|
|
if (oleiDecode.scanRadAngleInVec.Count == 0 ||
|
|
oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanRangeInVec.Count ||
|
|
oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanIntensityInVec.Count)
|
|
{
|
|
// Không có dữ liệu hoặc dữ liệu không đồng bộ
|
|
return;
|
|
}
|
|
|
|
var numberOfPoints = oleiDecode.scanRadAngleInVec.Count;
|
|
var ranges = new double[numberOfPoints];
|
|
var intensities = new double[numberOfPoints];
|
|
// Convert data to arrays (LaserScan format)
|
|
for(int i = 0; i < numberOfPoints; i++)
|
|
{
|
|
if((oleiDecode.scanAngleInVec[i] >= (minAngle + 180)) && (oleiDecode.scanAngleInVec[i] <= (maxAngle + 180)))
|
|
{
|
|
var distanceM = oleiDecode.scanRangeInVec[i] * 0.001f; // mm to meters
|
|
var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
|
|
ranges[i] = isValid ? distanceM : 0.0f;
|
|
intensities[i] = oleiDecode.scanIntensityInVec[i]; // normalize 0-1
|
|
}
|
|
}
|
|
float _minAngle = (float)(minAngle*Math.PI)/180;
|
|
float _maxAngle = (float)(maxAngle*Math.PI)/180;
|
|
float angleIncrement = numberOfPoints > 1 ?
|
|
(_maxAngle - _minAngle) / (numberOfPoints - 1) : 0.0f;
|
|
float _scanTime = 1.0f / oleiDecode.Frequency;
|
|
float _TimeIncrement = _scanTime / numberOfPoints;
|
|
|
|
var laserScan = new LaserScan
|
|
{
|
|
Header = new RobotNet10.Shared.Header
|
|
{
|
|
Seq = 0,
|
|
Stamp = timestamp,
|
|
FrameId = "olei_lidar_frame"
|
|
},
|
|
AngleMin = _minAngle,
|
|
AngleMax = _maxAngle,
|
|
AngleIncrement = angleIncrement,
|
|
TimeIncrement = _TimeIncrement,
|
|
ScanTime = _scanTime, // Convert Hz to seconds
|
|
RangeMin = (float)_minRangeM,
|
|
RangeMax = (float)_maxRangeM,
|
|
Ranges = ranges,
|
|
Intensities = intensities
|
|
};
|
|
|
|
// Cache LaserScan và packet
|
|
lock (_dataLock)
|
|
{
|
|
_lastLaserScan = laserScan;
|
|
_lastScanDataTimestamp = timestamp;
|
|
_pkt = pkt;
|
|
}
|
|
|
|
// Fire event
|
|
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(timestamp: timestamp, measurementData: laserScan));
|
|
|
|
UpdateProperties();
|
|
}
|
|
private void UpdateProperties()
|
|
{
|
|
lock (_dataLock)
|
|
{
|
|
SetProperty("NumberOfPoints", _numberOfPoints.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 Implementation
|
|
|
|
public LaserScan? CurrentMeasurementData
|
|
{
|
|
get { lock (_dataLock) { return _lastLaserScan; } }
|
|
}
|
|
|
|
public DateTime? LastScanDataTimestamp
|
|
{
|
|
get { lock (_dataLock) { return _lastScanDataTimestamp; } }
|
|
}
|
|
|
|
// ILidar Device Specifications
|
|
public double MinAngleRad => _startAngleRad;
|
|
public double MaxAngleRad => _endAngleRad;
|
|
public double MinRangeM => _minRangeM;
|
|
public double MaxRangeM => _maxRangeM;
|
|
public double? AngularResolutionRad => _angularResolutionRad;
|
|
public double? ScanFrequencyHz => _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();
|
|
}
|
|
base.Dispose(disposing);
|
|
}
|
|
}
|
|
|
|
|
|
}
|