Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/DetectSession.cs

@@ -0,0 +1,204 @@
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Geometry;
+
+namespace RobotNet10.RobotApp.Detection;
+
+public class DetectSession(Guid id, ISLAMService sLAMService) : IDetectSession
+{
+    private static uint SequenceDetection = 1;
+    private readonly List<DetectorInfo> _detectors = [];
+    private readonly Lock _lockGoal = new();
+    private Thread? _updateThread;
+    private bool _isRunning = false;
+    private const int UPDATE_FREQUENCY_HZ = 30;
+    private const double DETECTION_TIMEOUT_SECONDS = 2.0;
+
+    public Guid SessionId { get; } = id;
+
+    public PoseStamped? Goal
+    {
+        get
+        {
+            lock (_lockGoal)
+            {
+                return field;
+            }
+        }
+        private set
+        {
+            lock (_lockGoal)
+            {
+                field = value;
+            }
+        }
+    }
+
+    public void AddShapeReflectiveDetector(string markerId, int priority, Point2D[] markerReferencePoints, RectangleRegion searchRegion, ILidar lidar, Pose lidarPose, double intensityThreshold = 1000)
+    {
+        // Create detector with configurable intensity threshold
+        var detector = new ShapeReflectiveDetector(
+            markerReferencePoints,
+            searchRegion,
+            sLAMService,
+            lidar,
+            lidarPose,
+            intensityThreshold);
+
+        // Add to list with priority
+        _detectors.Add(new DetectorInfo(markerId, priority, detector));
+
+        // Sort by priority (higher priority first)
+        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
+    }
+
+    public void AddQRDetector(string markerId, int priority, string qrCode, ICameraQr camera, Pose cameraPose)
+    {
+        // Create QR detector
+        var detector = new QRDetector(
+            qrCode,
+            sLAMService,
+            camera,
+            cameraPose);
+
+        // Add to list with priority
+        _detectors.Add(new DetectorInfo(markerId, priority, detector));
+
+        // Sort by priority (higher priority first)
+        _detectors.Sort((a, b) => b.Priority.CompareTo(a.Priority));
+    }
+
+    public void Start()
+    {
+        if (_isRunning)
+            return;
+
+        _isRunning = true;
+
+        // Activate all detectors
+        foreach (var detectorInfo in _detectors)
+        {
+            detectorInfo.Detector.Active();
+        }
+
+        // Create and start goal update thread (30Hz)
+        _updateThread = new Thread(UpdateGoalLoop)
+        {
+            Name = "DetectSession_UpdateGoal",
+            IsBackground = true
+        };
+        _updateThread.Start();
+    }
+
+    public void Dispose()
+    {
+        if (!_isRunning)
+            return;
+
+        _isRunning = false;
+
+        // Disable all detectors
+        foreach (var detectorInfo in _detectors)
+        {
+            detectorInfo.Detector.Disable();
+            detectorInfo.Detector.Dispose();
+        }
+
+        // Wait for thread to finish
+        _updateThread?.Join();
+        _updateThread = null;
+
+        // Clear detectors list
+        _detectors.Clear();
+
+        GC.SuppressFinalize(this);
+    }
+
+    /// <summary>
+    /// Thread loop to update Goal based on detector priorities at 30Hz
+    /// Filters detectors by detection time, pose validity, and priority
+    /// </summary>
+    private void UpdateGoalLoop()
+    {
+        int delayMs = 1000 / UPDATE_FREQUENCY_HZ; // ~33ms for 30Hz
+
+        while (_isRunning)
+        {
+            try
+            {
+                var now = DateTime.UtcNow;
+                PoseStamped? bestGoal = null;
+                int bestPriority = int.MinValue;
+                DateTime bestDetectionTime = DateTime.MinValue;
+
+                // Find best detector based on priority and detection freshness
+                foreach (var detectorInfo in _detectors)
+                {
+                    var markerPose = detectorInfo.Detector.MarkerPose;
+                    var detectionTime = detectorInfo.Detector.DetectionTime;
+
+                    // Skip if pose is invalid (default value)
+                    if (IsDefaultPose(markerPose))
+                        continue;
+
+                    // Skip if detection is too old (timeout)
+                    var timeSinceDetection = (now - detectionTime).TotalSeconds;
+                    if (timeSinceDetection > DETECTION_TIMEOUT_SECONDS)
+                        continue;
+
+                    // Select detector with highest priority
+                    // If same priority, prefer more recent detection
+                    if (detectorInfo.Priority > bestPriority ||
+                        (detectorInfo.Priority == bestPriority && detectionTime > bestDetectionTime))
+                    {
+                        bestGoal = new PoseStamped()
+                        {
+                            Header = new RobotNet10.Shared.Header()
+                            {
+                                FrameId = detectorInfo.MakerId,
+                                Stamp = detectionTime,
+                                Seq = ++SequenceDetection,
+                            },
+                            Pose = markerPose,
+                        };
+                        bestPriority = detectorInfo.Priority;
+                        bestDetectionTime = detectionTime;
+                    }
+                }
+
+                // Update goal if we found a valid one
+                if (bestGoal.HasValue)
+                {
+                    Goal = bestGoal.Value;
+                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [DetectSession] Updated Goal from detector '{bestGoal.Value.Header.FrameId}' with priority {bestPriority} at pose: [{bestGoal.Value.Pose.Position.X}, {bestGoal.Value.Pose.Position.Y}, {bestGoal.Value.Pose.Orientation.ToYawDegrees()}deg]");
+                }
+            }
+            catch
+            {
+                // Ignore errors in update loop
+            }
+
+            // Sleep to maintain 30Hz update rate
+            Thread.Sleep(delayMs);
+        }
+    }
+
+    /// <summary>
+    /// Check if pose is default/invalid
+    /// </summary>
+    private static bool IsDefaultPose(Pose pose)
+    {
+        return pose.Position.X == 0 &&
+               pose.Position.Y == 0 &&
+               pose.Position.Z == 0 &&
+               pose.Orientation.X == 0 &&
+               pose.Orientation.Y == 0 &&
+               pose.Orientation.Z == 0 &&
+               pose.Orientation.W == 0;
+    }
+
+    /// <summary>
+    /// Internal class to store detector with priority
+    /// </summary>
+    private record DetectorInfo(string MakerId, int Priority, IDetector Detector);
+}