Initial commit
This commit is contained in:
@@ -0,0 +1,519 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user