Initial commit
This commit is contained in:
@@ -0,0 +1,374 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user