Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/LidarHub.cs

@@ -0,0 +1,145 @@
+using Microsoft.AspNetCore.Authorization;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet10.RobotApp.Client.Shared.Devices;
+using RobotNet10.RobotApp.Devices;
+using RobotNet10.Shared.Sensor;
+
+namespace RobotNet10.RobotApp.Hubs;
+
+/// <summary>
+/// SignalR Hub cho Lidar device - cung cấp real-time lidar scan data
+/// </summary>
+[Authorize]
+public class LidarHub(IDeviceProvider deviceProvider) : Hub
+{
+    /// <summary>
+    /// Độ phân giải mới: 1 độ (1 tia/độ) tính bằng radian
+    /// </summary>
+    private const double TARGET_ANGLE_INCREMENT_RAD = (Math.PI / 180.0); // 1 độ = π/180 radian
+
+    /// <summary>
+    /// Lấy thông tin device (DeviceName) theo device ID
+    /// </summary>
+    public async Task<DeviceInfoDto?> GetDeviceInfo(string deviceId)
+    {
+        var device = deviceProvider.GetDevice(deviceId);
+        if (device is not ILidar)
+        {
+            return null;
+        }
+
+        return new DeviceInfoDto
+        {
+            DeviceId = device.DeviceId,
+            DeviceName = device.DeviceName
+        };
+    }
+
+    /// <summary>
+    /// Lấy thông tin lidar scan data theo device ID
+    /// Trả về LaserScan với độ phân giải mới 1 độ (1 tia/độ)
+    /// </summary>
+    public async Task<LaserScan?> GetLidarData(string deviceId)
+    {
+        var device = deviceProvider.GetDevice(deviceId);
+        if (device is not ILidar lidar || lidar.CurrentMeasurementData == null)
+        {
+            return null;
+        }
+
+        var originalScan = lidar.CurrentMeasurementData.Value;
+
+        // Tạo LaserScan mới với độ phân giải 1°
+        var newScan = new LaserScan
+        {
+            Header = originalScan.Header,
+            AngleMin = originalScan.AngleMin,
+            AngleMax = originalScan.AngleMax,
+            AngleIncrement = TARGET_ANGLE_INCREMENT_RAD, // 1° spacing (π/180 rad)
+            TimeIncrement = originalScan.TimeIncrement,
+            ScanTime = originalScan.ScanTime,
+            RangeMin = originalScan.RangeMin,
+            RangeMax = originalScan.RangeMax
+        };
+
+        // Tính số điểm mới với độ phân giải 1 độ
+        var angleSpan = originalScan.AngleMax - originalScan.AngleMin;
+        var newPointCount = (int)Math.Round(angleSpan / TARGET_ANGLE_INCREMENT_RAD) + 1;
+        newScan.Ranges = new double[newPointCount];
+        newScan.Intensities = new double[newPointCount];
+
+        // Tính toán lại Ranges và Intensities với độ phân giải mới
+        for (int i = 0; i < newPointCount; i++)
+        {
+            // Tính góc bắt đầu và kết thúc cho bin hiện tại
+            var targetAngleStart = originalScan.AngleMin + i * TARGET_ANGLE_INCREMENT_RAD;
+            var targetAngleEnd = Math.Min(
+                originalScan.AngleMin + (i + 1) * TARGET_ANGLE_INCREMENT_RAD,
+                originalScan.AngleMax
+            );
+
+            // Tìm các index trong khoảng [targetAngleStart, targetAngleEnd]
+            var indexStart = (targetAngleStart - originalScan.AngleMin) / originalScan.AngleIncrement;
+            var indexEnd = (targetAngleEnd - originalScan.AngleMin) / originalScan.AngleIncrement;
+
+            var index0 = (int)Math.Floor(indexStart);
+            var index1 = (int)Math.Ceiling(indexEnd);
+
+            // Clamp indices to valid range
+            index0 = Math.Max(0, Math.Min(index0, originalScan.Ranges.Length - 1));
+            index1 = Math.Max(0, Math.Min(index1, originalScan.Ranges.Length - 1));
+
+            // Tính trung bình các điểm có intensity > 0 trong khoảng [index0, index1]
+            double sumRange = 0;
+            double sumIntensity = 0;
+            int validPointCount = 0;
+
+            for (int idx = index0; idx <= index1; idx++)
+            {
+                // Kiểm tra intensity để xác định điểm hợp lệ
+                bool hasValidIntensity = originalScan.Intensities != null &&
+                                        idx < originalScan.Intensities.Length &&
+                                        originalScan.Intensities[idx] > 0;
+
+                if (hasValidIntensity)
+                {
+                    var range = originalScan.Ranges[idx];
+
+                    // Chỉ lấy các điểm không phải NaN/Infinity
+                    if (!double.IsNaN(range) && !double.IsInfinity(range))
+                    {
+                        sumRange += range;
+                        sumIntensity += originalScan.Intensities![idx];
+                        validPointCount++;
+                    }
+                }
+            }
+
+            // Tính giá trị trung bình hoặc set -1 nếu không có điểm hợp lệ
+            if (validPointCount > 0)
+            {
+                newScan.Ranges[i] = sumRange / validPointCount;
+                newScan.Intensities[i] = sumIntensity / validPointCount;
+            }
+            else
+            {
+                newScan.Ranges[i] = -1.0;  // No valid data
+                newScan.Intensities[i] = 0.0;
+            }
+        }
+
+        // Final safety pass: ensure no NaN/Infinity values remain
+        // This should ideally never catch anything if the logic above is correct
+        for (int i = 0; i < newScan.Ranges.Length; i++)
+        {
+            if (double.IsNaN(newScan.Ranges[i]) || double.IsInfinity(newScan.Ranges[i]))
+            {
+                newScan.Ranges[i] = -1.0;
+            }
+        }
+
+        return newScan;
+    }
+}
+
+