520 lines
20 KiB
C#
520 lines
20 KiB
C#
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> oleiPackage { get; set; } = new List<oleiPackage>();
|
|
}
|
|
|
|
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<oleiPackage> packetList = new Queue<oleiPackage>();
|
|
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<byte> 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<byte> 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<byte> 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<byte> span) => sequence = new FiringSequence(span);
|
|
}
|
|
|
|
public readonly struct Packet
|
|
{
|
|
public readonly DataHeader Header;
|
|
public readonly DataBlock[] Block;
|
|
|
|
public Packet(ReadOnlySpan<byte> 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<double> scanRadAngleVec = new();
|
|
public readonly List<double> scanAngleVec = new();
|
|
public readonly List<ushort> scanRangeVec = new();
|
|
public readonly List<ushort> scanIntensityVec = new();
|
|
|
|
public readonly List<double> scanAngleInVec = new();
|
|
public readonly List<ushort> scanRangeInVec = new();
|
|
public readonly List<ushort> scanIntensityInVec = new();
|
|
public readonly List<double> 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();
|
|
|
|
/// <summary>
|
|
/// Set configuration for the decoder
|
|
/// </summary>
|
|
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<byte>(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;
|
|
}
|
|
}
|