QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

557
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/Olei2dLidarDriver.cs Normal file
View File

@@ -0,0 +1,557 @@
using System.Diagnostics;
using Olei.LidarSensor;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;
namespace RobotNet10.RobotApp.Drivers.Olei;
/// <summary>
/// Configuration for Olei 2D LiDAR Driver
/// </summary>
public class Olei2dLidarDriverConfig
{
/// <summary>
/// UDP port to listen for LiDAR data
/// Default: 2368 (typical LiDAR port)
/// </summary>
public int UdpPort { get; set; } = 2368;
/// <summary>
/// Frame ID for the LiDAR sensor
/// Used in ROS-style message headers
/// </summary>
public string FrameId { get; set; } = "laser";
}
/// <summary>
/// Driver for Olei 2D LiDAR Sensor (LR-1F / LR-1BS)
/// Implements DeviceBase and ILidar interface
/// Protocol: UDP/IP v2.1
/// </summary>
[Device(DeviceType.Lidar, "Olei", "Olei2dLidarDriver", "1.0.0",
Description = "2D LiDAR Sensor Communication Data Protocol v2.1")]
public class Olei2dLidarDriver : DeviceBase, ILidar
{
private readonly Olei2dLidarDriverConfig _config = new();
private OleiLidarServer? _lidarServer;
// Cached measurements
private LaserScan? _currentMeasurementData;
private DateTime? _lastScanDataTimestamp;
// Frequency update tracking (for UI properties)
private readonly Stopwatch _frequencyUpdateStopwatch = Stopwatch.StartNew();
// Scan frequency calculation (actual LaserScan generation rate)
private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
private long _scansGeneratedInCurrentSecond = 0;
private readonly Lock _scanFrequencyLock = new();
// Calculated LiDAR specifications from actual data
private const double _minAngleRad = 0.0;
private const double _maxAngleRad = 2.0 * Math.PI;
private double _angularResolutionRad = 0.0; // Calculated from actual packet data
private const double DEFAULT_ACCURACY_M = 0.02; // 2 cm accuracy (fixed by hardware)
// Hardware range specifications (no filtering applied)
private const double HARDWARE_MIN_RANGE_M = 0.0;
private const double HARDWARE_MAX_RANGE_M = 30.0; // 30m max range for Olei LiDAR
// Packet accumulation for full scan (0° ~ 360°)
private readonly List<LidarDataBlock> _accumulatedBlocks = [];
private double _lastPacketStartAngle = -1.0;
private readonly Lock _accumulationLock = new();
private uint _scanSequenceNumber = 0;
/// <summary>
/// Constructor with configuration
/// </summary>
public Olei2dLidarDriver(
string deviceId,
string deviceName, IConfigurationSection configuration)
: base(deviceId, deviceName, DeviceType.Lidar)
{
configuration.Bind(_config);
Description = "Olei 2D LiDAR Sensor (LR-1F/LR-1BS) - UDP Protocol v2.1";
}
#region ILidar Implementation
/// <summary>
/// Current measurement data (scan points)
/// </summary>
public LaserScan? CurrentMeasurementData => _currentMeasurementData;
/// <summary>
/// Timestamp of the most recent scan data
/// </summary>
public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;
/// <summary>
/// Minimum scan angle (radians)
/// Calculated from actual LiDAR data
/// </summary>
public double MinAngleRad => _minAngleRad;
/// <summary>
/// Maximum scan angle (radians)
/// Calculated from actual LiDAR data
/// </summary>
public double MaxAngleRad => _maxAngleRad;
/// <summary>
/// Minimum measurement range (meters)
/// Hardware specification (no filtering)
/// </summary>
public double MinRangeM => HARDWARE_MIN_RANGE_M;
/// <summary>
/// Maximum measurement range (meters)
/// Hardware specification (no filtering)
/// </summary>
public double MaxRangeM => HARDWARE_MAX_RANGE_M;
/// <summary>
/// Angular resolution (radians)
/// Calculated from actual LiDAR data
/// </summary>
public double? AngularResolutionRad => _angularResolutionRad;
/// <summary>
/// Scan frequency (Hz)
/// Typically 10-20 Hz for Olei LiDAR
/// </summary>
public double? ScanFrequencyHz { get; private set; }
/// <summary>
/// Field of View (radians)
/// </summary>
public double FieldOfViewRad => MaxAngleRad - MinAngleRad;
/// <summary>
/// Supports intensity measurements
/// </summary>
public bool SupportsIntensity => true;
/// <summary>
/// Measurement accuracy (meters)
/// </summary>
public double? AccuracyM => DEFAULT_ACCURACY_M;
/// <summary>
/// Event raised when new scan data is received
/// </summary>
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
#endregion
#region DeviceBase Implementation
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
{
await Task.Run(() =>
{
// Initialize LiDAR server
_lidarServer = new OleiLidarServer(_config.UdpPort);
// Subscribe to events
_lidarServer.DataReceived += OnLidarDataReceived;
_lidarServer.ErrorOccurred += OnLidarErrorOccurred;
SetProperty("UdpPort", _config.UdpPort.ToString());
SetProperty("FrameId", _config.FrameId);
SetProperty("MinRange", $"{HARDWARE_MIN_RANGE_M:F2} m");
SetProperty("MaxRange", $"{HARDWARE_MAX_RANGE_M:F2} m");
SetProperty("FOV", "N/A"); // Will be calculated from actual data
SetProperty("AngularResolution", "N/A"); // Will be calculated from actual data
}, cancellationToken);
}
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
{
if (_lidarServer == null)
throw new InvalidOperationException("LiDAR server not initialized. Call InitializeAsync first.");
// Start LiDAR server
_lidarServer.Start();
SetProperty("ServerStatus", "Running");
SetProperty("Statistics", _lidarServer.GetStatistics());
}
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
{
_lidarServer?.Stop();
SetProperty("ServerStatus", "Stopped");
SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
}
protected override async Task OnResetAsync(CancellationToken cancellationToken)
{
// Reset statistics
_lidarServer?.ResetStatistics();
// Reset scan frequency tracking
lock (_scanFrequencyLock)
{
Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
_scanFrequencyStopwatch.Restart();
ScanFrequencyHz = null;
}
SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
SetProperty("ScanFrequency", "N/A");
}
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
// Check if server is running and receiving data
await Task.Delay(500, cancellationToken);
if (_lidarServer == null || !_lidarServer.IsRunning)
return false;
// Check if we received data recently (within last 5 seconds)
if (_lastScanDataTimestamp.HasValue)
{
/*var timeSinceLastScan = DateTime.UtcNow - _lastScanDataTimestamp.Value;
Console.WriteLine($"timeSinceLastScan.TotalSeconds = {timeSinceLastScan.TotalSeconds}");
return timeSinceLastScan.TotalSeconds < 5.0;*/
return true;
}
return false;
}
protected override List<PropertyDescription> CreatePropertyDescriptions()
{
return
[
new PropertyDescription("UdpPort", "UDP Port", "UDP port for receiving LiDAR data"),
new PropertyDescription("FrameId", "Frame ID", "ROS-style frame identifier"),
new PropertyDescription("MinRange", "Min Range", "Minimum measurement range"),
new PropertyDescription("MaxRange", "Max Range", "Maximum measurement range"),
new PropertyDescription("FOV", "Field of View", "Total scanning angle"),
new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
new PropertyDescription("ServerStatus", "Server Status", "UDP server status"),
new PropertyDescription("Statistics", "Statistics", "Server statistics"),
new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
];
}
protected override void Dispose(bool disposing)
{
if (disposing && _lidarServer != null)
{
_lidarServer.DataReceived -= OnLidarDataReceived;
_lidarServer.ErrorOccurred -= OnLidarErrorOccurred;
_lidarServer.Dispose();
_lidarServer = null;
}
base.Dispose(disposing);
}
#endregion
#region Event Handlers
/// <summary>
/// Handle data received from LiDAR server
/// Accumulates packets until full 360° scan is complete
/// </summary>
private void OnLidarDataReceived(object? sender, LidarDataPacket e)
{
try
{
var packet = e;
// Validate packet
if (!packet.Header.IsValidFrame)
return;
// Get first valid block's angle to detect scan wrap
double packetStartAngle = -1.0;
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
{
if (packet.DataBlocks[i].IsValid)
{
packetStartAngle = packet.DataBlocks[i].GetAngleDegrees();
break;
}
}
if (packetStartAngle < 0)
return; // No valid blocks in packet
lock (_accumulationLock)
{
// Detect scan completion: angle wrapped back to near 0° after being > 300°
bool isScanComplete = false;
if (_lastPacketStartAngle > 300.0 && packetStartAngle < 50.0)
{
// Scan wrapped around - complete current scan
isScanComplete = true;
}
if (isScanComplete && _accumulatedBlocks.Count > 0)
{
// Create LaserScan from accumulated blocks
var laserScan = CreateLaserScanFromAccumulated(packet.Header);
// Update cached data
_currentMeasurementData = laserScan;
_lastScanDataTimestamp = laserScan.Header.Stamp;
// Increment scan count and update frequency
Interlocked.Increment(ref _scansGeneratedInCurrentSecond);
UpdateScanFrequency();
// Update properties (once per second)
if (_frequencyUpdateStopwatch.ElapsedMilliseconds > 1000)
{
SetProperty("LastScanTime", _lastScanDataTimestamp.Value.ToString("HH:mm:ss.fff"));
SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
SetProperty("FOV", $"{FieldOfViewRad * 180 / Math.PI:F1}°");
SetProperty("AngularResolution", $"{_angularResolutionRad * 180 / Math.PI:F3}°");
_frequencyUpdateStopwatch.Restart();
}
// Raise event
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(
laserScan.Header.Stamp,
laserScan
));
// Clear accumulated blocks for next scan
_accumulatedBlocks.Clear();
}
// Add current packet's blocks to accumulation buffer
byte distanceScale = packet.Header.DistanceScale;
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
{
var block = packet.DataBlocks[i];
if (block.IsValid)
{
// Normalize angle to 0-360° range
double angleDeg = block.GetAngleDegrees();
if (angleDeg >= 360.0)
{
angleDeg %= 360.0;
}
// Store block (will be sorted later)
_accumulatedBlocks.Add(block);
}
}
_lastPacketStartAngle = packetStartAngle;
}
}
catch (Exception ex)
{
OnErrorOccurred(new Exception($"Error processing LiDAR data: {ex.Message}", ex));
}
}
/// <summary>
/// Handle errors from LiDAR server
/// </summary>
private void OnLidarErrorOccurred(object? sender, LidarErrorEventArgs e)
{
OnErrorOccurred(e.Exception ?? new Exception(e.Message));
}
#endregion
#region Helper Methods
/// <summary>
/// Update scan frequency based on LaserScan generation rate
/// Calculates how many scans are generated per second
/// </summary>
private void UpdateScanFrequency()
{
// Check if 1 second has elapsed
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
{
lock (_scanFrequencyLock)
{
// Double-check inside lock to prevent race condition
if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
{
// Calculate frequency (scans per second)
long scanCount = Interlocked.Read(ref _scansGeneratedInCurrentSecond);
double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
ScanFrequencyHz = scanCount / elapsedSeconds;
// Reset for next measurement period
Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
_scanFrequencyStopwatch.Restart();
}
}
}
}
/// <summary>
/// Create LaserScan from accumulated blocks (full 360° scan)
/// </summary>
private LaserScan CreateLaserScanFromAccumulated(LidarHeader lastHeader)
{
var header = new Header(
seq: _scanSequenceNumber++,
stamp: DateTime.UtcNow,
frameId: _config.FrameId
);
// Get distance scale from last header
byte distanceScale = lastHeader.DistanceScale;
double distanceScaleToMeters = distanceScale / 1000.0;
// Calculate actual angle and range from accumulated data
double minAngleDeg = double.MaxValue;
double maxAngleDeg = double.MinValue;
double minDistanceM = double.MaxValue;
double maxDistanceM = double.MinValue;
// Create sorted list of angles to calculate angular resolution
var sortedAngles = new List<double>(_accumulatedBlocks.Count);
// First pass: Find actual min/max values from data and collect angles
foreach (var block in _accumulatedBlocks)
{
double angleDeg = block.GetAngleDegrees();
// Normalize angle to 0-360° range
if (angleDeg >= 360.0)
angleDeg %= 360.0;
minAngleDeg = Math.Min(minAngleDeg, angleDeg);
maxAngleDeg = Math.Max(maxAngleDeg, angleDeg);
sortedAngles.Add(angleDeg);
double distanceM = block.DistanceRaw * distanceScaleToMeters;
if (distanceM > 0) // Only consider valid distances
{
minDistanceM = Math.Min(minDistanceM, distanceM);
maxDistanceM = Math.Max(maxDistanceM, distanceM);
}
}
// Fallback to defaults if no valid data
if (minAngleDeg == double.MaxValue || maxAngleDeg == double.MinValue)
{
minAngleDeg = 0.0;
maxAngleDeg = 360.0;
}
if (minDistanceM == double.MaxValue || maxDistanceM == double.MinValue)
{
minDistanceM = HARDWARE_MIN_RANGE_M;
maxDistanceM = HARDWARE_MAX_RANGE_M;
}
// Calculate angular resolution from actual data
double angularResolutionDeg = 0.225; // Default fallback
if (sortedAngles.Count > 1)
{
sortedAngles.Sort();
// Find minimum difference between consecutive angles
double minAngleDiff = double.MaxValue;
for (int i = 1; i < sortedAngles.Count; i++)
{
double diff = sortedAngles[i] - sortedAngles[i - 1];
if (diff > 0.01) // Ignore very small differences (noise/duplicates)
{
minAngleDiff = Math.Min(minAngleDiff, diff);
}
}
if (minAngleDiff < double.MaxValue)
{
angularResolutionDeg = minAngleDiff;
}
}
// Update cached angular resolution for property reporting
_angularResolutionRad = angularResolutionDeg * Math.PI / 180.0;
// Convert angles to radians
double angleMinRad = minAngleDeg * Math.PI / 180.0;
double angleMaxRad = maxAngleDeg * Math.PI / 180.0;
// Calculate expected number of points based on calculated angular resolution
double angleSpanDeg = maxAngleDeg - minAngleDeg;
int expectedPoints = (int)Math.Ceiling(angleSpanDeg / angularResolutionDeg) + 1;
// Initialize arrays with expected size
double[] ranges = new double[expectedPoints];
double[] intensities = new double[expectedPoints];
// Initialize all to -1 (no data, JSON-safe)
Array.Fill(ranges, -1.0);
Array.Fill(intensities, 0.0);
// Second pass: Map accumulated blocks to array indices based on angle
int validCount = 0;
foreach (var block in _accumulatedBlocks)
{
// Reflect angle across Y-axis (vertical axis)
double angleDeg = 180 - block.GetAngleDegrees();
// Normalize angle to 0-360° range
if (angleDeg < 0)
angleDeg += 360.0;
else if (angleDeg >= 360.0)
angleDeg %= 360.0;
// Calculate array index from relative angle position using calculated angular resolution
double relativeAngle = angleDeg - minAngleDeg;
int index = (int)Math.Round(relativeAngle / angularResolutionDeg);
// Clamp index to valid range
if (index >= 0 && index < expectedPoints)
{
double distanceM = block.DistanceRaw * distanceScaleToMeters;
// Store all distance values (no range filtering)
ranges[index] = distanceM;
intensities[index] = block.SignalStrength;
validCount++;
}
}
// Calculate angle increment from actual data
double angleIncrementRad = expectedPoints > 1
? (angleMaxRad - angleMinRad) / (expectedPoints - 1)
: angularResolutionDeg * Math.PI / 180.0; // Use calculated resolution
// Calculate scan timing (estimate based on rotation rate if available)
double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
? 1.0 / ScanFrequencyHz.Value
: 0.1; // Default 10 Hz
double timeIncrement = scanTime / expectedPoints;
return new LaserScan
{
Header = header,
AngleMin = angleMinRad, // From actual data
AngleMax = angleMaxRad, // From actual data
AngleIncrement = angleIncrementRad, // Calculated from data
TimeIncrement = timeIncrement,
ScanTime = scanTime,
RangeMin = minDistanceM, // From actual data
RangeMax = maxDistanceM, // From actual data
Ranges = ranges,
Intensities = intensities
};
}
#endregion
}

919
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiGS15Driver.cs Normal file
View File

@@ -0,0 +1,919 @@
using System.Buffers.Binary;
using System.Linq;
using System.Net;
using System.Net.NetworkInformation;
using System.Net.Sockets;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;
namespace RobotNet10.RobotApp.Drivers.Lidar;
/// <summary>
/// UDP connection for Olei GS1-5 (V3 protocol). Packet size 1136 bytes, not 1240.
/// </summary>
internal sealed class OleiGS15UdpConnection
{
public const int ExpectedPacketSize = 1136;
private readonly Queue<Gs15Packet> _packetQueue = new();
private readonly object _packetLock = new();
public string device_IP { get; set; } = "192.168.110.22";
public int device_port { get; set; } = 2468;
public string local_ip { get; set; } = "192.168.110.114";
public UdpClient? udp;
public IPEndPoint check_ip_device = new(IPAddress.Any, 0);
public bool openPort()
{
try
{
if (!IpExists(local_ip))
{
Console.WriteLine($"[ERROR] OleiGS15: local_ip {local_ip} does NOT exist on any NIC.");
return false;
}
if (IsPortBusy(device_port))
{
Console.WriteLine($"[ERROR] OleiGS15: Port {device_port} is already in use!");
return false;
}
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] OleiGS15 UDP port opened.");
return true;
}
catch (Exception ex)
{
Console.WriteLine($"[ERROR] OleiGS15: Cannot open UDP {local_ip}:{device_port} - {ex.Message}");
return false;
}
}
public void readRawdata()
{
if (udp == null) return;
try
{
byte[] data = udp.Receive(ref check_ip_device);
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)
return;
if (check_ip_device.Address.ToString() != device_IP)
return;
if (data.Length != ExpectedPacketSize)
return;
var packet = new Gs15Packet { stamp = DateTime.UtcNow, data = data };
lock (_packetLock)
{
_packetQueue.Enqueue(packet);
}
}
catch (SocketException ex)
{
if (ex.SocketErrorCode != SocketError.TimedOut)
Console.WriteLine($"[ERROR] OleiGS15 socket: {ex.Message}");
}
catch (Exception) { }
}
public Gs15Packet? TryDequeuePacket()
{
lock (_packetLock)
{
if (_packetQueue.Count == 0) return null;
return _packetQueue.Dequeue();
}
}
private static bool IpExists(string ip)
{
foreach (var ni in NetworkInterface.GetAllNetworkInterfaces())
{
foreach (var ua in ni.GetIPProperties().UnicastAddresses)
{
if (ua.Address.ToString() == ip) return true;
}
}
return false;
}
private static bool IsPortBusy(int port)
{
var listeners = IPGlobalProperties.GetIPGlobalProperties().GetActiveUdpListeners();
return listeners.Any(ep => ep.Port == port);
}
}
internal sealed class Gs15Packet
{
public DateTime stamp;
public byte[] data = Array.Empty<byte>();
}
public sealed class OleiGS15DriverConfig
{
public int Version { get; set; } = 3;
public string ScannerIp { get; set; } = "192.168.110.22";
public string LocalIp { get; set; } = "192.168.110.114";
public int Port { get; set; } = 2468;
public string Transport { get; set; } = "udp";
public string FrameId { get; set; } = "olelidar";
public string ScanTopic { get; set; } = "scan";
public bool Inverted { get; set; } = false;
public bool TimeFromLidar { get; set; } = true;
public bool Ntp { get; set; } = false;
public string PacketType { get; set; } = "B";
public double RangeMin { get; set; } = 0.08;
public double RangeMax { get; set; } = 50.0;
public bool AutoReconnectEnabled { get; set; } = true;
public int ReconnectDelayMs { get; set; } = 3000;
public int MaxReconnectAttempts { get; set; } = 0;
public static OleiGS15DriverConfig Parse(IConfigurationSection connection)
{
var cfg = new OleiGS15DriverConfig
{
Version = connection.GetValue<int?>("Version")
?? connection.GetValue<int?>("version")
?? 3,
ScannerIp = connection["ScannerIp"]
?? connection["scanner_ip"]
?? connection["DeviceIp"]
?? "192.168.110.22",
LocalIp = connection["LocalIp"]
?? connection["local_ip"]
?? "192.168.110.114",
Transport = connection["Transport"]
?? connection["transport"]
?? "udp",
FrameId = connection["FrameId"]
?? connection["frame_id"]
?? "olelidar",
ScanTopic = connection["ScanTopic"]
?? connection["scan_topic"]
?? "scan",
PacketType = connection["PacketType"]
?? connection["packet_type"]
?? "B",
Inverted = connection.GetValue<bool?>("Inverted")
?? connection.GetValue<bool?>("inverted")
?? false,
TimeFromLidar = connection.GetValue<bool?>("TimeFromLidar")
?? connection.GetValue<bool?>("timeFromLidar")
?? true,
Ntp = connection.GetValue<bool?>("Ntp")
?? connection.GetValue<bool?>("ntp")
?? false,
RangeMin = connection.GetValue<double?>("RangeMin")
?? connection.GetValue<double?>("range_min")
?? 0.08,
RangeMax = connection.GetValue<double?>("RangeMax")
?? connection.GetValue<double?>("range_max")
?? 50.0,
AutoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled") ?? true,
ReconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs") ?? 3000,
MaxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts") ?? 0
};
var port = connection.GetValue<int?>("Port")
?? connection.GetValue<int?>("port")
?? connection.GetValue<int?>("DevicePort")
?? 2368;
if (port is < 1 or > 65535)
{
throw new InvalidOperationException($"Invalid Port: {port}. Must be between 1 and 65535");
}
cfg.Port = port;
if (!string.Equals(cfg.Transport, "udp", StringComparison.OrdinalIgnoreCase))
{
throw new InvalidOperationException("OleiGS15Driver currently supports transport=udp only");
}
if (cfg.RangeMin < 0)
{
throw new InvalidOperationException($"RangeMin must be >= 0. Current value: {cfg.RangeMin}");
}
if (cfg.RangeMax <= cfg.RangeMin)
{
throw new InvalidOperationException(
$"RangeMax must be greater than RangeMin. RangeMin={cfg.RangeMin}, RangeMax={cfg.RangeMax}");
}
return cfg;
}
}
[Device(DeviceType.Lidar, "Olei", "OleiGS15Driver", "1.0.0",
Description = "Olei GS1-5 LiDAR driver (ROS version3-compatible UDP parser)")]
public class OleiGS15Driver : DeviceBase, ILidar
{
private const int V3HeaderSize = 48;
private const ushort V3Magic = 0xFEAC;
private readonly OleiGS15DriverConfig _config;
private readonly OleiGS15UdpConnection _connection = new();
private readonly Lock _dataLock = new();
private CancellationTokenSource? _updateCts;
private Task? _updateTask;
private LaserScan? _lastLaserScan;
private DateTime? _lastScanDataTimestamp;
private int _lastPointCount;
private double? _lastScanFrequencyHz;
private bool _portOpened;
private V3ScanAccumulator? _scanAccumulator;
private ushort? _lastFirstIndex;
public OleiGS15Driver(string deviceId, string deviceName, IConfigurationSection connection)
: base(deviceId, deviceName, DeviceType.Lidar)
{
_config = OleiGS15DriverConfig.Parse(connection);
_connection.device_IP = _config.ScannerIp;
_connection.local_ip = _config.LocalIp;
_connection.device_port = _config.Port;
AutoReconnectEnabled = _config.AutoReconnectEnabled;
ReconnectDelayMs = _config.ReconnectDelayMs;
MaxReconnectAttempts = _config.MaxReconnectAttempts;
UpdateProperties();
}
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
{
yield return new PropertyDescription("Version", "Version", "Phiên bản protocol")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 1,
Category = "Cấu hình",
DefaultValue = "3"
};
yield return new PropertyDescription("PacketType", "Packet Type", "Loại packet A/B/C")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 2,
Category = "Cấu hình",
DefaultValue = "B"
};
yield return new PropertyDescription("ScannerIp", "Scanner IP", "IP của Olei GS1-5")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 3,
Category = "Kết nối",
DefaultValue = ""
};
yield return new PropertyDescription("LocalIp", "Local IP", "IP local nhận UDP packet")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 4,
Category = "Kết nối",
DefaultValue = ""
};
yield return new PropertyDescription("Port", "Port", "UDP port của lidar")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 5,
Category = "Kết nối",
DefaultValue = "2368"
};
yield return new PropertyDescription("FrameId", "Frame ID", "Frame ID của LaserScan")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 6,
Category = "Cấu hình",
DefaultValue = "olelidar"
};
yield return new PropertyDescription("RangeMin", "Range Min (m)", "Khoảng cách tối thiểu")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 7,
Category = "Cấu hình",
DefaultValue = "0.08"
};
yield return new PropertyDescription("RangeMax", "Range Max (m)", "Khoảng cách tối đa")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 8,
Category = "Cấu hình",
DefaultValue = "50.0"
};
yield return new PropertyDescription("ConnectionStatus", "Connection Status", "Trạng thái kết nối lidar")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 9,
Category = "Trạng thái",
DefaultValue = "Disconnected"
};
yield return new PropertyDescription("PointCount", "Point Count", "Số điểm scan gần nhất")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 10,
Category = "Trạng thái",
DefaultValue = "0"
};
yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan gần nhất")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 11,
Category = "Trạng thái",
DefaultValue = ""
};
}
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
{
lock (_dataLock)
{
_portOpened = _connection.openPort();
if (!_portOpened)
{
throw new InvalidOperationException(
$"Failed to open Olei GS1-5 UDP port at {_config.LocalIp}:{_config.Port}");
}
}
await Task.CompletedTask;
}
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
{
StartUpdateLoop();
SetProperty("ConnectionStatus", "Connected");
await Task.CompletedTask;
}
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
{
StopUpdateLoop();
CloseUdpPort();
SetProperty("ConnectionStatus", "Disconnected");
await Task.CompletedTask;
}
protected override async Task OnResetAsync(CancellationToken cancellationToken)
{
lock (_dataLock)
{
_lastLaserScan = null;
_lastScanDataTimestamp = null;
_lastPointCount = 0;
_lastScanFrequencyHz = null;
_scanAccumulator = null;
_lastFirstIndex = null;
}
UpdateProperties();
await Task.CompletedTask;
}
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
lock (_dataLock)
{
if (!_portOpened)
{
return false;
}
if (_updateTask == null || _updateTask.IsCompleted)
{
return false;
}
if (_lastScanDataTimestamp == null)
{
return true;
}
return (DateTime.UtcNow - _lastScanDataTimestamp.Value).TotalSeconds < 5.0;
}
}
private void StartUpdateLoop()
{
lock (_dataLock)
{
if (_updateTask != null && !_updateTask.IsCompleted)
{
return;
}
_updateCts = new CancellationTokenSource();
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token), _updateCts.Token);
}
}
private void StopUpdateLoop()
{
Task? updateTask;
CancellationTokenSource? cts;
lock (_dataLock)
{
updateTask = _updateTask;
cts = _updateCts;
_updateTask = null;
_updateCts = null;
}
try
{
cts?.Cancel();
updateTask?.Wait(TimeSpan.FromSeconds(2));
}
catch
{
}
finally
{
cts?.Dispose();
}
}
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
{
while (!cancellationToken.IsCancellationRequested)
{
try
{
_connection.readRawdata();
var processedAnyPacket = false;
while (TryProcessOnePacket())
{
processedAnyPacket = true;
}
if (!processedAnyPacket)
{
await Task.Delay(5, cancellationToken);
}
}
catch (OperationCanceledException)
{
break;
}
catch (Exception ex)
{
LastError = ex;
OnErrorOccurred(ex, "OleiGS15 update loop error");
await Task.Delay(100, cancellationToken);
}
}
}
private bool TryProcessOnePacket()
{
var packet = _connection.TryDequeuePacket();
if (packet == null)
{
return false;
}
if (!TryParseV3Packet(packet.data, out var header, out var rangesMm, out var intensities, out var hasIntensity))
{
return true;
}
var packetTimestamp = _config.TimeFromLidar
? (packet.stamp.Kind == DateTimeKind.Utc ? packet.stamp : packet.stamp.ToUniversalTime())
: DateTime.UtcNow;
V3ScanAccumulator? completedScan = null;
lock (_dataLock)
{
if (_lastFirstIndex.HasValue && _lastFirstIndex.Value > header.FirstIndex)
{
_scanAccumulator = null;
}
_lastFirstIndex = header.FirstIndex;
if (_scanAccumulator == null)
{
_scanAccumulator = new V3ScanAccumulator(header, _config);
}
_scanAccumulator.AddPacketData(rangesMm, intensities, hasIntensity, _config);
if (_scanAccumulator.IsCompleted)
{
completedScan = _scanAccumulator;
_scanAccumulator = null;
}
}
if (completedScan != null)
{
PublishCompletedScan(completedScan, packetTimestamp);
}
return true;
}
private void PublishCompletedScan(V3ScanAccumulator completed, DateTime packetTimestamp)
{
var scanTime = completed.ScanTimeSeconds > 0 ? completed.ScanTimeSeconds : 0.1f;
var scanStamp = packetTimestamp - TimeSpan.FromSeconds(scanTime);
var laserScan = new LaserScan
{
Header = new RobotNet10.Shared.Header
{
Seq = 0,
Stamp = scanStamp,
FrameId = _config.FrameId
},
AngleMin = completed.AngleMin,
AngleMax = completed.AngleMax,
AngleIncrement = completed.AngleIncrement,
TimeIncrement = completed.TimeIncrement,
ScanTime = scanTime,
RangeMin = _config.RangeMin,
RangeMax = _config.RangeMax,
Ranges = Array.ConvertAll(completed.Ranges, x => (double)x),
Intensities = Array.ConvertAll(completed.Intensities, x => (double)x)
};
lock (_dataLock)
{
_lastLaserScan = laserScan;
_lastScanDataTimestamp = scanStamp;
_lastPointCount = completed.Ranges.Length;
_lastScanFrequencyHz = completed.ScanFrequencyHz > 0 ? completed.ScanFrequencyHz : null;
}
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
UpdateProperties();
}
private static bool TryParseV3Packet(
byte[] data,
out V3HeaderLite header,
out ushort[] rangesMm,
out ushort[] intensities,
out bool hasIntensity)
{
header = default;
rangesMm = Array.Empty<ushort>();
intensities = Array.Empty<ushort>();
hasIntensity = false;
if (data.Length < V3HeaderSize)
{
return false;
}
var magic = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(0, 2));
if (magic != V3Magic)
{
return false;
}
header = new V3HeaderLite
{
Magic = magic,
Version = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(2, 2)),
PacketSize = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(4, 4)),
HeaderSize = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(8, 2)),
DistanceRatio = data[10],
Types = data[11],
ScanNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(12, 2)),
PacketNumber = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(14, 2)),
TimestampDecimal = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(16, 4)),
TimestampInteger = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(20, 4)),
ScanFrequencyRaw = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(24, 2)),
NumPointsScan = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(26, 2)),
InputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(28, 2)),
OutputStatus = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(30, 2)),
FieldStatus = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(32, 4)),
StartIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(36, 2)),
EndIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(38, 2)),
FirstIndex = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(40, 2)),
NumPointsPacket = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(42, 2)),
StatusFlags = BinaryPrimitives.ReadUInt32LittleEndian(data.AsSpan(44, 4))
};
var packetSize = header.PacketSize == 0 ? data.Length : (int)Math.Min(header.PacketSize, data.Length);
var headerSize = header.HeaderSize == 0 ? V3HeaderSize : header.HeaderSize;
if (headerSize < V3HeaderSize || headerSize > packetSize)
{
return false;
}
var bytesPerPoint = header.Types switch
{
0x00 => 2,
0x01 => 4,
0x10 => 4,
_ => 0
};
if (bytesPerPoint == 0)
{
return false;
}
var payloadBytes = packetSize - headerSize;
var pointCount = header.NumPointsPacket;
if (pointCount == 0 || pointCount * bytesPerPoint > payloadBytes)
{
pointCount = (ushort)(payloadBytes / bytesPerPoint);
}
if (pointCount == 0)
{
return false;
}
rangesMm = new ushort[pointCount];
if (header.Types == 0x01)
{
hasIntensity = true;
intensities = new ushort[pointCount];
}
var offset = headerSize;
for (int i = 0; i < pointCount; i++)
{
switch (header.Types)
{
case 0x00:
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
offset += 2;
break;
case 0x01:
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset, 2));
intensities[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
offset += 4;
break;
case 0x10:
rangesMm[i] = BinaryPrimitives.ReadUInt16LittleEndian(data.AsSpan(offset + 2, 2));
offset += 4;
break;
}
}
return true;
}
private void UpdateProperties()
{
DateTime? timestamp;
int pointCount;
lock (_dataLock)
{
timestamp = _lastScanDataTimestamp;
pointCount = _lastPointCount;
}
SetProperty("Version", _config.Version.ToString());
SetProperty("PacketType", _config.PacketType);
SetProperty("ScannerIp", _config.ScannerIp);
SetProperty("LocalIp", _config.LocalIp);
SetProperty("Port", _config.Port.ToString());
SetProperty("FrameId", _config.FrameId);
SetProperty("RangeMin", _config.RangeMin.ToString("F3"));
SetProperty("RangeMax", _config.RangeMax.ToString("F3"));
SetProperty("PointCount", pointCount.ToString());
SetProperty("LastScanTime", timestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
}
private void CloseUdpPort()
{
lock (_dataLock)
{
_portOpened = false;
try
{
_connection.udp?.Close();
}
catch
{
}
try
{
_connection.udp?.Dispose();
}
catch
{
}
}
}
public LaserScan? CurrentMeasurementData
{
get
{
lock (_dataLock)
{
return _lastLaserScan;
}
}
}
public DateTime? LastScanDataTimestamp
{
get
{
lock (_dataLock)
{
return _lastScanDataTimestamp;
}
}
}
public double MinAngleRad
{
get
{
lock (_dataLock)
{
return _lastLaserScan?.AngleMin ?? -Math.PI;
}
}
}
public double MaxAngleRad
{
get
{
lock (_dataLock)
{
return _lastLaserScan?.AngleMax ?? Math.PI;
}
}
}
public double MinRangeM => _config.RangeMin;
public double MaxRangeM => _config.RangeMax;
public double? AngularResolutionRad
{
get
{
lock (_dataLock)
{
var scan = _lastLaserScan;
if (scan?.Ranges == null || scan.Value.Ranges.Length < 2)
{
return null;
}
return scan.Value.AngleIncrement;
}
}
}
public double? ScanFrequencyHz
{
get
{
lock (_dataLock)
{
return _lastScanFrequencyHz;
}
}
}
public double FieldOfViewRad => Math.Abs(MaxAngleRad - MinAngleRad);
public bool SupportsIntensity => true;
public double? AccuracyM => null;
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
protected override void Dispose(bool disposing)
{
if (disposing)
{
StopUpdateLoop();
CloseUdpPort();
}
base.Dispose(disposing);
}
private readonly struct V3HeaderLite
{
public ushort Magic { get; init; }
public ushort Version { get; init; }
public uint PacketSize { get; init; }
public ushort HeaderSize { get; init; }
public byte DistanceRatio { get; init; }
public byte Types { get; init; }
public ushort ScanNumber { get; init; }
public ushort PacketNumber { get; init; }
public uint TimestampDecimal { get; init; }
public uint TimestampInteger { get; init; }
public ushort ScanFrequencyRaw { get; init; }
public ushort NumPointsScan { get; init; }
public ushort InputStatus { get; init; }
public ushort OutputStatus { get; init; }
public uint FieldStatus { get; init; }
public ushort StartIndex { get; init; }
public ushort EndIndex { get; init; }
public ushort FirstIndex { get; init; }
public ushort NumPointsPacket { get; init; }
public uint StatusFlags { get; init; }
}
private sealed class V3ScanAccumulator
{
private int _writeIndex;
public float[] Ranges { get; }
public float[] Intensities { get; }
public bool IsCompleted => _writeIndex >= Ranges.Length;
public float AngleMin { get; }
public float AngleMax { get; }
public float AngleIncrement { get; }
public float TimeIncrement { get; }
public float ScanTimeSeconds { get; }
public double ScanFrequencyHz { get; }
public V3ScanAccumulator(V3HeaderLite header, OleiGS15DriverConfig config)
{
var actualNum = Math.Max(1, (int)(header.EndIndex - header.StartIndex));
var pointsPerScan = Math.Max(1, (int)header.NumPointsScan);
Ranges = new float[actualNum];
for (int i = 0; i < Ranges.Length; i++)
{
Ranges[i] = 0.0f;
}
Intensities = new float[actualNum];
AngleIncrement = (float)((360.0 / pointsPerScan) * Math.PI / 180.0);
AngleMin = (float)(header.StartIndex * AngleIncrement - Math.PI);
AngleMax = (float)(header.EndIndex * AngleIncrement - Math.PI);
var rpm = header.ScanFrequencyRaw & 0x7FFF;
ScanFrequencyHz = rpm > 0 ? rpm / 60.0 : 0.0;
ScanTimeSeconds = rpm > 0 ? (float)(60.0 / rpm) : 0.1f;
TimeIncrement = ScanTimeSeconds / pointsPerScan;
}
public void AddPacketData(ushort[] rangesMm, ushort[] intensities, bool hasIntensity, OleiGS15DriverConfig config)
{
for (int i = 0; i < rangesMm.Length && _writeIndex < Ranges.Length; i++)
{
var rangeM = rangesMm[i] / 1000.0f;
if (rangeM < config.RangeMin || rangeM > config.RangeMax || rangeM <= 0)
{
Ranges[_writeIndex] = 0.0f;
}
else
{
Ranges[_writeIndex] = rangeM;
}
if (hasIntensity && i < intensities.Length)
{
Intensities[_writeIndex] = intensities[i];
}
else
{
Intensities[_writeIndex] = 0.0f;
}
_writeIndex++;
}
}
}
}

374
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarDriver.cs Normal file
View File

@@ -0,0 +1,374 @@
using Microsoft.AspNetCore.Mvc.Diagnostics;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Sensor;
using System;
using System.Net;
using System.Net.Sockets;
//using static RobotNet10.RobotApp.Drivers.Lidar.OleiLidarDecode;
namespace RobotNet10.RobotApp.Drivers.Lidar
{
[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
public class OleiLidarDriver : DeviceBase, ILidar
{
private readonly object _scanDataLock = new();
// Cached scan data
//private LidarMeasurementData? _cachedMeasurementData = null;
// Device specifications
private readonly double _minRangeM;
private readonly double _maxRangeM;
private readonly double? _angularResolutionRad;
private readonly double? _scanFrequencyHz;
private readonly bool _supportsIntensity;
private readonly double? _accuracyM;
private readonly int _numberOfPoints;
private readonly double _startAngleRad;
private readonly double _endAngleRad;
private DateTime? _lastScanDataTimestamp;
private LaserScan? _lastLaserScan;
private oleiPackage? _pkt;
Connect oleiConnect = new Connect();
DecodeLidar oleiDecode = new DecodeLidar();
DecoderConfig decoderConfig = new DecoderConfig();
private readonly Lock _dataLock = new();
private CancellationTokenSource? _updateCts;
private Task? _updateTask;
public float maxAngle;
public float minAngle;
public UdpClient udp;
public IPEndPoint check_ip_device;
bool portOpened;
//Event
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
//Cached data
//private LidarMeasurementData? _currentMeasurementData;
public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
: base(deviceId, deviceName, DeviceType.Lidar)
{
// Đọc cấu hình kết nối (IP và Port)
string deviceIp = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
int devicePort = connection.GetValue<int?>("DevicePort") ?? 2368;
string localIp = connection.GetValue<string>("LocalIp") ?? "192.168.254.10";
// Cấu hình Connect object với IP và Port từ config
oleiConnect.device_IP = deviceIp;
oleiConnect.device_port = devicePort;
oleiConnect.local_ip = localIp;
// Đọc cấu hình
minAngle = connection.GetValue<float?>("MinAngle") ?? -130.0f;
maxAngle = connection.GetValue<float?>("MaxAngle") ?? 130.0f;
_maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
_minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.1; // Default 20cm
_scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz") ?? 10.0; // Default 10Hz
_supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
_accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02; // Default 2cm accuracy
// Set decoder config from JSON
decoderConfig.Inverted = connection.GetValue<bool?>("Inverted") ?? false;
// Apply config to decoder
oleiDecode.SetConfig(decoderConfig);
var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
if (autoReconnectEnabled.HasValue)
AutoReconnectEnabled = autoReconnectEnabled.Value;
else
AutoReconnectEnabled = false; // Không cần reconnect cho simulation
if (reconnectDelayMs.HasValue)
ReconnectDelayMs = reconnectDelayMs.Value;
if (maxReconnectAttempts.HasValue)
MaxReconnectAttempts = maxReconnectAttempts.Value;
// Khởi tạo giá trị properties
UpdateProperties();
}
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
{
yield return new PropertyDescription("NumberOfPoints", "Number of Points", "Số điểm scan")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 1,
Category = "Cấu hình",
DefaultValue = "2000"
};
yield return new PropertyDescription("StartAngle", "Start Angle (deg)", "Góc bắt đầu (độ)")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 2,
Category = "Cấu hình",
DefaultValue = "-180"
};
yield return new PropertyDescription("EndAngle", "End Angle (deg)", "Góc kết thúc (độ)")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 3,
Category = "Cấu hình",
DefaultValue = "180"
};
yield return new PropertyDescription("MaxRange", "Max Range (m)", "Tầm quét tối đa (m)")
{
DataType = "number",
IsReadOnly = true,
DisplayOrder = 4,
Category = "Cấu hình",
DefaultValue = "20"
};
yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan cuối cùng")
{
DataType = "string",
IsReadOnly = true,
DisplayOrder = 5,
Category = "Trạng thái",
DefaultValue = ""
};
}
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
{
// Mở cổng UDP - kiểm tra kết quả và throw exception nếu fail
lock (_dataLock)
{
portOpened = oleiConnect.openPort();
}
await Task.CompletedTask;
}
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
{
// Bắt đầu vòng lặp cập nhật dữ liệu với tần số 1Hz
StartUpdateLoop();
await Task.CompletedTask;
}
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
{
// Dừng vòng lặp cập nhật
StopUpdateLoop();
await Task.CompletedTask;
}
protected override async Task OnResetAsync(CancellationToken cancellationToken)
{
// Reset về giá trị mặc định
lock (_dataLock)
{
_lastLaserScan = null;
_lastScanDataTimestamp = null;
_pkt = null;
}
UpdateProperties();
await Task.CompletedTask;
}
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
// Simulation luôn connected
if (!portOpened)
{
return await Task.FromResult(false);
}
return await Task.FromResult(true);
}
private void StartUpdateLoop()
{
lock (_dataLock)
{
if (_updateTask != null && !_updateTask.IsCompleted)
return;
_updateCts = new CancellationTokenSource();
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
}
}
private void StopUpdateLoop()
{
lock (_dataLock)
{
_updateCts?.Cancel();
_updateCts?.Dispose();
_updateCts = null;
_updateTask = null;
}
}
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
{
//_connect.openPort();
//Console.WriteLine("[INFO] START READ DATA");
while (!cancellationToken.IsCancellationRequested)
{
try
{
// Đọc dữ liệu từ UDP
lock (_dataLock)
{
oleiConnect.readRawdata();
if (oleiConnect.GetPacketCount() > 0)
{
GenerateScanData();
}
}
}
catch (OperationCanceledException)
{
break;
}
catch (Exception ex)
{
LastError = ex;
OnErrorOccurred(ex, "Update loop error");
await Task.Delay(1000, cancellationToken);
}
}
}
private void GenerateScanData()
{
// Dequeue packet an toàn (thread-safe)
oleiPackage? pkt = oleiConnect.TryDequeuePacket();
if (pkt == null)
{
// Không log để tránh spam khi queue rỗng
return;
}
var timestamp = DateTime.UtcNow;
// Decode packet
oleiDecode.PacketCb(pkt);
// Kiểm tra xem có dữ liệu không
if (oleiDecode.scanRadAngleInVec.Count == 0 ||
oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanRangeInVec.Count ||
oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanIntensityInVec.Count)
{
// Không có dữ liệu hoặc dữ liệu không đồng bộ
return;
}
var numberOfPoints = oleiDecode.scanRadAngleInVec.Count;
var ranges = new double[numberOfPoints];
var intensities = new double[numberOfPoints];
// Convert data to arrays (LaserScan format)
for(int i = 0; i < numberOfPoints; i++)
{
if((oleiDecode.scanAngleInVec[i] >= (minAngle + 180)) && (oleiDecode.scanAngleInVec[i] <= (maxAngle + 180)))
{
var distanceM = oleiDecode.scanRangeInVec[i] * 0.001f; // mm to meters
var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
ranges[i] = isValid ? distanceM : 0.0f;
intensities[i] = oleiDecode.scanIntensityInVec[i]; // normalize 0-1
}
}
float _minAngle = (float)(minAngle*Math.PI)/180;
float _maxAngle = (float)(maxAngle*Math.PI)/180;
float angleIncrement = numberOfPoints > 1 ?
(_maxAngle - _minAngle) / (numberOfPoints - 1) : 0.0f;
float _scanTime = 1.0f / oleiDecode.Frequency;
float _TimeIncrement = _scanTime / numberOfPoints;
var laserScan = new LaserScan
{
Header = new RobotNet10.Shared.Header
{
Seq = 0,
Stamp = timestamp,
FrameId = "olei_lidar_frame"
},
AngleMin = _minAngle,
AngleMax = _maxAngle,
AngleIncrement = angleIncrement,
TimeIncrement = _TimeIncrement,
ScanTime = _scanTime, // Convert Hz to seconds
RangeMin = (float)_minRangeM,
RangeMax = (float)_maxRangeM,
Ranges = ranges,
Intensities = intensities
};
// Cache LaserScan và packet
lock (_dataLock)
{
_lastLaserScan = laserScan;
_lastScanDataTimestamp = timestamp;
_pkt = pkt;
}
// Fire event
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(timestamp: timestamp, measurementData: laserScan));
UpdateProperties();
}
private void UpdateProperties()
{
lock (_dataLock)
{
SetProperty("NumberOfPoints", _numberOfPoints.ToString());
SetProperty("StartAngle", (_startAngleRad * 180.0 / Math.PI).ToString("F1"));
SetProperty("EndAngle", (_endAngleRad * 180.0 / Math.PI).ToString("F1"));
SetProperty("MaxRange", _maxRangeM.ToString("F1"));
SetProperty("LastScanTime", _lastScanDataTimestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
}
}
#region ILidar Implementation
public LaserScan? CurrentMeasurementData
{
get { lock (_dataLock) { return _lastLaserScan; } }
}
public DateTime? LastScanDataTimestamp
{
get { lock (_dataLock) { return _lastScanDataTimestamp; } }
}
// ILidar Device Specifications
public double MinAngleRad => _startAngleRad;
public double MaxAngleRad => _endAngleRad;
public double MinRangeM => _minRangeM;
public double MaxRangeM => _maxRangeM;
public double? AngularResolutionRad => _angularResolutionRad;
public double? ScanFrequencyHz => _scanFrequencyHz;
public double FieldOfViewRad => Math.Abs(_endAngleRad - _startAngleRad);
public bool SupportsIntensity => _supportsIntensity;
public double? AccuracyM => _accuracyM;
#endregion
protected override void Dispose(bool disposing)
{
if (disposing)
{
StopUpdateLoop();
}
base.Dispose(disposing);
}
}
}

519
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/Olei/OleiLidarHandler.cs Normal file
View File

@@ -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;
}
}