Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Detection/MarkerDetector.cs

@@ -0,0 +1,178 @@
+using Microsoft.Extensions.Configuration;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.RobotApp.SLAM;
+using RobotNet10.Shared.Detection;
+using RobotNet10.Shared.Enum;
+using RobotNet10.Shared.Geometry;
+using SkiaSharp;
+
+namespace RobotNet10.RobotApp.Detection;
+
+public class MarkerDetector(
+    IConfiguration configuration,
+    IDeviceProvider deviceProvider,
+    ISLAMService slamService) : IMarkerDetector
+{
+    private readonly MarkerDetectorConfiguration _config = BindConfig(configuration);
+    private readonly List<IDetectSession> _sessions = [];
+    private readonly Lock _lock = new();
+
+    private static MarkerDetectorConfiguration BindConfig(IConfiguration configuration)
+    {
+        var section = configuration.GetSection("Detection:MarkerDetector");
+        if (!section.Exists())
+            throw new InvalidOperationException("Configuration section 'Detection:MarkerDetector' not found.");
+
+        var config = new MarkerDetectorConfiguration();
+        section.Bind(config);
+        return config;
+    }
+
+    public Task<IDetectSession> CreateSessionAsync(MarkersSearchRequest request)
+    {
+        var session = new DetectSession(Guid.NewGuid(), slamService);
+
+        // Create search region from request in global frame
+        // (X, Y as center, Yaw as rotation - all in global/world coordinates)
+        var searchRegion = new RectangleRegion(
+            request.X,
+            request.Y,
+            request.Width,
+            request.Length,
+            request.Yaw);
+
+        foreach (var entry in request.MarkerSearchRequests)
+        {
+            switch (entry.Type)
+            {
+                case MarkerType.ShapeReflective:
+                    AddShapeReflectiveDetector(session, entry, searchRegion);
+                    break;
+
+                case MarkerType.QRCode:
+                    AddQRDetector(session, entry);
+                    break;
+
+                case MarkerType.ArUco:
+                    // TODO: Implement ArUco detector
+                    break;
+
+                default:
+                    // TODO: Handle other marker types
+                    break;
+            }
+        }
+
+        session.Start();
+
+        lock (_lock)
+        {
+            _sessions.Add(session);
+        }
+
+        return Task.FromResult<IDetectSession>(session);
+    }
+
+    public IReadOnlyList<IDetectSession> GetActiveSessions()
+    {
+        lock (_lock)
+        {
+            return [.. _sessions];
+        }
+    }
+
+    public IDetectSession? GetSession(Guid sessionId)
+    {
+        lock (_lock)
+        {
+            return _sessions.FirstOrDefault(s => s.SessionId == sessionId);
+        }
+    }
+
+    public void Dispose()
+    {
+        lock (_lock)
+        {
+            foreach (var session in _sessions)
+            {
+                session.Dispose();
+            }
+            _sessions.Clear();
+        }
+        GC.SuppressFinalize(this);
+    }
+
+    private void AddShapeReflectiveDetector(DetectSession session, MarkerEntry entry, RectangleRegion searchRegion)
+    {
+        // Get device from provider and cast to ILidar
+        var device = deviceProvider.GetDevice(entry.DeviceId)
+            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");
+
+        if (device is not ILidar lidar)
+            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ILidar.");
+
+        // Get lidar transform from configuration (includes pose and intensity threshold)
+        var lidarTransform = GetDeviceTransform(_config.LidarDevices, entry.DeviceId);
+        var lidarPose = new Pose(
+            lidarTransform.Position,
+            new RobotNet10.Shared.Geometry.Quaternion(
+                lidarTransform.Orientation.X,
+                lidarTransform.Orientation.Y,
+                lidarTransform.Orientation.Z,
+                lidarTransform.Orientation.W));
+
+        // Parse reference points from Parameters [x1, y1, x2, y2, ...]
+        var referencePoints = entry.ReferencePoints.Select(p => new Point2D(p.X, p.Y)).ToArray();
+
+        session.AddShapeReflectiveDetector(entry.MarkerId,
+            entry.Priority,
+            referencePoints,
+            searchRegion,
+            lidar,
+            lidarPose,
+            lidarTransform.IntensityThreshold);
+    }
+
+    private void AddQRDetector(DetectSession session, MarkerEntry entry)
+    {
+        // Get device from provider and cast to ICameraQr
+        var device = deviceProvider.GetDevice(entry.DeviceId)
+            ?? throw new ArgumentException($"Device '{entry.DeviceId}' not found.");
+
+        if (device is not ICameraQr camera)
+            throw new ArgumentException($"Device '{entry.DeviceId}' is not an ICameraQr.");
+
+        // Get camera pose from configuration
+        var cameraPose = GetDevicePose(_config.QrDevices, entry.DeviceId);
+
+        // Use MarkerId as QR code string to detect
+        var qrCode = entry.Code;
+
+        session.AddQRDetector(entry.MarkerId,
+            entry.Priority,
+            qrCode,
+            camera,
+            cameraPose);
+    }
+
+    private static Pose GetDevicePose(DeviceTransform[] devices, string deviceId)
+    {
+        var transform = GetDeviceTransform(devices, deviceId);
+        return new Pose(
+            transform.Position,
+            new RobotNet10.Shared.Geometry.Quaternion(
+                transform.Orientation.X,
+                transform.Orientation.Y,
+                transform.Orientation.Z,
+                transform.Orientation.W));
+    }
+
+    private static DeviceTransform GetDeviceTransform(DeviceTransform[] devices, string deviceId)
+    {
+        var transform = Array.Find(devices, d => d.DeviceId == deviceId);
+        if (string.IsNullOrEmpty(transform.DeviceId))
+            throw new ArgumentException($"Device transform for '{deviceId}' not found in configuration.");
+
+        return transform;
+    }
+}