Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/XlocPoseHub.cs

@@ -0,0 +1,273 @@
+using Microsoft.AspNetCore.SignalR;
+using System.Threading.Tasks;
+
+namespace RobotNet10.RobotApp.Hubs;
+
+/// <summary>
+/// SignalR Hub for streaming XLOC pose data in realtime
+/// AND controlling SLAM operations manually (start/stop localization/mapping)
+/// </summary>
+public class XlocPoseHub : Hub
+{
+    private readonly ILogger<XlocPoseHub> _logger;
+    private readonly Xloc.XlocIntegrationService? _xlocService;
+
+    public XlocPoseHub(ILogger<XlocPoseHub> logger, Xloc.XlocIntegrationService? xlocService = null)
+    {
+        _logger = logger;
+        _xlocService = xlocService;
+    }
+
+    public override async Task OnConnectedAsync()
+    {
+        _logger.LogInformation("Client connected to XlocPoseHub: {ConnectionId}", Context.ConnectionId);
+        await base.OnConnectedAsync();
+    }
+
+    public override async Task OnDisconnectedAsync(Exception? exception)
+    {
+        _logger.LogInformation("Client disconnected from XlocPoseHub: {ConnectionId}", Context.ConnectionId);
+        await base.OnDisconnectedAsync(exception);
+    }
+
+    /// <summary>
+    /// Client requests current pose (on-demand)
+    /// </summary>
+    public async Task RequestCurrentPose()
+    {
+        _logger.LogDebug("Client {ConnectionId} requested current pose", Context.ConnectionId);
+        await Task.CompletedTask;
+    }
+
+    #region Manual SLAM Control Methods
+
+    /// <summary>
+    /// Activate a map for localization
+    /// </summary>
+    public async Task<bool> ActivateMap(string mapFilePath)
+    {
+        if (_xlocService == null)
+        {
+            _logger.LogWarning("XlocIntegrationService not available");
+            return false;
+        }
+
+        _logger.LogInformation("Client {ConnectionId} activating map: {MapPath}", Context.ConnectionId, mapFilePath);
+        return _xlocService.ActivateMap(mapFilePath);
+    }
+
+    /// <summary>
+    /// Start localization mode
+    /// </summary>
+    public async Task<bool> StartLocalization()
+    {
+        if (_xlocService == null)
+        {
+            _logger.LogWarning("XlocIntegrationService not available");
+            return false;
+        }
+
+        _logger.LogInformation("Client {ConnectionId} starting localization", Context.ConnectionId);
+        return _xlocService.StartLocalization();
+    }
+
+    /// <summary>
+    /// Stop localization mode
+    /// </summary>
+    public async Task<bool> StopLocalization()
+    {
+        if (_xlocService == null)
+        {
+            _logger.LogWarning("XlocIntegrationService not available");
+            return false;
+        }
+
+        _logger.LogInformation("Client {ConnectionId} stopping localization", Context.ConnectionId);
+        return _xlocService.StopLocalization();
+    }
+
+    /// <summary>
+    /// Start mapping mode
+    /// </summary>
+    public async Task<bool> StartMapping()
+    {
+        if (_xlocService == null)
+        {
+            _logger.LogWarning("XlocIntegrationService not available");
+            return false;
+        }
+
+        _logger.LogInformation("Client {ConnectionId} starting mapping", Context.ConnectionId);
+        return _xlocService.StartMapping();
+    }
+
+    /// <summary>
+    /// Stop mapping and save to file
+    /// </summary>
+    public async Task<bool> StopMapping(string saveMapFilePath)
+    {
+        if (_xlocService == null)
+        {
+            _logger.LogWarning("XlocIntegrationService not available");
+            return false;
+        }
+
+        _logger.LogInformation("Client {ConnectionId} stopping mapping, saving to: {MapPath}", 
+            Context.ConnectionId, saveMapFilePath);
+        return _xlocService.StopMapping(saveMapFilePath);
+    }
+
+    /// <summary>
+    /// Get current 2D pose (x, y, yaw)
+    /// </summary>
+    public async Task<object?> GetCurrentPose2D()
+    {
+        if (_xlocService == null)
+            return null;
+
+        var pose = _xlocService.GetCurrentPose2D();
+        if (!pose.HasValue)
+            return null;
+
+        var (x, y, yaw) = pose.Value;
+        return new { x, y, yaw, yawDegrees = yaw * 180.0 / Math.PI };
+    }
+
+    /// <summary>
+    /// Get sampled laser scan data (1 degree intervals) for web visualization
+    /// Returns array of {angle, range} objects
+    /// </summary>
+    public async Task<object?> GetSampledLaserScan()
+    {
+        if (_xlocService == null)
+            return null;
+
+        var laserData = _xlocService.GetSampledLaserScan();
+        if (laserData == null || laserData.Count == 0)
+            return null;
+
+        // Convert to JSON-friendly format
+        var points = laserData.Select(p => new { angle = p.angle, range = p.range }).ToArray();
+        
+        return new { 
+            pointCount = points.Length,
+            points = points,
+            timestamp = DateTime.UtcNow
+        };
+    }
+
+    /// <summary>
+    /// Get XLOC diagnostics data
+    /// </summary>
+    public async Task<object?> GetDiagnostics()
+    {
+        if (_xlocService == null)
+            return null;
+
+        var diagnostics = _xlocService.GetDiagnostics();
+        if (diagnostics == null)
+            return null;
+
+        return new
+        {
+            header = new
+            {
+                seq = diagnostics.HeaderSeq,
+                stamp = new
+                {
+                    sec = diagnostics.HeaderStampSec,
+                    nsec = diagnostics.HeaderStampNsec
+                },
+                frameId = diagnostics.HeaderFrameId
+            },
+            xlocState = diagnostics.XlocState,
+            stateString = diagnostics.StateString,
+            currentActiveMap = diagnostics.CurrentActiveMap,
+            reliability = diagnostics.Reliability,
+            matchingScore = diagnostics.MatchingScore
+        };
+    }
+
+    #endregion
+}
+
+/// <summary>
+/// Pose data for SignalR clients
+/// </summary>
+public class XlocPoseData
+{
+    /// <summary>
+    /// X position in meters
+    /// </summary>
+    public double X { get; set; }
+
+    /// <summary>
+    /// Y position in meters
+    /// </summary>
+    public double Y { get; set; }
+
+    /// <summary>
+    /// Z position in meters
+    /// </summary>
+    public double Z { get; set; }
+
+    /// <summary>
+    /// Yaw angle in radians
+    /// </summary>
+    public double Yaw { get; set; }
+
+    /// <summary>
+    /// Yaw angle in degrees
+    /// </summary>
+    public double YawDegrees => Yaw * 180.0 / Math.PI;
+
+    /// <summary>
+    /// Quaternion X
+    /// </summary>
+    public double QuaternionX { get; set; }
+
+    /// <summary>
+    /// Quaternion Y
+    /// </summary>
+    public double QuaternionY { get; set; }
+
+    /// <summary>
+    /// Quaternion Z
+    /// </summary>
+    public double QuaternionZ { get; set; }
+
+    /// <summary>
+    /// Quaternion W
+    /// </summary>
+    public double QuaternionW { get; set; }
+
+    /// <summary>
+    /// Timestamp of pose
+    /// </summary>
+    public DateTime Timestamp { get; set; }
+
+    /// <summary>
+    /// Is localization active
+    /// </summary>
+    public bool IsActive { get; set; }
+
+    /// <summary>
+    /// Xloc state (0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR)
+    /// </summary>
+    public byte XlocState { get; set; }
+
+    /// <summary>
+    /// Current active map name
+    /// </summary>
+    public string MapName { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Localization reliability (0.0 to 1.0)
+    /// </summary>
+    public double Reliability { get; set; }
+
+    /// <summary>
+    /// SLAM matching score
+    /// </summary>
+    public double MatchingScore { get; set; }
+}