Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/QRDetector.cs

@@ -0,0 +1,213 @@
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Geometry;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.Detection;
+
+/// <summary>
+/// Detector for QR code markers using camera
+/// Polls QR camera for specific QR code and transforms pose to global frame
+/// </summary>
+/// <remarks>
+/// Create a new QR code detection session
+/// </remarks>
+/// <param name="qrCode">QR code string to detect</param>
+/// <param name="slamService">SLAM service for current robot pose</param>
+/// <param name="camera">QR camera device</param>
+/// <param name="cameraPose">Camera pose relative to robot base</param>
+public class QRDetector(
+    string qrCode,
+    ISLAMService slamService,
+    ICameraQr camera,
+    Pose cameraPose) : IDetector
+{
+    private readonly Lock _lockPose = new();
+    private bool _isActive = false;
+    private Thread? _pollingThread;
+    private const int POLLING_FREQUENCY_HZ = 30; // Poll at 30Hz
+
+    /// <summary>
+    /// Detected marker pose in global frame
+    /// Initialized with default pose, updated when QR code is detected
+    /// </summary>
+    public Pose MarkerPose
+    {
+        get
+        {
+            lock (_lockPose)
+            {
+                return field;
+            }
+        }
+        private set
+        {
+            lock (_lockPose)
+            {
+                field = value;
+            }
+        }
+    }
+
+    /// <summary>
+    /// Timestamp of the last successful QR detection
+    /// </summary>
+    public DateTime DetectionTime { get; private set; } = DateTime.MinValue;
+
+    /// <summary>
+    /// Activate the QR detection
+    /// Starts polling QR camera in background thread
+    /// </summary>
+    public void Active()
+    {
+        if (_isActive)
+            return;
+
+        _isActive = true;
+
+        // Create and start polling thread
+        _pollingThread = new Thread(PollingThreadLoop)
+        {
+            Name = $"QRDetection_{qrCode}",
+            IsBackground = true
+        };
+        _pollingThread.Start();
+    }
+
+    /// <summary>
+    /// Disable the QR detection
+    /// Stops polling thread
+    /// </summary>
+    public void Disable()
+    {
+        if (!_isActive)
+            return;
+
+        _isActive = false;
+
+        // Wait for thread to finish
+        _pollingThread?.Join();
+        _pollingThread = null;
+    }
+
+    /// <summary>
+    /// Polling thread loop that checks QR camera periodically
+    /// Runs at 30Hz to check for QR code detection
+    /// </summary>
+    private void PollingThreadLoop()
+    {
+        int delayMs = 1000 / POLLING_FREQUENCY_HZ; // ~33ms for 30Hz
+
+        while (_isActive)
+        {
+            try
+            {
+                // Check if camera is connected
+                if (!camera.IsConnected)
+                {
+                    Thread.Sleep(delayMs);
+                    continue;
+                }
+
+                // Get QR code pose from camera (in camera frame)
+                var poseStampedInCamera = camera[qrCode];
+
+                if (poseStampedInCamera.HasValue)
+                {
+                    // Get current robot pose in global frame
+                    var currentRobotPose = slamService.CurrentPose;
+
+                    // Transform: camera frame -> robot frame -> global frame
+                    var poseInRobot = TransformPose(poseStampedInCamera.Value.Pose, cameraPose);
+                    var poseInGlobal = TransformPose(poseInRobot, currentRobotPose);
+
+                    // Update marker pose and detection time
+                    MarkerPose = poseInGlobal;
+                    DetectionTime = poseStampedInCamera.Value.Header.Stamp;
+
+                    // Console.WriteLine($"{DetectionTime:HH:mm:ss.ffffff} [QRDetector] Detected found QR code '{qrCode}' at global pose: [{MarkerPose.Position.X}, {MarkerPose.Position.Y}, {MarkerPose.Orientation.ToYawDegrees()}deg]");
+                }
+            }
+            catch
+            {
+                // Ignore errors in polling loop
+            }
+
+            // Sleep to maintain polling rate
+            Thread.Sleep(delayMs);
+        }
+    }
+
+    /// <summary>
+    /// Transform a pose by another pose (pose composition)
+    /// result = parentPose * childPose
+    /// </summary>
+    /// <param name="childPose">Pose in child frame</param>
+    /// <param name="parentPose">Parent frame pose</param>
+    /// <returns>Transformed pose in parent's parent frame</returns>
+    private static Pose TransformPose(Pose childPose, Pose parentPose)
+    {
+        // Get yaw angles
+        double parentYaw = parentPose.Orientation.ToYawRadian();
+        double childYaw = childPose.Orientation.ToYawRadian();
+
+        // Rotate child position by parent orientation
+        double cosParent = Math.Cos(parentYaw);
+        double sinParent = Math.Sin(parentYaw);
+
+        double globalX = parentPose.Position.X + childPose.Position.X * cosParent - childPose.Position.Y * sinParent;
+        double globalY = parentPose.Position.Y + childPose.Position.X * sinParent + childPose.Position.Y * cosParent;
+
+        // Combine orientations
+        double globalYaw = NormalizeAngle(parentYaw + childYaw);
+
+        return new Pose
+        {
+            Position = new Vector3
+            {
+                X = globalX,
+                Y = globalY,
+                Z = parentPose.Position.Z + childPose.Position.Z
+            },
+            Orientation = CreateQuaternionFromYaw(globalYaw)
+        };
+    }
+
+    /// <summary>
+    /// Create a quaternion from yaw angle (rotation around Z axis)
+    /// </summary>
+    /// <param name="yaw">Yaw angle in radians</param>
+    /// <returns>Quaternion representing rotation around Z axis</returns>
+    private static RobotNet10.Shared.Geometry.Quaternion CreateQuaternionFromYaw(double yaw)
+    {
+        double halfYaw = yaw / 2.0;
+        return new RobotNet10.Shared.Geometry.Quaternion
+        {
+            X = 0,
+            Y = 0,
+            Z = Math.Sin(halfYaw),
+            W = Math.Cos(halfYaw)
+        };
+    }
+
+    /// <summary>
+    /// Normalize angle to [-π, π]
+    /// </summary>
+    /// <param name="angle">Angle in radians</param>
+    /// <returns>Normalized angle in [-π, π]</returns>
+    private static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+
+    public void Dispose()
+    {
+        // Disable detector (stops thread)
+        Disable();
+
+        // Suppress finalization since we've cleaned up
+        GC.SuppressFinalize(this);
+    }
+}