Initial commit
This commit is contained in:
@@ -0,0 +1,442 @@
|
||||
using RobotNet10.RobotApp.Client.Shared.Devices;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using System.Diagnostics;
|
||||
using System.Net;
|
||||
using System.Net.Sockets;
|
||||
using System.Text;
|
||||
|
||||
namespace RobotNet10.RobotApp.Drivers.Hik;
|
||||
|
||||
/// <summary>
|
||||
/// Driver Hik cho Camera QR
|
||||
/// </summary>
|
||||
[Device(DeviceType.CameraQr, "Hik", "HikVisionQr", "1.0.0", Description = "Camera QR Code Detection Hik Driver")]
|
||||
public class HikVisionQr : DeviceBase, ICameraQr
|
||||
{
|
||||
private readonly ILogger _logger;
|
||||
private readonly Lock _dataLock = new();
|
||||
private Thread? _receiveThread;
|
||||
private CancellationTokenSource? _receiveCts;
|
||||
private UdpClient? _udpClient;
|
||||
private IPEndPoint? _localEndPoint;
|
||||
private IPEndPoint? _remoteEndPoint;
|
||||
|
||||
// Camera parameters
|
||||
private readonly int _cameraWidth;
|
||||
private readonly int _cameraHeight;
|
||||
private readonly double _distanceToQr;
|
||||
private readonly double _fovRangeWidthMm;
|
||||
private readonly double _fovRangeHeightMm;
|
||||
private readonly double _fovRangeDistanceMm;
|
||||
private readonly double _focalLengthPixels;
|
||||
private readonly string _localIp;
|
||||
private readonly int _localPort;
|
||||
private readonly double _qrDataTimeoutSeconds;
|
||||
|
||||
// QR data dictionary (PoseStamped already contains timestamp in Header.Stamp)
|
||||
private readonly Dictionary<string, PoseStamped> _qrDictionary = [];
|
||||
|
||||
// Data rate tracking
|
||||
private int _packetCount = 0;
|
||||
private double _currentDataRate = 0.0;
|
||||
|
||||
public Dictionary<string, PoseStamped> Codes
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
var validCodes = new Dictionary<string, PoseStamped>();
|
||||
var now = DateTime.UtcNow;
|
||||
var expiredKeys = new List<string>();
|
||||
|
||||
foreach (var (code, poseStamped) in _qrDictionary)
|
||||
{
|
||||
var elapsed = (now - poseStamped.Header.Stamp).TotalSeconds;
|
||||
if (elapsed <= _qrDataTimeoutSeconds)
|
||||
{
|
||||
// Data is still valid
|
||||
validCodes[code] = poseStamped;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Data expired, mark for removal
|
||||
expiredKeys.Add(code);
|
||||
}
|
||||
}
|
||||
|
||||
// Clean up expired entries
|
||||
foreach (var key in expiredKeys)
|
||||
{
|
||||
_qrDictionary.Remove(key);
|
||||
}
|
||||
|
||||
return validCodes;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public HikVisionQr(string deviceId, string deviceName, IConfigurationSection connection, IServiceProvider serviceProvider)
|
||||
: base(deviceId, deviceName, DeviceType.CameraQr)
|
||||
{
|
||||
|
||||
_logger = serviceProvider.GetRequiredService<ILoggerFactory>().CreateLogger<HikVisionQr>();
|
||||
// Read configuration
|
||||
var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
|
||||
var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
|
||||
var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");
|
||||
|
||||
// Camera parameters
|
||||
_cameraWidth = connection.GetValue<int?>("CameraWidth") ?? 1920;
|
||||
_cameraHeight = connection.GetValue<int?>("CameraHeight") ?? 1080;
|
||||
_distanceToQr = connection.GetValue<double?>("DistanceToQr") ?? 0.1;
|
||||
_fovRangeWidthMm = connection.GetValue<double?>("FovRangeWidthMm") ?? 170.0;
|
||||
_fovRangeHeightMm = connection.GetValue<double?>("FovRangeHeightMm") ?? 130.0;
|
||||
_fovRangeDistanceMm = connection.GetValue<double?>("FovRangeDistanceMm") ?? 100.0;
|
||||
_qrDataTimeoutSeconds = connection.GetValue<double?>("QrDataTimeoutSeconds") ?? 3.0;
|
||||
|
||||
// Compute focal length in pixels from FOV range (pinhole camera model)
|
||||
// f = CameraWidth * FovRangeDistance / FovRangeWidth
|
||||
_focalLengthPixels = _cameraWidth * _fovRangeDistanceMm / _fovRangeWidthMm;
|
||||
|
||||
// UDP parameters
|
||||
_localIp = connection.GetValue<string>("LocalIP") ?? "192.168.254.100";
|
||||
_localPort = connection.GetValue<int?>("LocalPort") ?? 1024;
|
||||
|
||||
if (autoReconnectEnabled.HasValue)
|
||||
AutoReconnectEnabled = autoReconnectEnabled.Value;
|
||||
|
||||
if (reconnectDelayMs.HasValue)
|
||||
ReconnectDelayMs = reconnectDelayMs.Value;
|
||||
|
||||
if (maxReconnectAttempts.HasValue)
|
||||
MaxReconnectAttempts = maxReconnectAttempts.Value;
|
||||
|
||||
// Initialize properties
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
|
||||
{
|
||||
yield return new PropertyDescription("DataRate", "Data Rate", "Tần số nhận dữ liệu (packets/s)")
|
||||
{
|
||||
DataType = "number",
|
||||
IsReadOnly = true,
|
||||
DisplayOrder = 1,
|
||||
Category = "Trạng thái",
|
||||
DefaultValue = "0"
|
||||
};
|
||||
}
|
||||
|
||||
protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
return await Task.FromResult(_udpClient != null && _receiveThread != null && _receiveThread.IsAlive);
|
||||
}
|
||||
|
||||
protected override async Task OnConnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
// Create local endpoint and UDP client
|
||||
_localEndPoint = new IPEndPoint(IPAddress.Parse(_localIp), _localPort);
|
||||
_udpClient = new UdpClient(_localEndPoint);
|
||||
|
||||
// Create cancellation token source for receive thread
|
||||
_receiveCts = new CancellationTokenSource();
|
||||
|
||||
// Create and start high-priority thread for receiving data
|
||||
_receiveThread = new Thread(() => ReceiveDataLoop(_receiveCts.Token))
|
||||
{
|
||||
IsBackground = true,
|
||||
Priority = ThreadPriority.Highest,
|
||||
Name = $"HikQr-{DeviceId}-ReceiveThread"
|
||||
};
|
||||
_receiveThread.Start();
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
// Stop receive thread
|
||||
_receiveCts?.Cancel();
|
||||
|
||||
// Wait for thread to finish (with timeout)
|
||||
if (_receiveThread != null && _receiveThread.IsAlive)
|
||||
{
|
||||
var joined = _receiveThread.Join(TimeSpan.FromSeconds(5));
|
||||
if (!joined)
|
||||
{
|
||||
// Thread didn't finish gracefully
|
||||
System.Diagnostics.Debug.WriteLine($"[HikQr] Receive thread did not finish in time");
|
||||
}
|
||||
}
|
||||
|
||||
_receiveCts?.Dispose();
|
||||
_receiveCts = null;
|
||||
_receiveThread = null;
|
||||
|
||||
// Close and dispose UDP client
|
||||
_udpClient?.Close();
|
||||
_udpClient?.Dispose();
|
||||
_udpClient = null;
|
||||
_localEndPoint = null;
|
||||
_remoteEndPoint = null;
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
protected override async Task OnResetAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
// Clear QR dictionary
|
||||
_qrDictionary.Clear();
|
||||
_packetCount = 0;
|
||||
_currentDataRate = 0.0;
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
private void ReceiveDataLoop(CancellationToken cancellationToken)
|
||||
{
|
||||
Thread.BeginThreadAffinity();
|
||||
try
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
var packetCountInSecond = 0;
|
||||
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
try
|
||||
{
|
||||
// Check if 1 second has elapsed
|
||||
if (stopwatch.Elapsed.TotalSeconds >= 1.0)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
_currentDataRate = packetCountInSecond / stopwatch.Elapsed.TotalSeconds;
|
||||
UpdateProperties();
|
||||
}
|
||||
|
||||
// Reset counter and stopwatch
|
||||
packetCountInSecond = 0;
|
||||
stopwatch.Restart();
|
||||
}
|
||||
|
||||
// Check if UDP client is available
|
||||
if (_udpClient == null)
|
||||
break;
|
||||
|
||||
// Set receive timeout to allow checking cancellation token
|
||||
_udpClient.Client.ReceiveTimeout = 100;
|
||||
|
||||
// Receive UDP packet
|
||||
var receivedData = _udpClient.Receive(ref _remoteEndPoint);
|
||||
|
||||
if (receivedData.Length > 0)
|
||||
{
|
||||
packetCountInSecond++;
|
||||
lock (_dataLock)
|
||||
{
|
||||
_packetCount++;
|
||||
}
|
||||
|
||||
// Process received data
|
||||
UpdateData(receivedData);
|
||||
}
|
||||
}
|
||||
catch (SocketException ex) when (ex.SocketErrorCode == SocketError.TimedOut)
|
||||
{
|
||||
// Timeout is expected, continue loop
|
||||
continue;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
break;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// Log error and continue
|
||||
_logger.LogError(ex, "[HikVisionQr] ReceiveDataLoop error");
|
||||
}
|
||||
}
|
||||
|
||||
stopwatch.Stop();
|
||||
}
|
||||
finally
|
||||
{
|
||||
Thread.EndThreadAffinity();
|
||||
}
|
||||
}
|
||||
|
||||
private void UpdateData(byte[] receivedData)
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (receivedData.Length == 0)
|
||||
{
|
||||
// No data, clear dictionary
|
||||
_qrDictionary.Clear();
|
||||
}
|
||||
else
|
||||
{
|
||||
try
|
||||
{
|
||||
// Extract message (40 bytes from position 37)
|
||||
if (receivedData.Length < 77)
|
||||
{
|
||||
// Invalid packet size
|
||||
return;
|
||||
}
|
||||
|
||||
byte[] messageBytes = new byte[40];
|
||||
Array.Copy(receivedData, 37, messageBytes, 0, 40);
|
||||
string message = Encoding.UTF8.GetString(messageBytes);
|
||||
|
||||
// Extract QR data (9 bytes)
|
||||
byte[] dataBytes = new byte[9];
|
||||
Array.Copy(messageBytes, 0, dataBytes, 0, 9);
|
||||
string qrCode = Encoding.UTF8.GetString(dataBytes);
|
||||
|
||||
// Extract X position (3 bytes from position 11)
|
||||
byte[] xBytes = new byte[3];
|
||||
Array.Copy(messageBytes, 11, xBytes, 0, 3);
|
||||
int qrPositionX = int.Parse(Encoding.ASCII.GetString(xBytes));
|
||||
|
||||
// Extract Y position (3 bytes from position 19)
|
||||
byte[] yBytes = new byte[3];
|
||||
Array.Copy(messageBytes, 19, yBytes, 0, 3);
|
||||
int qrPositionY = int.Parse(Encoding.ASCII.GetString(yBytes));
|
||||
|
||||
// Extract angle (between ')' and '@')
|
||||
double qrAngle = 0;
|
||||
int start = message.IndexOf(')') + 1;
|
||||
int end = message.IndexOf('@', start);
|
||||
if (start > 0 && end > start)
|
||||
{
|
||||
string numberAngle = message[start..end];
|
||||
qrAngle = double.Parse(numberAngle);
|
||||
}
|
||||
|
||||
// Calculate pose immediately
|
||||
var pose = CreatePoseFromPixels(
|
||||
qrPositionX,
|
||||
qrPositionY,
|
||||
qrAngle,
|
||||
_cameraWidth,
|
||||
_cameraHeight,
|
||||
_distanceToQr,
|
||||
_focalLengthPixels);
|
||||
|
||||
if (pose.HasValue)
|
||||
{
|
||||
// Console.WriteLine($"[HikQr] Detected QR Code: {qrCode}, X: {pose.Value.Pose.Position.X}, Y: {pose.Value.Pose.Position.Y}, Angle: {pose.Value.Pose.Orientation.ToYawDegrees()} degrees");
|
||||
// Add or update dictionary (timestamp is in pose.Header.Stamp)
|
||||
_qrDictionary[qrCode] = pose.Value;
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
System.Diagnostics.Debug.WriteLine($"[HikQr] UpdateData parsing error: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
UpdateProperties();
|
||||
}
|
||||
}
|
||||
|
||||
private void UpdateProperties()
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
SetProperty("DataRate", _currentDataRate.ToString("F2"));
|
||||
}
|
||||
}
|
||||
|
||||
protected override void Dispose(bool disposing)
|
||||
{
|
||||
if (disposing)
|
||||
{
|
||||
_receiveCts?.Cancel();
|
||||
_receiveCts?.Dispose();
|
||||
_udpClient?.Close();
|
||||
_udpClient?.Dispose();
|
||||
}
|
||||
base.Dispose(disposing);
|
||||
}
|
||||
|
||||
#region ICameraQr Implementation
|
||||
|
||||
public PoseStamped? this[string code]
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_dataLock)
|
||||
{
|
||||
if (!_qrDictionary.TryGetValue(code, out var poseStamped))
|
||||
return null;
|
||||
|
||||
// Check if data is still valid (within timeout)
|
||||
var elapsed = (DateTime.UtcNow - poseStamped.Header.Stamp).TotalSeconds;
|
||||
if (elapsed > _qrDataTimeoutSeconds)
|
||||
{
|
||||
// Data expired, remove from dictionary
|
||||
_qrDictionary.Remove(code);
|
||||
return null;
|
||||
}
|
||||
|
||||
// Data is valid, return pose
|
||||
return poseStamped;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static PoseStamped? CreatePoseFromPixels(
|
||||
int? px,
|
||||
int? py,
|
||||
double? angleDeg,
|
||||
int cameraWidth,
|
||||
int cameraHeight,
|
||||
double distanceMeters,
|
||||
double focalLengthPixels)
|
||||
{
|
||||
if (!px.HasValue || !py.HasValue || !angleDeg.HasValue)
|
||||
return null;
|
||||
|
||||
// Convert pixel coordinates to meters using pinhole camera model
|
||||
// Same focal length for both axes (square pixels: 4.8μm × 4.8μm)
|
||||
double xMeters = (px.Value - cameraWidth / 2.0) * distanceMeters / focalLengthPixels;
|
||||
double yMeters = -(py.Value - cameraHeight / 2.0) * distanceMeters / focalLengthPixels; // Invert Y axis
|
||||
double zMeters = distanceMeters;
|
||||
|
||||
// Convert angle to quaternion
|
||||
double angleRad = angleDeg.Value * Math.PI / 180.0;
|
||||
var quat = RobotNet10.Shared.Numbers.Quaternion.FromYawRadian(angleRad);
|
||||
|
||||
// Create header
|
||||
var header = new RobotNet10.Shared.Header
|
||||
{
|
||||
Seq = 0,
|
||||
Stamp = DateTime.UtcNow,
|
||||
FrameId = "camera"
|
||||
};
|
||||
|
||||
// Create pose
|
||||
var pose = new Pose
|
||||
{
|
||||
Position = new Point { X = xMeters, Y = yMeters, Z = zMeters },
|
||||
Orientation = new Quaternion(quat.X, quat.Y, quat.Z, quat.W)
|
||||
};
|
||||
|
||||
return new PoseStamped(header, pose);
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user