Initial commit

2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarHandler.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarHandler.cs
@@ -0,0 +1,437 @@
+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);
+                }
+            }
+        }
+    }
+}