Initial commit
This commit is contained in:
@@ -0,0 +1,919 @@
|
||||
using System.Buffers.Binary;
|
||||
using System.Linq;
|
||||
using System.Net;
|
||||
using System.Net.NetworkInformation;
|
||||
using System.Net.Sockets;
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Lidar;
|
||||
|
||||
/// <summary>
|
||||
/// UDP connection for Olei GS1-5 (V3 protocol). Packet size 1136 bytes, not 1240.
|
||||
/// </summary>
|
||||
internal sealed class OleiGS15UdpConnection
|
||||
{
|
||||
public const int ExpectedPacketSize = 1136;
|
||||
|
||||
private readonly Queue<Gs15Packet> _packetQueue = new();
|
||||
private readonly object _packetLock = new();
|
||||
|
||||
public string device_IP { get; set; } = "192.168.110.22";
|
||||
public int device_port { get; set; } = 2468;
|
||||
public string local_ip { get; set; } = "192.168.110.114";
|
||||
public UdpClient? udp;
|
||||
public IPEndPoint check_ip_device = new(IPAddress.Any, 0);
|
||||
|
||||
public bool openPort()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (!IpExists(local_ip))
|
||||
{
|
||||
Console.WriteLine($"[ERROR] OleiGS15: local_ip {local_ip} does NOT exist on any NIC.");
|
||||
return false;
|
||||
}
|
||||
if (IsPortBusy(device_port))
|
||||
{
|
||||
Console.WriteLine($"[ERROR] OleiGS15: Port {device_port} is already in use!");
|
||||
return false;
|
||||
}
|
||||
udp = new UdpClient(new IPEndPoint(IPAddress.Parse(local_ip), device_port));
|
||||
check_ip_device = new IPEndPoint(IPAddress.Parse(local_ip), device_port);
|
||||
udp.Client.ReceiveTimeout = 1000;
|
||||
Console.WriteLine("[INFO] OleiGS15 UDP port opened.");
|
||||
return true;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine($"[ERROR] OleiGS15: Cannot open UDP {local_ip}:{device_port} - {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
public void readRawdata()
|
||||
{
|
||||
if (udp == null) return;
|
||||
try
|
||||
{
|
||||
byte[] data = udp.Receive(ref check_ip_device);
|
||||
if (check_ip_device.Address == null || check_ip_device.Address.Equals(IPAddress.Any) || check_ip_device.Address.Equals(IPAddress.None) || check_ip_device.Port == 0)
|
||||
return;
|
||||
if (check_ip_device.Address.ToString() != device_IP)
|
||||
return;
|
||||
if (data.Length != ExpectedPacketSize)
|
||||
return;
|
||||
|
||||
var packet = new Gs15Packet { stamp = DateTime.UtcNow, data = data };
|
||||
lock (_packetLock)
|
||||
{
|
||||
_packetQueue.Enqueue(packet);
|
||||
}
|
||||
}
|
||||
catch (SocketException ex)
|
||||
{
|
||||
if (ex.SocketErrorCode != SocketError.TimedOut)
|
||||
Console.WriteLine($"[ERROR] OleiGS15 socket: {ex.Message}");
|
||||
}
|
||||
catch (Exception) { }
|
||||
}
|
||||
|
||||
public Gs15Packet? TryDequeuePacket()
|
||||
{
|
||||
lock (_packetLock)
|
||||
{
|
||||
if (_packetQueue.Count == 0) return null;
|
||||
return _packetQueue.Dequeue();
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IpExists(string ip)
|
||||
{
|
||||
foreach (var ni in NetworkInterface.GetAllNetworkInterfaces())
|
||||
{
|
||||
foreach (var ua in ni.GetIPProperties().UnicastAddresses)
|
||||
{
|
||||
if (ua.Address.ToString() == ip) return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsPortBusy(int port)
|
||||
{
|
||||
var listeners = IPGlobalProperties.GetIPGlobalProperties().GetActiveUdpListeners();
|
||||
return listeners.Any(ep => ep.Port == port);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class Gs15Packet
|
||||
{
|
||||
public DateTime stamp;
|
||||
public byte[] data = Array.Empty<byte>();
|
||||
}
|
||||
|
||||
public sealed class OleiGS15DriverConfig
|
||||
{
|
||||
public int Version { get; set; } = 3;
|
||||
public string ScannerIp { get; set; } = "192.168.110.22";
|
||||
public string LocalIp { get; set; } = "192.168.110.114";
|
||||
public int Port { get; set; } = 2468;
|
||||
public string Transport { get; set; } = "udp";
|
||||
public string FrameId { get; set; } = "olelidar";
|
||||
public string ScanTopic { get; set; } = "scan";
|
||||
public bool Inverted { get; set; } = false;
|
||||
public bool TimeFromLidar { get; set; } = true;
|
||||
public bool Ntp { get; set; } = false;
|
||||
public string PacketType { get; set; } = "B";
|
||||
public double RangeMin { get; set; } = 0.08;
|
||||
public double RangeMax { get; set; } = 50.0;
|
||||
public bool AutoReconnectEnabled { get; set; } = true;
|
||||
public int ReconnectDelayMs { get; set; } = 3000;
|
||||
public int MaxReconnectAttempts { get; set; } = 0;
|
||||
|
||||
public static OleiGS15DriverConfig Parse(IConfigurationSection connection)
|
||||
{
|
||||
var cfg = new OleiGS15DriverConfig
|
||||
{
|
||||
Version = connection.GetValue<int?>("Version")
|
||||
?? connection.GetValue<int?>("version")
|
||||
?? 3,
|
||||
ScannerIp = connection["ScannerIp"]
|
||||
?? connection["scanner_ip"]
|
||||
?? connection["DeviceIp"]
|
||||
?? "192.168.110.22",
|
||||
LocalIp = connection["LocalIp"]
|
||||
?? connection["local_ip"]
|
||||
?? "192.168.110.114",
|
||||
Transport = connection["Transport"]
|
||||
?? connection["transport"]
|
||||
?? "udp",
|
||||
FrameId = connection["FrameId"]
|
||||
?? connection["frame_id"]
|
||||
?? "olelidar",
|
||||
ScanTopic = connection["ScanTopic"]
|
||||
?? connection["scan_topic"]
|
||||
?? "scan",
|
||||
PacketType = connection["PacketType"]
|
||||
?? connection["packet_type"]
|
||||
?? "B",
|
||||
Inverted = connection.GetValue<bool?>("Inverted")
|
||||
?? connection.GetValue<bool?>("inverted")
|
||||
?? false,
|
||||
TimeFromLidar = connection.GetValue<bool?>("TimeFromLidar")
|
||||
?? connection.GetValue<bool?>("timeFromLidar")
|
||||
?? true,
|
||||
Ntp = connection.GetValue<bool?>("Ntp")
|
||||
?? connection.GetValue<bool?>("ntp")
|
||||
?? false,
|
||||
RangeMin = connection.GetValue<double?>("RangeMin")
|
||||
?? connection.GetValue<double?>("range_min")
|
||||
?? 0.08,
|
||||
RangeMax = connection.GetValue<double?>("RangeMax")
|
||||
?? connection.GetValue<double?>("range_max")
|
||||
?? 50.0,
|
||||
AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true,
|
||||
ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 3000,
|
||||
MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0
|
||||
};
|
||||
|
||||
var port = connection.GetValue<int?>("Port")
|
||||
?? connection.GetValue<int?>("port")
|
||||
?? connection.GetValue<int?>("DevicePort")
|
||||
?? 2368;
|
||||
if (port is < 1 or > 65535)
|
||||
{
|
||||
throw new InvalidOperationException($"Invalid Port: {port}. Must be between 1 and 65535");
|
||||
}
|
||||
|
||||
cfg.Port = port;
|
||||
|
||||
if (!string.Equals(cfg.Transport, "udp", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
throw new InvalidOperationException("OleiGS15Driver currently supports transport=udp only");
|
||||
}
|
||||
|
||||
if (cfg.RangeMin < 0)
|
||||
{
|
||||
throw new InvalidOperationException($"RangeMin must be >= 0. Current value: {cfg.RangeMin}");
|
||||
}
|
||||
|
||||
if (cfg.RangeMax <= cfg.RangeMin)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"RangeMax must be greater than RangeMin. RangeMin={cfg.RangeMin}, RangeMax={cfg.RangeMax}");
|
||||
}
|
||||
|
||||
return cfg;
|
||||
}
|
||||
}
|
||||
|
||||
[Device(DeviceType.Lidar, "Olei", "OleiGS15Driver", "1.0.0",
|
||||
Description = "Olei GS1-5 LiDAR driver (ROS version3-compatible UDP parser)")]
|
||||
public class OleiGS15Driver : DeviceBase, ILidar
|
||||
{
|
||||
private const int V3HeaderSize = 48;
|
||||
private const ushort V3Magic = 0xFEAC;
|
||||
|
||||
private readonly OleiGS15DriverConfig _config;
|
||||
private readonly OleiGS15UdpConnection _connection = new();
|
||||
private readonly Lock _dataLock = new();
|
||||
|
||||
private CancellationTokenSource? _updateCts;
|
||||
private Task? _updateTask;
|
||||
|
||||
private LaserScan? _lastLaserScan;
|
||||
private DateTime? _lastScanDataTimestamp;
|
||||
private int _lastPointCount;
|
||||
private double? _lastScanFrequencyHz;
|
||||
private bool _portOpened;
|
||||
|
||||
private V3ScanAccumulator? _scanAccumulator;
|
||||
private ushort? _lastFirstIndex;
|
||||
|
||||
public OleiGS15Driver(string deviceId, string deviceName, IConfigurationSection connection)
|
||||
: base(deviceId, deviceName, DeviceType.Lidar)
|
||||
{
|
||||
_config = OleiGS15DriverConfig.Parse(connection);
|
||||
|
||||
_connection.device_IP = _config.ScannerIp;
|
||||
_connection.local_ip = _config.LocalIp;
|
||||
_connection.device_port = _config.Port;
|
||||
|
||||
AutoReconnectEnabled = _config.AutoReconnectEnabled;
|
||||
ReconnectDelayMs = _config.ReconnectDelayMs;
|
||||
MaxReconnectAttempts = _config.MaxReconnectAttempts;
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
yield return new PropertyDescription("Version", "Version", "Phiên bản protocol")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 1,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "3"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("PacketType", "Packet Type", "Loại packet A/B/C")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 2,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "B"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("ScannerIp", "Scanner IP", "IP của Olei GS1-5")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 3,
|
||||
Category = "Kết nối",
|
||||
DefaultValue = ""
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("LocalIp", "Local IP", "IP local nhận UDP packet")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 4,
|
||||
Category = "Kết nối",
|
||||
DefaultValue = ""
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("Port", "Port", "UDP port của lidar")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 5,
|
||||
Category = "Kết nối",
|
||||
DefaultValue = "2368"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("FrameId", "Frame ID", "Frame ID của LaserScan")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 6,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "olelidar"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("RangeMin", "Range Min (m)", "Khoảng cách tối thiểu")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 7,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "0.08"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("RangeMax", "Range Max (m)", "Khoảng cách tối đa")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 8,
|
||||
Category = "Cấu hình",
|
||||
DefaultValue = "50.0"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("ConnectionStatus", "Connection Status", "Trạng thái kết nối lidar")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 9,
|
||||
Category = "Trạng thái",
|
||||
DefaultValue = "Disconnected"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("PointCount", "Point Count", "Số điểm scan gần nhất")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 10,
|
||||
Category = "Trạng thái",
|
||||
DefaultValue = "0"
|
||||
};
|
||||
|
||||
yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan gần nhất")
|
||||
{
|
||||
DataType = "string",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 11,
|
||||
Category = "Trạng thái",
|
||||
DefaultValue = ""
|
||||
};
|
||||
}
|
||||
|
||||
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_portOpened = _connection.openPort();
|
||||
if (!_portOpened)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"Failed to open Olei GS1-5 UDP port at {_config.LocalIp}:{_config.Port}");
|
||||
}
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StartUpdateLoop();
|
||||
SetProperty("ConnectionStatus", "Connected");
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
StopUpdateLoop();
|
||||
CloseUdpPort();
|
||||
SetProperty("ConnectionStatus", "Disconnected");
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_lastLaserScan = null;
|
||||
_lastScanDataTimestamp = null;
|
||||
_lastPointCount = 0;
|
||||
_lastScanFrequencyHz = null;
|
||||
_scanAccumulator = null;
|
||||
_lastFirstIndex = null;
|
||||
}
|
||||
|
||||
UpdateProperties();
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (!_portOpened)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_updateTask == null || _updateTask.IsCompleted)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_lastScanDataTimestamp == null)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return (DateTime.UtcNow - _lastScanDataTimestamp.Value).TotalSeconds < 5.0;
|
||||
}
|
||||
}
|
||||
|
||||
private void StartUpdateLoop()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_updateTask != null && !_updateTask.IsCompleted)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_updateCts = new CancellationTokenSource();
|
||||
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token), _updateCts.Token);
|
||||
}
|
||||
}
|
||||
|
||||
private void StopUpdateLoop()
|
||||
{
|
||||
Task? updateTask;
|
||||
CancellationTokenSource? cts;
|
||||
|
||||
lock (_dataLock)
|
||||
{
|
||||
updateTask = _updateTask;
|
||||
cts = _updateCts;
|
||||
_updateTask = null;
|
||||
_updateCts = null;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
cts?.Cancel();
|
||||
updateTask?.Wait(TimeSpan.FromSeconds(2));
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
finally
|
||||
{
|
||||
cts?.Dispose();
|
||||
}
|
||||
}
|
||||
|
||||
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
_connection.readRawdata();
|
||||
|
||||
var processedAnyPacket = false;
|
||||
while (TryProcessOnePacket())
|
||||
{
|
||||
processedAnyPacket = true;
|
||||
}
|
||||
|
||||
if (!processedAnyPacket)
|
||||
{
|
||||
await Task.Delay(5, cancellationToken);
|
||||
}
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LastError = ex;
|
||||
OnErrorOccurred(ex, "OleiGS15 update loop error");
|
||||
await Task.Delay(100, cancellationToken);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryProcessOnePacket()
|
||||
{
|
||||
var packet = _connection.TryDequeuePacket();
|
||||
if (packet == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryParseV3Packet(packet.data, out var header, out var rangesMm, out var intensities, out var hasIntensity))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
var packetTimestamp = _config.TimeFromLidar
|
||||
? (packet.stamp.Kind == DateTimeKind.Utc ? packet.stamp : packet.stamp.ToUniversalTime())
|
||||
: DateTime.UtcNow;
|
||||
|
||||
V3ScanAccumulator? completedScan = null;
|
||||
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (_lastFirstIndex.HasValue && _lastFirstIndex.Value > header.FirstIndex)
|
||||
{
|
||||
_scanAccumulator = null;
|
||||
}
|
||||
_lastFirstIndex = header.FirstIndex;
|
||||
|
||||
if (_scanAccumulator == null)
|
||||
{
|
||||
_scanAccumulator = new V3ScanAccumulator(header, _config);
|
||||
}
|
||||
|
||||
_scanAccumulator.AddPacketData(rangesMm, intensities, hasIntensity, _config);
|
||||
|
||||
if (_scanAccumulator.IsCompleted)
|
||||
{
|
||||
completedScan = _scanAccumulator;
|
||||
_scanAccumulator = null;
|
||||
}
|
||||
}
|
||||
|
||||
if (completedScan != null)
|
||||
{
|
||||
PublishCompletedScan(completedScan, packetTimestamp);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private void PublishCompletedScan(V3ScanAccumulator completed, DateTime packetTimestamp)
|
||||
{
|
||||
var scanTime = completed.ScanTimeSeconds > 0 ? completed.ScanTimeSeconds : 0.1f;
|
||||
var scanStamp = packetTimestamp - TimeSpan.FromSeconds(scanTime);
|
||||
|
||||
var laserScan = new LaserScan
|
||||
{
|
||||
Header = new RobotNet10.Shared.Header
|
||||
{
|
||||
Seq = 0,
|
||||
Stamp = scanStamp,
|
||||
FrameId = _config.FrameId
|
||||
},
|
||||
AngleMin = completed.AngleMin,
|
||||
AngleMax = completed.AngleMax,
|
||||
AngleIncrement = completed.AngleIncrement,
|
||||
TimeIncrement = completed.TimeIncrement,
|
||||
ScanTime = scanTime,
|
||||
RangeMin = _config.RangeMin,
|
||||
RangeMax = _config.RangeMax,
|
||||
Ranges = Array.ConvertAll(completed.Ranges, x => (double)x),
|
||||
Intensities = Array.ConvertAll(completed.Intensities, x => (double)x)
|
||||
};
|
||||
|
||||
lock (_dataLock)
|
||||
{
|
||||
_lastLaserScan = laserScan;
|
||||
_lastScanDataTimestamp = scanStamp;
|
||||
_lastPointCount = completed.Ranges.Length;
|
||||
_lastScanFrequencyHz = completed.ScanFrequencyHz > 0 ? completed.ScanFrequencyHz : null;
|
||||
}
|
||||
|
||||
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
private static bool TryParseV3Packet(
|
||||
byte[] data,
|
||||
out V3HeaderLite header,
|
||||
out ushort[] rangesMm,
|
||||
out ushort[] intensities,
|
||||
out bool hasIntensity)
|
||||
{
|
||||
header = default;
|
||||
rangesMm = Array.Empty<ushort>();
|
||||
intensities = Array.Empty<ushort>();
|
||||
hasIntensity = false;
|
||||
|
||||
if (data.Length < V3HeaderSize)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var magic = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(0, 2));
|
||||
if (magic != V3Magic)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
header = new V3HeaderLite
|
||||
{
|
||||
Magic = magic,
|
||||
Version = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(2, 2)),
|
||||
PacketSize = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(4, 4)),
|
||||
HeaderSize = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(8, 2)),
|
||||
DistanceRatio = data[10],
|
||||
Types = data[11],
|
||||
ScanNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(12, 2)),
|
||||
PacketNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(14, 2)),
|
||||
TimestampDecimal = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(16, 4)),
|
||||
TimestampInteger = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(20, 4)),
|
||||
ScanFrequencyRaw = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(24, 2)),
|
||||
NumPointsScan = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(26, 2)),
|
||||
InputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(28, 2)),
|
||||
OutputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(30, 2)),
|
||||
FieldStatus = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(32, 4)),
|
||||
StartIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(36, 2)),
|
||||
EndIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(38, 2)),
|
||||
FirstIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(40, 2)),
|
||||
NumPointsPacket = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(42, 2)),
|
||||
StatusFlags = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(44, 4))
|
||||
};
|
||||
|
||||
var packetSize = header.PacketSize == 0 ? data.Length : (int)Math.Min(header.PacketSize, data.Length);
|
||||
var headerSize = header.HeaderSize == 0 ? V3HeaderSize : header.HeaderSize;
|
||||
if (headerSize < V3HeaderSize || headerSize > packetSize)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var bytesPerPoint = header.Types switch
|
||||
{
|
||||
0x00 => 2,
|
||||
0x01 => 4,
|
||||
0x10 => 4,
|
||||
_ => 0
|
||||
};
|
||||
|
||||
if (bytesPerPoint == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var payloadBytes = packetSize - headerSize;
|
||||
var pointCount = header.NumPointsPacket;
|
||||
if (pointCount == 0 || pointCount * bytesPerPoint > payloadBytes)
|
||||
{
|
||||
pointCount = (ushort)(payloadBytes / bytesPerPoint);
|
||||
}
|
||||
|
||||
if (pointCount == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
rangesMm = new ushort[pointCount];
|
||||
|
||||
if (header.Types == 0x01)
|
||||
{
|
||||
hasIntensity = true;
|
||||
intensities = new ushort[pointCount];
|
||||
}
|
||||
|
||||
var offset = headerSize;
|
||||
for (int i = 0; i < pointCount; i++)
|
||||
{
|
||||
switch (header.Types)
|
||||
{
|
||||
case 0x00:
|
||||
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
|
||||
offset += 2;
|
||||
break;
|
||||
case 0x01:
|
||||
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
|
||||
intensities[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
|
||||
offset += 4;
|
||||
break;
|
||||
case 0x10:
|
||||
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
|
||||
offset += 4;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private void UpdateProperties()
|
||||
{
|
||||
DateTime? timestamp;
|
||||
int pointCount;
|
||||
|
||||
lock (_dataLock)
|
||||
{
|
||||
timestamp = _lastScanDataTimestamp;
|
||||
pointCount = _lastPointCount;
|
||||
}
|
||||
|
||||
SetProperty("Version", _config.Version.ToString());
|
||||
SetProperty("PacketType", _config.PacketType);
|
||||
SetProperty("ScannerIp", _config.ScannerIp);
|
||||
SetProperty("LocalIp", _config.LocalIp);
|
||||
SetProperty("Port", _config.Port.ToString());
|
||||
SetProperty("FrameId", _config.FrameId);
|
||||
SetProperty("RangeMin", _config.RangeMin.ToString("F3"));
|
||||
SetProperty("RangeMax", _config.RangeMax.ToString("F3"));
|
||||
SetProperty("PointCount", pointCount.ToString());
|
||||
SetProperty("LastScanTime", timestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
|
||||
}
|
||||
|
||||
private void CloseUdpPort()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_portOpened = false;
|
||||
|
||||
try
|
||||
{
|
||||
_connection.udp?.Close();
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
_connection.udp?.Dispose();
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public LaserScan? CurrentMeasurementData
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
return _lastLaserScan;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public DateTime? LastScanDataTimestamp
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
return _lastScanDataTimestamp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public double MinAngleRad
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
return _lastLaserScan?.AngleMin ?? -Math.PI;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public double MaxAngleRad
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
return _lastLaserScan?.AngleMax ?? Math.PI;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public double MinRangeM => _config.RangeMin;
|
||||
public double MaxRangeM => _config.RangeMax;
|
||||
|
||||
public double? AngularResolutionRad
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
var scan = _lastLaserScan;
|
||||
if (scan?.Ranges == null || scan.Value.Ranges.Length < 2)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return scan.Value.AngleIncrement;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public double? ScanFrequencyHz
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
return _lastScanFrequencyHz;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public double FieldOfViewRad => Math.Abs(MaxAngleRad - MinAngleRad);
|
||||
public bool SupportsIntensity => true;
|
||||
public double? AccuracyM => null;
|
||||
|
||||
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
StopUpdateLoop();
|
||||
CloseUdpPort();
|
||||
}
|
||||
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
private readonly struct V3HeaderLite
|
||||
{
|
||||
public ushort Magic { get; init; }
|
||||
public ushort Version { get; init; }
|
||||
public uint PacketSize { get; init; }
|
||||
public ushort HeaderSize { get; init; }
|
||||
public byte DistanceRatio { get; init; }
|
||||
public byte Types { get; init; }
|
||||
public ushort ScanNumber { get; init; }
|
||||
public ushort PacketNumber { get; init; }
|
||||
public uint TimestampDecimal { get; init; }
|
||||
public uint TimestampInteger { get; init; }
|
||||
public ushort ScanFrequencyRaw { get; init; }
|
||||
public ushort NumPointsScan { get; init; }
|
||||
public ushort InputStatus { get; init; }
|
||||
public ushort OutputStatus { get; init; }
|
||||
public uint FieldStatus { get; init; }
|
||||
public ushort StartIndex { get; init; }
|
||||
public ushort EndIndex { get; init; }
|
||||
public ushort FirstIndex { get; init; }
|
||||
public ushort NumPointsPacket { get; init; }
|
||||
public uint StatusFlags { get; init; }
|
||||
}
|
||||
|
||||
private sealed class V3ScanAccumulator
|
||||
{
|
||||
private int _writeIndex;
|
||||
|
||||
public float[] Ranges { get; }
|
||||
public float[] Intensities { get; }
|
||||
public bool IsCompleted => _writeIndex >= Ranges.Length;
|
||||
public float AngleMin { get; }
|
||||
public float AngleMax { get; }
|
||||
public float AngleIncrement { get; }
|
||||
public float TimeIncrement { get; }
|
||||
public float ScanTimeSeconds { get; }
|
||||
public double ScanFrequencyHz { get; }
|
||||
|
||||
public V3ScanAccumulator(V3HeaderLite header, OleiGS15DriverConfig config)
|
||||
{
|
||||
var actualNum = Math.Max(1, (int)(header.EndIndex - header.StartIndex));
|
||||
var pointsPerScan = Math.Max(1, (int)header.NumPointsScan);
|
||||
|
||||
Ranges = new float[actualNum];
|
||||
for (int i = 0; i < Ranges.Length; i++)
|
||||
{
|
||||
Ranges[i] = 0.0f;
|
||||
}
|
||||
|
||||
Intensities = new float[actualNum];
|
||||
|
||||
AngleIncrement = (float)((360.0 / pointsPerScan) * Math.PI / 180.0);
|
||||
AngleMin = (float)(header.StartIndex * AngleIncrement - Math.PI);
|
||||
AngleMax = (float)(header.EndIndex * AngleIncrement - Math.PI);
|
||||
|
||||
var rpm = header.ScanFrequencyRaw & 0x7FFF;
|
||||
ScanFrequencyHz = rpm > 0 ? rpm / 60.0 : 0.0;
|
||||
ScanTimeSeconds = rpm > 0 ? (float)(60.0 / rpm) : 0.1f;
|
||||
TimeIncrement = ScanTimeSeconds / pointsPerScan;
|
||||
}
|
||||
|
||||
public void AddPacketData(ushort[] rangesMm, ushort[] intensities, bool hasIntensity, OleiGS15DriverConfig config)
|
||||
{
|
||||
for (int i = 0; i < rangesMm.Length && _writeIndex < Ranges.Length; i++)
|
||||
{
|
||||
var rangeM = rangesMm[i] / 1000.0f;
|
||||
if (rangeM < config.RangeMin || rangeM > config.RangeMax || rangeM <= 0)
|
||||
{
|
||||
Ranges[_writeIndex] = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
Ranges[_writeIndex] = rangeM;
|
||||
}
|
||||
|
||||
if (hasIntensity && i < intensities.Length)
|
||||
{
|
||||
Intensities[_writeIndex] = intensities[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
Intensities[_writeIndex] = 0.0f;
|
||||
}
|
||||
|
||||
_writeIndex++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user