QUYVN/BQP
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-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -0,0 +1,562 @@
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>
/// Whether the LiDAR is mounted inverted (upside down).
/// When true, scan angles are mirrored (180° - angle) to correct orientation.
/// </summary>
public bool Inverted { get; set; } = false;
}
/// <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;
private DateTime _currentScanStartTime = DateTime.UtcNow;
/// <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)
{
await Task.Delay(500, cancellationToken);
if (_lidarServer == null || !_lidarServer.IsRunning)
return false;
lock (_accumulationLock)
{
return _lastScanDataTimestamp.HasValue;
}
}
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
LaserScan? completedScan = null;
bool updateProperties = false;
lock (_accumulationLock)
{
// Detect scan completion: angle wrapped back to near 0° after being > 300°
if (_lastPacketStartAngle > 300.0 && packetStartAngle < 50.0 && _accumulatedBlocks.Count > 0)
{
var scan = CreateLaserScanFromAccumulated(packet.Header, _currentScanStartTime);
completedScan = scan;
_currentMeasurementData = scan;
_lastScanDataTimestamp = scan.Header.Stamp;
Interlocked.Increment(ref _scansGeneratedInCurrentSecond);
UpdateScanFrequency();
if (_frequencyUpdateStopwatch.ElapsedMilliseconds > 1000)
{
updateProperties = true;
_frequencyUpdateStopwatch.Restart();
}
_accumulatedBlocks.Clear();
_currentScanStartTime = DateTime.UtcNow;
}
else if (_accumulatedBlocks.Count == 0)
{
_currentScanStartTime = DateTime.UtcNow;
}
// Add current packet's blocks to accumulation buffer
for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
{
var block = packet.DataBlocks[i];
if (block.IsValid)
{
double angleDeg = block.GetAngleDegrees();
if (angleDeg >= 360.0)
angleDeg %= 360.0;
_accumulatedBlocks.Add(block);
}
}
_lastPacketStartAngle = packetStartAngle;
}
// Fire event and update properties outside the lock to avoid blocking UDP reception
if (completedScan is { } publishedScan)
{
if (updateProperties)
{
SetProperty("LastScanTime", publishedScan.Header.Stamp.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}°");
}
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(
publishedScan.Header.Stamp,
publishedScan
));
}
}
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, DateTime scanStartTime)
{
var header = new Header(
seq: _scanSequenceNumber++,
stamp: scanStartTime,
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)
{
double angleDeg = _config.Inverted
? (block.GetAngleDegrees() + 180.0) % 360.0
: 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;
if (distanceM <= 0 || distanceM >= HARDWARE_MAX_RANGE_M)
{
ranges[index] = -1.0; // No detection / max-range return
intensities[index] = 0.0;
continue;
}
// 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 = HARDWARE_MIN_RANGE_M,
RangeMax = HARDWARE_MAX_RANGE_M,
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++;
}
}
}
}

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

@@ -0,0 +1,390 @@
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Sensor;
namespace RobotNet10.RobotApp.Drivers.Lidar;
/// <summary>
/// Olei 2D LiDAR driver (LR-1BS5 / olelidar ROS protocol, 1240-byte UDP packets).
/// </summary>
[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
public class OleiLidarDriver : DeviceBase, ILidar
{
private readonly Connect _connect = new();
private readonly DecodeLidar _decode = new();
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 double _startAngleRad;
private readonly double _endAngleRad;
private readonly string _frameId;
private readonly int _angleMinHundredths;
private readonly int _angleMaxHundredths;
private readonly int _poly;
private DateTime? _lastScanDataTimestamp;
private LaserScan? _lastLaserScan;
private bool _portOpened;
private readonly Lock _dataLock = new();
private CancellationTokenSource? _updateCts;
private Task? _updateTask;
private uint _scanSequence;
public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;
public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
: base(deviceId, deviceName, DeviceType.Lidar)
{
_connect.device_IP = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
_connect.device_port = connection.GetValue<int?>("DevicePort")
?? connection.GetValue<int?>("UdpPort")
?? 2368;
_connect.local_ip = connection.GetValue<string>("LocalIp") ?? string.Empty;
_connect.multiaddr_ip = connection.GetValue<string>("Multiaddr") ?? string.Empty;
var minAngleDeg = connection.GetValue<double?>("MinAngle") ?? -135.0;
var maxAngleDeg = connection.GetValue<double?>("MaxAngle") ?? 135.0;
_startAngleRad = minAngleDeg * Math.PI / 180.0;
_endAngleRad = maxAngleDeg * Math.PI / 180.0;
_angleMinHundredths = (int)(minAngleDeg * 100 + 18000);
_angleMaxHundredths = (int)(maxAngleDeg * 100 + 18000);
_minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.2;
_maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
_scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz");
_supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
_accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02;
_frameId = connection.GetValue<string>("FrameId") ?? "olei_lidar_frame";
_poly = connection.GetValue<int?>("Poly") ?? 1;
var stepDeg = connection.GetValue<double?>("StepDeg") ?? 0.225;
_angularResolutionRad = stepDeg * Math.PI / 180.0;
var decoderConfig = new DecoderConfig
{
AngleMin = minAngleDeg,
AngleMax = maxAngleDeg,
RangeMin = _minRangeM,
RangeMax = _maxRangeM,
Poly = _poly,
Inverted = connection.GetValue<bool?>("Inverted") ?? false,
StepDeg = stepDeg,
FrameId = _frameId
};
_decode.SetConfig(decoderConfig);
if (connection.GetValue<bool?>("AutoReconnectEnabled") is bool autoReconnect)
AutoReconnectEnabled = autoReconnect;
else
AutoReconnectEnabled = true;
if (connection.GetValue<int?>("ReconnectDelayMs") is int reconnectDelay)
ReconnectDelayMs = reconnectDelay;
if (connection.GetValue<int?>("MaxReconnectAttempts") is int maxAttempts)
MaxReconnectAttempts = maxAttempts;
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 = "0"
};
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 = (_startAngleRad * 180.0 / Math.PI).ToString("F1")
};
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 = (_endAngleRad * 180.0 / Math.PI).ToString("F1")
};
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 = _maxRangeM.ToString("F1")
};
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 Task OnInitializeAsync(CancellationToken cancellationToken)
{
lock (_dataLock)
{
_portOpened = _connect.openPort();
if (!_portOpened)
throw new InvalidOperationException(
$"Failed to open Olei UDP port {_connect.device_port} on local {_connect.local_ip}");
}
return Task.CompletedTask;
}
protected override Task OnConnectAsync(CancellationToken cancellationToken)
{
StartUpdateLoop();
return Task.CompletedTask;
}
protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
{
StopUpdateLoop();
return Task.CompletedTask;
}
protected override Task OnResetAsync(CancellationToken cancellationToken)
{
lock (_dataLock)
{
_lastLaserScan = null;
_lastScanDataTimestamp = null;
_scanSequence = 0;
}
UpdateProperties();
return Task.CompletedTask;
}
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
{
if (!_portOpened || !_connect.IsOpen)
return false;
// LiDAR may need a few seconds after motor start before UDP data arrives.
for (var i = 0; i < 40; i++)
{
if (_lastScanDataTimestamp.HasValue)
return true;
if (_connect.TotalPacketsReceived > 0 || _connect.GetPacketCount() > 0)
return true;
await Task.Delay(250, cancellationToken);
}
Console.WriteLine(
$"[WARN] Olei LiDAR connection check failed: packets={_connect.TotalPacketsReceived}, " +
$"queued={_connect.GetPacketCount()}, wrongSrc={_connect.DroppedWrongSource}, " +
$"wrongSize={_connect.DroppedWrongSize}, device={_connect.device_IP}:{_connect.device_port}");
return false;
}
private void StartUpdateLoop()
{
lock (_dataLock)
{
if (_updateCts != null)
return;
_updateCts = new CancellationTokenSource();
_updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
}
}
private void StopUpdateLoop()
{
CancellationTokenSource? cts;
lock (_dataLock)
{
cts = _updateCts;
_updateCts = null;
_updateTask = null;
}
cts?.Cancel();
cts?.Dispose();
}
private async Task UpdateLoopAsync(CancellationToken cancellationToken)
{
while (!cancellationToken.IsCancellationRequested)
{
try
{
var framePublished = false;
while (_connect.TryDequeuePacket() is { } pkt)
{
if (_decode.PacketCb(pkt))
framePublished |= TryPublishScan(pkt.stamp);
}
if (!framePublished)
await Task.Delay(1, cancellationToken);
}
catch (OperationCanceledException)
{
break;
}
catch (Exception ex)
{
LastError = ex;
OnErrorOccurred(ex, "Olei LiDAR update loop error");
await Task.Delay(500, cancellationToken);
}
}
}
private bool TryPublishScan(DateTime packetReceiveStamp)
{
var angles = _decode.scanAngleInVec;
var rangesMm = _decode.scanRangeInVec;
var intensitiesRaw = _decode.scanIntensityInVec;
if (angles.Count == 0 ||
angles.Count != rangesMm.Count ||
angles.Count != intensitiesRaw.Count)
return false;
var rangeList = new List<double>();
var intensityList = new List<double>();
var angleRadList = new List<double>();
for (var i = 0; i < angles.Count; i++)
{
if (i % _poly != 0)
continue;
var angleHundredths = angles[i];
if (angleHundredths < _angleMinHundredths || angleHundredths > _angleMaxHundredths)
continue;
var angleDeg = (angleHundredths - 18000) / 100.0;
var angleRad = angleDeg * Math.PI / 180.0;
angleRadList.Add(angleRad);
var distanceM = rangesMm[i] * OleiConstants.DistanceResolution;
var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
rangeList.Add(isValid ? distanceM : double.NaN);
intensityList.Add(intensitiesRaw[i]);
}
if (rangeList.Count == 0)
return false;
var frequency = _decode.Frequency > 0.001f
? _decode.Frequency
: (float)(_scanFrequencyHz ?? 10.0);
var scanTime = 1.0 / frequency;
var numberOfPoints = rangeList.Count;
var angleMin = angleRadList[0];
var angleMax = angleRadList[^1];
var angleIncrement = numberOfPoints > 1
? (angleMax - angleMin) / (numberOfPoints - 1)
: _decode.StepDeg * Math.PI / 180.0;
var receiveUtc = packetReceiveStamp.Kind == DateTimeKind.Utc
? packetReceiveStamp
: packetReceiveStamp.ToUniversalTime();
var lidarEndUtc = _decode.LastCompletedScanLidarEndUtc ?? receiveUtc;
var scanStamp = lidarEndUtc - TimeSpan.FromSeconds(scanTime);
var laserScan = new LaserScan
{
Header = new RobotNet10.Shared.Header
{
Seq = _scanSequence++,
Stamp = scanStamp,
FrameId = _frameId
},
AngleMin = angleMin,
AngleMax = angleMax,
AngleIncrement = angleIncrement,
TimeIncrement = scanTime / numberOfPoints,
ScanTime = scanTime,
RangeMin = (float)_minRangeM,
RangeMax = (float)_maxRangeM,
Ranges = rangeList.ToArray(),
Intensities = intensityList.ToArray()
};
lock (_dataLock)
{
_lastLaserScan = laserScan;
_lastScanDataTimestamp = scanStamp;
}
ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
UpdateProperties();
return true;
}
private void UpdateProperties()
{
lock (_dataLock)
{
var pointCount = _lastLaserScan?.Ranges.Length ?? 0;
SetProperty("NumberOfPoints", pointCount.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
public LaserScan? CurrentMeasurementData
{
get { lock (_dataLock) { return _lastLaserScan; } }
}
public DateTime? LastScanDataTimestamp
{
get { lock (_dataLock) { return _lastScanDataTimestamp; } }
}
public double MinAngleRad => _startAngleRad;
public double MaxAngleRad => _endAngleRad;
public double MinRangeM => _minRangeM;
public double MaxRangeM => _maxRangeM;
public double? AngularResolutionRad => _angularResolutionRad;
public double? ScanFrequencyHz => _decode.Frequency > 0.001f ? _decode.Frequency : _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();
_connect.Dispose();
}
base.Dispose(disposing);
}
}

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

@@ -0,0 +1,437 @@
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
namespace RobotNet10.RobotApp.Drivers.Lidar;
/// <summary>
/// UDP transport + packet decode aligned with olelidar ROS driver (driver.cpp / decoder.cpp).
/// </summary>
public sealed class Connect : IDisposable
{
public const int kPacketSize = OleiConstants.PacketSize;
public string device_IP { get; set; } = "192.168.254.13";
public int device_port { get; set; } = 2368;
public string local_ip { get; set; } = "192.168.254.10";
public string multiaddr_ip { get; set; } = string.Empty;
private readonly Queue<oleiPackage> _packetList = new();
private readonly object _packetListLock = new();
private UdpClient? _udp;
private Thread? _receiveThread;
private volatile bool _running;
private IPAddress? _deviceIpAddress;
private long _totalPacketsReceived;
private long _droppedWrongSource;
private long _droppedWrongSize;
public long TotalPacketsReceived => Interlocked.Read(ref _totalPacketsReceived);
public long DroppedWrongSource => Interlocked.Read(ref _droppedWrongSource);
public long DroppedWrongSize => Interlocked.Read(ref _droppedWrongSize);
public bool IsOpen => _udp != null && _running;
public bool openPort()
{
try
{
_deviceIpAddress = IPAddress.Parse(device_IP).MapToIPv4();
var bindAddress = string.IsNullOrWhiteSpace(local_ip)
? IPAddress.Any
: IPAddress.Parse(local_ip).MapToIPv4();
var bindEp = new IPEndPoint(bindAddress, device_port);
_udp = new UdpClient();
_udp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
_udp.Client.Bind(bindEp);
_udp.Client.ReceiveTimeout = 500;
if (!string.IsNullOrWhiteSpace(multiaddr_ip))
{
try
{
_udp.JoinMulticastGroup(IPAddress.Parse(multiaddr_ip));
}
catch (Exception ex)
{
Console.WriteLine($"[WARN] Olei multicast join failed ({multiaddr_ip}): {ex.Message}");
}
}
StartReceiveLoop();
Console.WriteLine($"[INFO] Olei UDP listening on {bindEp.Address}:{bindEp.Port}, device {device_IP}");
return true;
}
catch (Exception ex)
{
Console.WriteLine($"[ERROR] Cannot open Olei UDP port: {ex.Message}");
return false;
}
}
public void closePort()
{
_running = false;
try
{
_receiveThread?.Join(TimeSpan.FromSeconds(2));
}
catch { /* ignore */ }
_receiveThread = null;
_udp?.Close();
_udp?.Dispose();
_udp = null;
}
private void StartReceiveLoop()
{
_running = true;
_receiveThread = new Thread(ReceiveLoop)
{
IsBackground = true,
Name = $"OleiLidar-UDP-{device_port}",
Priority = ThreadPriority.Highest
};
_receiveThread.Start();
}
private void ReceiveLoop()
{
while (_running && _udp != null)
{
try
{
var remote = new IPEndPoint(IPAddress.Any, 0);
var data = _udp.Receive(ref remote);
var senderIp = remote.Address.MapToIPv4();
if (_deviceIpAddress != null && !senderIp.Equals(_deviceIpAddress))
{
Interlocked.Increment(ref _droppedWrongSource);
continue;
}
if (data.Length < kPacketSize)
{
Interlocked.Increment(ref _droppedWrongSize);
continue;
}
Interlocked.Increment(ref _totalPacketsReceived);
var packet = new oleiPackage
{
stamp = DateTime.UtcNow,
data = new byte[kPacketSize]
};
Array.Copy(data, packet.data, kPacketSize);
lock (_packetListLock)
{
_packetList.Enqueue(packet);
}
}
catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
{
// Normal when no packets (ROS poll timeout).
}
catch (SocketException ex) when (!_running)
{
break;
}
catch (ObjectDisposedException)
{
break;
}
catch (Exception ex)
{
if (_running)
Console.WriteLine($"[ERROR] Olei UDP receive: {ex.Message}");
}
}
}
public int GetPacketCount()
{
lock (_packetListLock)
return _packetList.Count;
}
public oleiPackage? TryDequeuePacket()
{
lock (_packetListLock)
return _packetList.Count == 0 ? null : _packetList.Dequeue();
}
public void Dispose() => closePort();
}
public sealed class oleiPackage
{
public DateTime stamp { get; set; }
public byte[] data { get; set; } = new byte[OleiConstants.PacketSize];
}
public sealed class DecoderConfig
{
public double AngleMin { get; set; } = 0.0;
public double AngleMax { get; set; } = 360.0;
public double RangeMin { get; set; } = 0.2;
public double RangeMax { get; set; } = 30.0;
public int Poly { get; set; } = 1;
public bool Inverted { get; set; }
public double StepDeg { get; set; } = 0.225;
public string FrameId { get; set; } = "olelidar";
}
public static class OleiConstants
{
public const int DataHeadSize = 40;
public const int PointBytes = 8;
public const int BlocksPerPacket = 150;
public const int PacketSize = DataHeadSize + BlocksPerPacket * PointBytes;
public const float DistanceResolution = 0.001f;
public const float AzimuthResolutionDeg = 0.01f;
}
public readonly struct DataPoint
{
public readonly ushort Azimuth;
public readonly ushort Distance;
public readonly ushort Reflectivity;
public DataPoint(ReadOnlySpan<byte> span)
{
Azimuth = BinaryPrimitives.ReadUInt16LittleEndian(span);
Distance = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(2));
Reflectivity = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(4));
}
}
public readonly struct DataBlock
{
public readonly DataPoint Point;
public DataBlock(ReadOnlySpan<byte> span) => Point = new DataPoint(span);
}
public readonly struct DataHeader
{
public readonly byte[] Code;
public readonly uint Timestamp;
public readonly ushort Rpm;
public readonly uint Rsv;
public DataHeader(ReadOnlySpan<byte> span)
{
Code = span.Slice(22, 2).ToArray();
Timestamp = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(28));
Rpm = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(32));
Rsv = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(36));
}
}
public readonly struct Packet
{
public readonly DataHeader Header;
public readonly DataBlock[] Blocks;
public Packet(ReadOnlySpan<byte> span)
{
if (span.Length < OleiConstants.PacketSize)
throw new ArgumentException($"packet must be {OleiConstants.PacketSize} bytes");
Header = new DataHeader(span.Slice(0, OleiConstants.DataHeadSize));
Blocks = new DataBlock[OleiConstants.BlocksPerPacket];
var offset = OleiConstants.DataHeadSize;
for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
{
Blocks[i] = new DataBlock(span.Slice(offset, OleiConstants.PointBytes));
offset += OleiConstants.PointBytes;
}
}
}
/// <summary>
/// Packet decoder — port of olelidar/src/decoder.cpp PacketCb / DecodeAndFill / PublishMsg.
/// </summary>
public sealed class DecodeLidar
{
public readonly List<ushort> scanAngleInVec = new();
public readonly List<ushort> scanRangeInVec = new();
public readonly List<ushort> scanIntensityInVec = new();
private readonly List<ushort> _scanAngleVec = new();
private readonly List<ushort> _scanRangeVec = new();
private readonly List<ushort> _scanIntensityVec = new();
public ushort AzimuthLast { get; private set; }
public ushort AzimuthNow { get; private set; }
public ushort AzimuthFirst { get; private set; } = 0xFFFF;
private DateTime _machineTimeBase;
private uint _innerTimestampBaseMs;
private bool _isTimeBase;
private uint _lastStampMs;
/// <summary>End-of-scan time mapped from lidar internal clock (UTC).</summary>
public DateTime? LastCompletedScanLidarEndUtc { get; private set; }
public float Frequency { get; private set; }
public byte LidarType { get; private set; } = 0x01;
public int Direction { get; private set; }
public double StepDeg { get; private set; } = 0.225;
private DecoderConfig _config = new();
private readonly object _locker = new();
public void SetConfig(DecoderConfig config)
{
_config = config;
StepDeg = config.StepDeg;
}
public bool PacketCb(oleiPackage dataMsg)
{
if (dataMsg.data.Length < OleiConstants.PacketSize)
return false;
var pkt = new Packet(dataMsg.data);
AzimuthNow = pkt.Blocks[0].Point.Azimuth;
if (AzimuthFirst == 0xFFFF)
{
LidarType = pkt.Header.Code.Length > 1 ? pkt.Header.Code[1] : (byte)0x01;
AzimuthFirst = AzimuthNow;
var rpm = pkt.Header.Rpm & 0x7FFF;
Direction = pkt.Header.Rpm >> 15;
if (_config.Inverted)
Direction = 1 - Direction;
if (Frequency < 0.001f && rpm > 0)
{
Frequency = rpm / 60.0f;
if (LidarType == 0x01)
StepDeg = 0.225;
}
}
if (AzimuthLast < AzimuthNow)
{
DecodeAndFill(pkt);
AzimuthLast = AzimuthNow;
return false;
}
AzimuthLast = AzimuthNow;
if (AzimuthFirst >= 200)
{
AzimuthFirst = AzimuthNow;
return false;
}
var nowStampMs = pkt.Header.Timestamp;
if (!_isTimeBase)
{
_machineTimeBase = dataMsg.stamp.Kind == DateTimeKind.Utc
? dataMsg.stamp
: dataMsg.stamp.ToUniversalTime();
_innerTimestampBaseMs = nowStampMs;
_isTimeBase = true;
}
_lastStampMs = nowStampMs;
LastCompletedScanLidarEndUtc = ComputeLidarTimeUtc(nowStampMs, dataMsg.stamp);
if (Frequency < 0.001f)
{
if (LidarType == 0x01)
{
var rpm = pkt.Header.Rpm & 0x7FFF;
Frequency = rpm / 60.0f;
StepDeg = 0.225;
}
else if (_scanAngleVec.Count > 2)
{
StepDeg = (_scanAngleVec[1] - _scanAngleVec[0]) / 100.0;
Frequency = (float)(StepDeg * 10000.0 / 60.0);
}
else
{
return false;
}
}
lock (_locker)
{
scanAngleInVec.Clear();
scanRangeInVec.Clear();
scanIntensityInVec.Clear();
scanAngleInVec.AddRange(_scanAngleVec);
scanRangeInVec.AddRange(_scanRangeVec);
scanIntensityInVec.AddRange(_scanIntensityVec);
if (Direction == 0)
{
scanRangeInVec.Reverse();
scanIntensityInVec.Reverse();
}
_scanAngleVec.Clear();
_scanRangeVec.Clear();
_scanIntensityVec.Clear();
}
DecodeAndFill(pkt);
return scanAngleInVec.Count > 0;
}
private DateTime ComputeLidarTimeUtc(uint lidarTimestampMs, DateTime receiveStamp)
{
var receiveUtc = receiveStamp.Kind == DateTimeKind.Utc
? receiveStamp
: receiveStamp.ToUniversalTime();
if (!_isTimeBase)
return receiveUtc;
var deltaMs = lidarTimestampMs - _innerTimestampBaseMs;
return _machineTimeBase.AddMilliseconds(deltaMs);
}
private void DecodeAndFill(Packet pkt)
{
var rangeMaxMm = (ushort)(_config.RangeMax * 1000);
var rangeMinMm = (ushort)(_config.RangeMin * 1000);
for (var i = 0; i < OleiConstants.BlocksPerPacket; i++)
{
var dp = pkt.Blocks[i].Point;
var azimuth = dp.Azimuth;
var range = dp.Distance;
var intensity = dp.Reflectivity;
if (range > rangeMaxMm || range < rangeMinMm)
{
range = 0;
intensity = 0;
}
if (azimuth < 0xFF00)
{
lock (_locker)
{
_scanAngleVec.Add(azimuth);
_scanRangeVec.Add(range);
_scanIntensityVec.Add(intensity);
}
}
}
}
}