using System.Buffers.Binary; using System.Net; using System.Net.NetworkInformation; using System.Net.Sockets; namespace RobotNet10.RobotApp.Drivers.Lidar { public class oleiScan { public Header header { get; set; } = new Header(); public List oleiPackage { get; set; } = new List(); } public class oleiPackage { public DateTime stamp { get; set; } public byte[] data { get; set; } = new byte[1240]; } public class Header { public uint Seq { get; set; } // uint32 public DateTime Stamp { get; set; } // time public string FrameId { get; set; } // string } public class Connect { public Queue packetList = new Queue(); private readonly object _packetListLock = new object(); // Lock object cho thread-safe access public static readonly int kPacketSize = new oleiPackage().data.Length; public const int kError = -1; public string device_IP { get; set; } = "192.168.254.13"; // Lidar public int device_port { get; set; } = 2368; // Lidar port public string local_ip { get; set; } = "192.168.254.10"; // PC/VM IP public UdpClient udp; public IPEndPoint check_ip_device; // ---------------------------------------------------------- // CHECK LOCAL IP TỒN TẠI TRÊN NIC THẬT // ---------------------------------------------------------- private bool IpExists(string ip) { foreach (var ni in System.Net.NetworkInformation.NetworkInterface.GetAllNetworkInterfaces()) { foreach (var ua in ni.GetIPProperties().UnicastAddresses) { if (ua.Address.ToString() == ip) return true; } } return false; } // ---------------------------------------------------------- // CHECK PORT CÓ ĐANG ĐƯỢC SỬ DỤNG KHÔNG // ---------------------------------------------------------- private bool IsPortBusy(int port) { var props = IPGlobalProperties.GetIPGlobalProperties(); var listeners = props.GetActiveUdpListeners(); foreach (var ep in listeners) if (ep.Port == port) return true; return false; } // ---------------------------------------------------------- // MỞ CỔNG UDP // ---------------------------------------------------------- public bool openPort() { try { // 1) Check local_ip có tồn tại trên NIC thật không if (!IpExists(local_ip)) { Console.WriteLine($"[ERROR] local_ip {local_ip} does NOT exist on any NIC."); return false; } // 2) Check port có đang bận không if (IsPortBusy(device_port)) { Console.WriteLine($"[ERROR] Port {device_port} is already in use!"); return false; } // 3) Bind vào local_ip + port 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] CONNECT SUCCESSFULLY!"); return true; } catch (Exception ex) { Console.WriteLine($"[ERROR] Cannot open UDP port {local_ip}:{device_port}"); Console.WriteLine($"Reason: {ex.Message}"); return false; } } // ---------------------------------------------------------- // ĐỌC RAW DATA // ---------------------------------------------------------- public void readRawdata() { // Kiểm tra UDP client đã được khởi tạo chưa if (udp == null) { // Console.WriteLine("[ERROR] UDP client is not initialized. Call openPort() first."); return; } try { // Reset check_ip_device về giá trị ban đầu trước khi receive //check_ip_device = new IPEndPoint(IPAddress.Any, 0); byte[] data = udp.Receive(ref check_ip_device); // Console.WriteLine($"[INFO] check ip: {check_ip_device}"); // Kiểm tra xem có nhận được dữ liệu từ địa chỉ hợp lệ không 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) { Console.WriteLine("[WARN] Received data from invalid endpoint (0.0.0.0:0), ignoring"); return; } // Console.WriteLine($"[INFO] Received from {check_ip_device.Address}:{check_ip_device.Port}"); // 1) Kiểm tra đúng IP lidar hay chưa if (check_ip_device.Address.ToString() != device_IP) { Console.WriteLine($"[WARN] Ignored packet from {check_ip_device.Address}, expected {device_IP}"); return; } // 2) Check size đúng 1240 bytes if (data.Length != kPacketSize) { Console.WriteLine($"[WARN] Wrong packet size {data.Length}, expected {kPacketSize}"); return; } // 3) Đổ dữ liệu vào queue (thread-safe) oleiPackage packet = new oleiPackage(); Array.Copy(data, packet.data, kPacketSize); packet.stamp = DateTime.Now; lock (_packetListLock) { packetList.Enqueue(packet); } } catch (SocketException ex) { // Reset check_ip_device khi timeout hoặc lỗi check_ip_device = new IPEndPoint(IPAddress.Any, 0); Console.WriteLine($"SocketException caught!, IP checked: {check_ip_device}"); if (ex.SocketErrorCode == SocketError.TimedOut) { // Không log timeout để tránh spam log // Console.WriteLine("[INFO] No data received (timeout)"); } else { Console.WriteLine($"[ERROR] Socket error: {ex.Message}"); } } catch (Exception ex) { // Reset check_ip_device khi có lỗi check_ip_device = new IPEndPoint(IPAddress.Any, 0); Console.WriteLine("[ERROR] Unexpected error: " + ex.Message); } } // ---------------------------------------------------------- // THREAD-SAFE METHODS CHO PACKET QUEUE // ---------------------------------------------------------- public int GetPacketCount() { lock (_packetListLock) { return packetList.Count; } } public oleiPackage? TryDequeuePacket() { lock (_packetListLock) { if (packetList.Count == 0) { return null; } return packetList.Dequeue(); } } } public class DecoderConfig { public double AngleMin { get; set; } = -180.0; // degrees public double AngleMax { get; set; } = 180.0; // degrees public double RangeMin { get; set; } = 0.1; // meters public double RangeMax { get; set; } = 50.0; // meters public int Poly { get; set; } = 1; public bool Inverted { get; set; } = false; public double StepHundredthDeg { get; set; } = 225.0; // 0.225 deg * 100 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; // 1240 public const float DistanceResolution = 0.001f; // meters public const float AzimuthResolutionDeg = 0.01f; // degrees public const float DistanceMax = 20.0f; } // DTOs public readonly struct DataPoint { public readonly ushort Azimuth; // hundredth degree public readonly ushort Distance; // mm public readonly ushort Reflectivity; // unitless public readonly ushort Distance2; // reserved public DataPoint(ReadOnlySpan span) { Azimuth = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(0, 2)); Distance = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(2, 2)); Reflectivity = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(4, 2)); Distance2 = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(6, 2)); } public float DistanceMeters => Distance * OleiConstants.DistanceResolution; public double AzimuthDegrees => Azimuth * OleiConstants.AzimuthResolutionDeg; } public readonly struct FiringSequence { public readonly DataPoint Point; public FiringSequence(ReadOnlySpan span) => Point = new DataPoint(span); } public readonly struct DataHeader { public readonly byte[] Magic; // 4 bytes public readonly byte[] Version; // 2 bytes public readonly byte Scale; // 1 byte public readonly byte[] Oem; // 3 bytes public readonly byte[] Model; // 12 bytes public readonly byte[] Code; // 2 bytes public readonly byte[] Hw; // 2 bytes public readonly byte[] Sw; // 2 bytes public readonly uint Timestamp; // 4 bytes (device ms) public readonly ushort Rpm; // 2 bytes public readonly byte[] Flag; // 2 bytes public readonly uint Rsv; // 4 bytes (NTP seconds maybe) public DataHeader(ReadOnlySpan span) { Magic = span.Slice(0, 4).ToArray(); Version = span.Slice(4, 2).ToArray(); Scale = span[6]; Oem = span.Slice(7, 3).ToArray(); Model = span.Slice(10, 12).ToArray(); Code = span.Slice(22, 2).ToArray(); Hw = span.Slice(24, 2).ToArray(); Sw = span.Slice(26, 2).ToArray(); Timestamp = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(28, 4)); Rpm = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(32, 2)); Flag = span.Slice(34, 2).ToArray(); Rsv = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(36, 4)); } } public readonly struct DataBlock { public readonly FiringSequence sequence; public DataBlock(ReadOnlySpan span) => sequence = new FiringSequence(span); } public readonly struct Packet { public readonly DataHeader Header; public readonly DataBlock[] Block; public Packet(ReadOnlySpan span) { if (span.Length < OleiConstants.PacketSize) throw new ArgumentException($"packet must be at least {OleiConstants.PacketSize} bytes"); Header = new DataHeader(span.Slice(0, OleiConstants.DataHeadSize)); Block = new DataBlock[OleiConstants.BlocksPerPacket]; int offset = OleiConstants.DataHeadSize; int blockBytes = OleiConstants.PointBytes; for (int i = 0; i < OleiConstants.BlocksPerPacket; i++) { Block[i] = new DataBlock(span.Slice(offset, blockBytes)); offset += blockBytes; } } } // Main decoder public class DecodeLidar { // buffers public readonly List scanRadAngleVec = new(); public readonly List scanAngleVec = new(); public readonly List scanRangeVec = new(); public readonly List scanIntensityVec = new(); public readonly List scanAngleInVec = new(); public readonly List scanRangeInVec = new(); public readonly List scanIntensityInVec = new(); public readonly List scanRadAngleInVec = new(); // state public ushort AzimuthLast { get; private set; } = 0; public ushort AzimuthNow { get; private set; } = 0; public ushort AzimuthFirst { get; private set; } = 0xFFFF; private DateTime machineTimeBase; private uint innerTimestampBase; // ms private bool isTimeBase = false; private uint laststamp = 0; public float Frequency { get; private set; } = 0.0f; public byte LidarType { get; private set; } = 0x01; public int Direction { get; private set; } = 0; // private uint scanMsgSeq = 0; private double _stepHundredthDeg = 225.0; // default 0.225*100 private readonly DecoderConfig _config = new DecoderConfig(); // event to notify a completed scan private readonly object locker = new(); /// /// Set configuration for the decoder /// public void SetConfig(DecoderConfig config) { _config.Inverted = config.Inverted; } public void PacketCb(oleiPackage data_msg) { if (data_msg == null || data_msg.data == null || data_msg.data.Length < OleiConstants.PacketSize) return; // parse packet Packet pkt = new Packet(new ReadOnlySpan(data_msg.data)); // update azimuth AzimuthNow = pkt.Block[0].sequence.Point.Azimuth; // first packet initialization if (AzimuthFirst == 0xFFFF) { LidarType = pkt.Header.Code.Length > 1 ? pkt.Header.Code[1] : (byte)0x01; AzimuthFirst = AzimuthNow; int rpm = pkt.Header.Rpm & 0x7FFF; //Console.WriteLine($"[DecodeLidar] LidarType: 0x{LidarType:X2}, Initial RPM: {rpm}"); Direction = (pkt.Header.Rpm >> 15) & 1; if (_config.Inverted) Direction = 1 - Direction; if (Frequency < 0.001f && rpm > 0) { Frequency = rpm / 60.0f; // if (LidarType == 0x01) _stepHundredthDeg = 225.0; // 0.225 *100 } } // if azimuth increased -> decode into buffer and return (same behavior as original) if (AzimuthLast < AzimuthNow) { DecodeAndFill(pkt); AzimuthLast = AzimuthNow; return; } else { AzimuthLast = AzimuthNow; } // guard if (AzimuthFirst >= 200) { AzimuthFirst = AzimuthNow; return; } // timestamp handling: lidar internal timestamp in ms uint nowstamp = pkt.Header.Timestamp; // ms byte[] safetySignal = pkt.Header.Flag; byte inputIOSignal = (byte)((safetySignal[0] >> 4) & 0x0F); // 4 bits cao byte outputIOSignal = (byte)(safetySignal[0] & 0x0F); // 4 bits thấp byte errorStatus = (byte)(safetySignal[0] & 0x01); byte dangerZone = (byte)((safetySignal[0] >> 1) & 0x01); byte protectZone = (byte)((safetySignal[0] >> 2) & 0x01); byte warningZone = (byte)((safetySignal[0] >> 3) & 0x01); DateTime lidarTimeUtc; if (!isTimeBase) { // set base mapping: machineTimeBase corresponds to innerTimestampBase machineTimeBase = data_msg.stamp.Kind == DateTimeKind.Utc ? data_msg.stamp : data_msg.stamp.ToUniversalTime(); innerTimestampBase = nowstamp; lidarTimeUtc = machineTimeBase; isTimeBase = true; } else { uint deltaMs = nowstamp - innerTimestampBase; // wrap-around handled naturally with unsigned TimeSpan delta = TimeSpan.FromMilliseconds(deltaMs); lidarTimeUtc = machineTimeBase + delta; } // scan time for frame (delta between internal timestamps) uint scantime = nowstamp - laststamp; laststamp = nowstamp; // compute frequency if unknown (fallback) // move accumulated to in-vec (thread-safe) lock (locker) { scanAngleInVec.Clear(); scanRangeInVec.Clear(); scanIntensityInVec.Clear(); scanRadAngleInVec.Clear(); scanAngleInVec.AddRange(scanAngleVec); scanRangeInVec.AddRange(scanRangeVec); scanIntensityInVec.AddRange(scanIntensityVec); scanRadAngleInVec.AddRange(scanRadAngleVec); if(Direction == 0){ scanRangeInVec.Reverse(); scanIntensityInVec.Reverse(); } scanAngleVec.Clear(); scanRadAngleVec.Clear(); scanRangeVec.Clear(); scanIntensityVec.Clear(); } // publish one frame //PublishScan(lidarTimeUtc); // after publishing, decode current packet to start next frame DecodeAndFill(pkt); } public void DecodeAndFill(Packet pkt) { // pkt.Block[] already filled in constructor // Note: Range filtering is done in OleiLidarDriver based on its own config // We only filter obviously invalid data here (range = 0 or invalid azimuth) for (int iblk = 0; iblk < OleiConstants.BlocksPerPacket; iblk++) { var dp = pkt.Block[iblk].sequence.Point; double az = (double)(dp.Azimuth * 0.01); // dp.Azimuth unit: 0.01 deg/LSB, convert to degrees ushort range = dp.Distance; // mm var radaz = Deg2Rad(az); // Convert to radians ushort intensity = dp.Reflectivity; // Range: 0 - 65535 // Invalid azimuth guard: skip if azimuth is invalid (>= 0xFF00) // 0xFF00 = 65280 in decimal, which is used as invalid marker if (dp.Azimuth >= 0xFF00) { continue; // Skip invalid azimuth points } // Skip points with zero range (no valid measurement) // Range filtering based on MinRangeM/MaxRangeM is done in OleiLidarDriver if (range == 0) { continue; // Skip zero-range points } // Add valid point to buffers lock (locker) { scanAngleVec.Add(az); // Angle in degrees scanRangeVec.Add(range); // Distance in mm scanIntensityVec.Add(intensity); // Intensity 0-65535 scanRadAngleVec.Add(radaz); // Angle in radians } // Console.WriteLine($"[DecodeLidar] pointCloud added azimuth:{az}° range:{range}mm intensity:{intensity}"); } } private static double Deg2Rad(double deg) => deg * Math.PI / 180.0; } }