920 lines
29 KiB
C#
920 lines
29 KiB
C#
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++;
|
|
}
|
|
}
|
|
}
|
|
}
|