438 lines
13 KiB
C#
438 lines
13 KiB
C#
using System.Buffers.Binary;
|
|
using System.Net;
|
|
using System.Net.Sockets;
|
|
|
|
namespace RobotNet10.RobotApp.Drivers.Lidar;
|
|
|
|
/// <summary>
|
|
/// UDP transport + packet decode aligned with olelidar ROS driver (driver.cpp / decoder.cpp).
|
|
/// </summary>
|
|
public sealed class Connect : IDisposable
|
|
{
|
|
public const int kPacketSize = OleiConstants.PacketSize;
|
|
|
|
public string device_IP { get; set; } = "192.168.254.13";
|
|
public int device_port { get; set; } = 2368;
|
|
public string local_ip { get; set; } = "192.168.254.10";
|
|
public string multiaddr_ip { get; set; } = string.Empty;
|
|
|
|
private readonly Queue<oleiPackage> _packetList = new();
|
|
private readonly object _packetListLock = new();
|
|
|
|
private UdpClient? _udp;
|
|
private Thread? _receiveThread;
|
|
private volatile bool _running;
|
|
private IPAddress? _deviceIpAddress;
|
|
|
|
private long _totalPacketsReceived;
|
|
private long _droppedWrongSource;
|
|
private long _droppedWrongSize;
|
|
|
|
public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);
|
|
public long DroppedWrongSource => Interlocked.Read(ref _droppedWrongSource);
|
|
public long DroppedWrongSize => Interlocked.Read(ref _droppedWrongSize);
|
|
|
|
public bool IsOpen => _udp != null && _running;
|
|
|
|
public bool openPort()
|
|
{
|
|
try
|
|
{
|
|
_deviceIpAddress = IPAddress.Parse(device_IP).MapToIPv4();
|
|
|
|
var bindAddress = string.IsNullOrWhiteSpace(local_ip)
|
|
? IPAddress.Any
|
|
: IPAddress.Parse(local_ip).MapToIPv4();
|
|
var bindEp = new IPEndPoint(bindAddress, device_port);
|
|
|
|
_udp = new UdpClient();
|
|
_udp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
|
|
_udp.Client.Bind(bindEp);
|
|
_udp.Client.ReceiveTimeout = 500;
|
|
|
|
if (!string.IsNullOrWhiteSpace(multiaddr_ip))
|
|
{
|
|
try
|
|
{
|
|
_udp.JoinMulticastGroup(IPAddress.Parse(multiaddr_ip));
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"[WARN] Olei multicast join failed ({multiaddr_ip}): {ex.Message}");
|
|
}
|
|
}
|
|
|
|
StartReceiveLoop();
|
|
Console.WriteLine($"[INFO] Olei UDP listening on {bindEp.Address}:{bindEp.Port}, device {device_IP}");
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"[ERROR] Cannot open Olei UDP port: {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
public void closePort()
|
|
{
|
|
_running = false;
|
|
try
|
|
{
|
|
_receiveThread?.Join(TimeSpan.FromSeconds(2));
|
|
}
|
|
catch { /* ignore */ }
|
|
|
|
_receiveThread = null;
|
|
_udp?.Close();
|
|
_udp?.Dispose();
|
|
_udp = null;
|
|
}
|
|
|
|
private void StartReceiveLoop()
|
|
{
|
|
_running = true;
|
|
_receiveThread = new Thread(ReceiveLoop)
|
|
{
|
|
IsBackground = true,
|
|
Name = $"OleiLidar-UDP-{device_port}",
|
|
Priority = ThreadPriority.Highest
|
|
};
|
|
_receiveThread.Start();
|
|
}
|
|
|
|
private void ReceiveLoop()
|
|
{
|
|
while (_running && _udp != null)
|
|
{
|
|
try
|
|
{
|
|
var remote = new IPEndPoint(IPAddress.Any, 0);
|
|
var data = _udp.Receive(ref remote);
|
|
|
|
var senderIp = remote.Address.MapToIPv4();
|
|
if (_deviceIpAddress != null && !senderIp.Equals(_deviceIpAddress))
|
|
{
|
|
Interlocked.Increment(ref _droppedWrongSource);
|
|
continue;
|
|
}
|
|
|
|
if (data.Length < kPacketSize)
|
|
{
|
|
Interlocked.Increment(ref _droppedWrongSize);
|
|
continue;
|
|
}
|
|
|
|
Interlocked.Increment(ref _totalPacketsReceived);
|
|
|
|
var packet = new oleiPackage
|
|
{
|
|
stamp = DateTime.UtcNow,
|
|
data = new byte[kPacketSize]
|
|
};
|
|
Array.Copy(data, packet.data, kPacketSize);
|
|
|
|
lock (_packetListLock)
|
|
{
|
|
_packetList.Enqueue(packet);
|
|
}
|
|
}
|
|
catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
|
|
{
|
|
// Normal when no packets (ROS poll timeout).
|
|
}
|
|
catch (SocketException ex) when (!_running)
|
|
{
|
|
break;
|
|
}
|
|
catch (ObjectDisposedException)
|
|
{
|
|
break;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
if (_running)
|
|
Console.WriteLine($"[ERROR] Olei UDP receive: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
|
|
public int GetPacketCount()
|
|
{
|
|
lock (_packetListLock)
|
|
return _packetList.Count;
|
|
}
|
|
|
|
public oleiPackage? TryDequeuePacket()
|
|
{
|
|
lock (_packetListLock)
|
|
return _packetList.Count == 0 ? null : _packetList.Dequeue();
|
|
}
|
|
|
|
public void Dispose() => closePort();
|
|
}
|
|
|
|
public sealed class oleiPackage
|
|
{
|
|
public DateTime stamp { get; set; }
|
|
public byte[] data { get; set; } = new byte[OleiConstants.PacketSize];
|
|
}
|
|
|
|
public sealed class DecoderConfig
|
|
{
|
|
public double AngleMin { get; set; } = 0.0;
|
|
public double AngleMax { get; set; } = 360.0;
|
|
public double RangeMin { get; set; } = 0.2;
|
|
public double RangeMax { get; set; } = 30.0;
|
|
public int Poly { get; set; } = 1;
|
|
public bool Inverted { get; set; }
|
|
public double StepDeg { get; set; } = 0.225;
|
|
public string FrameId { get; set; } = "olelidar";
|
|
}
|
|
|
|
public static class OleiConstants
|
|
{
|
|
public const int DataHeadSize = 40;
|
|
public const int PointBytes = 8;
|
|
public const int BlocksPerPacket = 150;
|
|
public const int PacketSize = DataHeadSize + BlocksPerPacket * PointBytes;
|
|
public const float DistanceResolution = 0.001f;
|
|
public const float AzimuthResolutionDeg = 0.01f;
|
|
}
|
|
|
|
public readonly struct DataPoint
|
|
{
|
|
public readonly ushort Azimuth;
|
|
public readonly ushort Distance;
|
|
public readonly ushort Reflectivity;
|
|
|
|
public DataPoint(ReadOnlySpan<byte> span)
|
|
{
|
|
Azimuth = BinaryPrimitives.ReadUInt16LittleEndian(span);
|
|
Distance = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(2));
|
|
Reflectivity = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(4));
|
|
}
|
|
}
|
|
|
|
public readonly struct DataBlock
|
|
{
|
|
public readonly DataPoint Point;
|
|
public DataBlock(ReadOnlySpan<byte> span) => Point = new DataPoint(span);
|
|
}
|
|
|
|
public readonly struct DataHeader
|
|
{
|
|
public readonly byte[] Code;
|
|
public readonly uint Timestamp;
|
|
public readonly ushort Rpm;
|
|
public readonly uint Rsv;
|
|
|
|
public DataHeader(ReadOnlySpan<byte> span)
|
|
{
|
|
Code = span.Slice(22, 2).ToArray();
|
|
Timestamp = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(28));
|
|
Rpm = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(32));
|
|
Rsv = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(36));
|
|
}
|
|
}
|
|
|
|
public readonly struct Packet
|
|
{
|
|
public readonly DataHeader Header;
|
|
public readonly DataBlock[] Blocks;
|
|
|
|
public Packet(ReadOnlySpan<byte> span)
|
|
{
|
|
if (span.Length < OleiConstants.PacketSize)
|
|
throw new ArgumentException($"packet must be {OleiConstants.PacketSize} bytes");
|
|
|
|
Header = new DataHeader(span.Slice(0, OleiConstants.DataHeadSize));
|
|
Blocks = new DataBlock[OleiConstants.BlocksPerPacket];
|
|
var offset = OleiConstants.DataHeadSize;
|
|
for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
|
|
{
|
|
Blocks[i] = new DataBlock(span.Slice(offset, OleiConstants.PointBytes));
|
|
offset += OleiConstants.PointBytes;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Packet decoder — port of olelidar/src/decoder.cpp PacketCb / DecodeAndFill / PublishMsg.
|
|
/// </summary>
|
|
public sealed class DecodeLidar
|
|
{
|
|
public readonly List<ushort> scanAngleInVec = new();
|
|
public readonly List<ushort> scanRangeInVec = new();
|
|
public readonly List<ushort> scanIntensityInVec = new();
|
|
|
|
private readonly List<ushort> _scanAngleVec = new();
|
|
private readonly List<ushort> _scanRangeVec = new();
|
|
private readonly List<ushort> _scanIntensityVec = new();
|
|
|
|
public ushort AzimuthLast { get; private set; }
|
|
public ushort AzimuthNow { get; private set; }
|
|
public ushort AzimuthFirst { get; private set; } = 0xFFFF;
|
|
|
|
private DateTime _machineTimeBase;
|
|
private uint _innerTimestampBaseMs;
|
|
private bool _isTimeBase;
|
|
private uint _lastStampMs;
|
|
|
|
/// <summary>End-of-scan time mapped from lidar internal clock (UTC).</summary>
|
|
public DateTime? LastCompletedScanLidarEndUtc { get; private set; }
|
|
|
|
public float Frequency { get; private set; }
|
|
public byte LidarType { get; private set; } = 0x01;
|
|
public int Direction { get; private set; }
|
|
public double StepDeg { get; private set; } = 0.225;
|
|
|
|
private DecoderConfig _config = new();
|
|
private readonly object _locker = new();
|
|
|
|
public void SetConfig(DecoderConfig config)
|
|
{
|
|
_config = config;
|
|
StepDeg = config.StepDeg;
|
|
}
|
|
|
|
public bool PacketCb(oleiPackage dataMsg)
|
|
{
|
|
if (dataMsg.data.Length < OleiConstants.PacketSize)
|
|
return false;
|
|
|
|
var pkt = new Packet(dataMsg.data);
|
|
AzimuthNow = pkt.Blocks[0].Point.Azimuth;
|
|
|
|
if (AzimuthFirst == 0xFFFF)
|
|
{
|
|
LidarType = pkt.Header.Code.Length > 1 ? pkt.Header.Code[1] : (byte)0x01;
|
|
AzimuthFirst = AzimuthNow;
|
|
var rpm = pkt.Header.Rpm & 0x7FFF;
|
|
Direction = pkt.Header.Rpm >> 15;
|
|
if (_config.Inverted)
|
|
Direction = 1 - Direction;
|
|
|
|
if (Frequency < 0.001f && rpm > 0)
|
|
{
|
|
Frequency = rpm / 60.0f;
|
|
if (LidarType == 0x01)
|
|
StepDeg = 0.225;
|
|
}
|
|
}
|
|
|
|
if (AzimuthLast < AzimuthNow)
|
|
{
|
|
DecodeAndFill(pkt);
|
|
AzimuthLast = AzimuthNow;
|
|
return false;
|
|
}
|
|
|
|
AzimuthLast = AzimuthNow;
|
|
|
|
if (AzimuthFirst >= 200)
|
|
{
|
|
AzimuthFirst = AzimuthNow;
|
|
return false;
|
|
}
|
|
|
|
var nowStampMs = pkt.Header.Timestamp;
|
|
if (!_isTimeBase)
|
|
{
|
|
_machineTimeBase = dataMsg.stamp.Kind == DateTimeKind.Utc
|
|
? dataMsg.stamp
|
|
: dataMsg.stamp.ToUniversalTime();
|
|
_innerTimestampBaseMs = nowStampMs;
|
|
_isTimeBase = true;
|
|
}
|
|
|
|
_lastStampMs = nowStampMs;
|
|
LastCompletedScanLidarEndUtc = ComputeLidarTimeUtc(nowStampMs, dataMsg.stamp);
|
|
|
|
if (Frequency < 0.001f)
|
|
{
|
|
if (LidarType == 0x01)
|
|
{
|
|
var rpm = pkt.Header.Rpm & 0x7FFF;
|
|
Frequency = rpm / 60.0f;
|
|
StepDeg = 0.225;
|
|
}
|
|
else if (_scanAngleVec.Count > 2)
|
|
{
|
|
StepDeg = (_scanAngleVec[1] - _scanAngleVec[0]) / 100.0;
|
|
Frequency = (float)(StepDeg * 10000.0 / 60.0);
|
|
}
|
|
else
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
lock (_locker)
|
|
{
|
|
scanAngleInVec.Clear();
|
|
scanRangeInVec.Clear();
|
|
scanIntensityInVec.Clear();
|
|
|
|
scanAngleInVec.AddRange(_scanAngleVec);
|
|
scanRangeInVec.AddRange(_scanRangeVec);
|
|
scanIntensityInVec.AddRange(_scanIntensityVec);
|
|
|
|
if (Direction == 0)
|
|
{
|
|
scanRangeInVec.Reverse();
|
|
scanIntensityInVec.Reverse();
|
|
}
|
|
|
|
_scanAngleVec.Clear();
|
|
_scanRangeVec.Clear();
|
|
_scanIntensityVec.Clear();
|
|
}
|
|
|
|
DecodeAndFill(pkt);
|
|
return scanAngleInVec.Count > 0;
|
|
}
|
|
|
|
private DateTime ComputeLidarTimeUtc(uint lidarTimestampMs, DateTime receiveStamp)
|
|
{
|
|
var receiveUtc = receiveStamp.Kind == DateTimeKind.Utc
|
|
? receiveStamp
|
|
: receiveStamp.ToUniversalTime();
|
|
|
|
if (!_isTimeBase)
|
|
return receiveUtc;
|
|
|
|
var deltaMs = lidarTimestampMs - _innerTimestampBaseMs;
|
|
return _machineTimeBase.AddMilliseconds(deltaMs);
|
|
}
|
|
|
|
private void DecodeAndFill(Packet pkt)
|
|
{
|
|
var rangeMaxMm = (ushort)(_config.RangeMax * 1000);
|
|
var rangeMinMm = (ushort)(_config.RangeMin * 1000);
|
|
|
|
for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
|
|
{
|
|
var dp = pkt.Blocks[i].Point;
|
|
var azimuth = dp.Azimuth;
|
|
var range = dp.Distance;
|
|
var intensity = dp.Reflectivity;
|
|
|
|
if (range > rangeMaxMm || range < rangeMinMm)
|
|
{
|
|
range = 0;
|
|
intensity = 0;
|
|
}
|
|
|
|
if (azimuth < 0xFF00)
|
|
{
|
|
lock (_locker)
|
|
{
|
|
_scanAngleVec.Add(azimuth);
|
|
_scanRangeVec.Add(range);
|
|
_scanIntensityVec.Add(intensity);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|