477 lines
16 KiB
C#
477 lines
16 KiB
C#
using Microsoft.AspNetCore.Authorization;
|
|
using Microsoft.AspNetCore.SignalR;
|
|
using RobotNet10.RobotApp.Client.Shared.SLAM;
|
|
using RobotNet10.RobotApp.Shared;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.SLAM;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.Hubs;
|
|
|
|
/// <summary>
|
|
/// SignalR Hub cho Cartographer/Localization - cung cấp real-time mapping và localization data
|
|
/// </summary>
|
|
[Authorize]
|
|
public class SLAMHub(ISLAMService _slamService, ILogger<SLAMHub> _logger) : Hub
|
|
{
|
|
/// <summary>
|
|
/// Called when a client connects to the hub
|
|
/// </summary>
|
|
public override async Task OnConnectedAsync()
|
|
{
|
|
await base.OnConnectedAsync();
|
|
// Clients will poll for occupancy grid using GetOccupancyGridBase() and GetOccupancyGridUpdating() methods
|
|
}
|
|
|
|
/// <summary>
|
|
/// Called when a client disconnects from the hub
|
|
/// </summary>
|
|
public override async Task OnDisconnectedAsync(Exception? exception)
|
|
{
|
|
await base.OnDisconnectedAsync(exception);
|
|
}
|
|
/// <summary>
|
|
/// Lấy trạng thái hiện tại của CartographerService
|
|
/// </summary>
|
|
public Task<SLAMState> GetCurrentState()
|
|
{
|
|
var state = _slamService.State;
|
|
return Task.FromResult(MapState(state));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy tên map hiện tại đang được sử dụng
|
|
/// </summary>
|
|
public Task<string?> GetCurrentMap()
|
|
{
|
|
return Task.FromResult(_slamService.CurrentMap);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Bắt đầu localization với map đã lưu
|
|
/// </summary>
|
|
public async Task<bool> StartLocalization(string mapName, PoseDto? initialPoseDto = null)
|
|
{
|
|
try
|
|
{
|
|
Pose? initialPose = initialPoseDto != null ? MapPoseFromDto(initialPoseDto) : null;
|
|
_slamService.StartLocalization(mapName, initialPose);
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to start localization");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dừng localization
|
|
/// </summary>
|
|
public Task StopLocalization()
|
|
{
|
|
_slamService.StopLocalization();
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Bắt đầu scan & mapping
|
|
/// </summary>
|
|
public async Task<bool> StartScanMapping(string mapName)
|
|
{
|
|
try
|
|
{
|
|
_slamService.StartScanMapping(mapName);
|
|
return true;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to start scan mapping");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lưu map hiện tại.
|
|
/// Note: This is now fire-and-forget. The actual save happens asynchronously.
|
|
/// Monitor state changes to know when save completes.
|
|
/// </summary>
|
|
public async Task<string?> SaveMap()
|
|
{
|
|
try
|
|
{
|
|
_slamService.SaveScanMap();
|
|
// Return null since we no longer wait for completion
|
|
// Clients should monitor state changes instead
|
|
return null;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to save map");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Liệt kê tất cả maps có sẵn
|
|
/// </summary>
|
|
public MapInfoDto[] ListMaps()
|
|
{
|
|
try
|
|
{
|
|
var maps = _slamService.ListMaps();
|
|
return [.. maps.Select(MapMapInfoToDto)];
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to list maps");
|
|
return [];
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy thông tin map và đăng ký client vào group để nhận cập nhật trạng thái xử lý
|
|
/// </summary>
|
|
/// <param name="mapName">Tên map</param>
|
|
/// <returns>MapInfoDto với trạng thái IsProcessing</returns>
|
|
public async Task<MapInfoDto?> GetMapInfoAndSubscribeProcessing(string mapName)
|
|
{
|
|
try
|
|
{
|
|
// Subscribe client to group with mapName
|
|
await Groups.AddToGroupAsync(Context.ConnectionId, mapName);
|
|
|
|
// Get map info
|
|
var mapInfo = _slamService.GetMapInfo(mapName);
|
|
if (mapInfo == null)
|
|
{
|
|
_logger.LogWarning("SLAMHub: Map not found: {MapName}", mapName);
|
|
return null;
|
|
}
|
|
|
|
// Get processing status
|
|
var isProcessing = _slamService.GetMapProcessingStatus(mapName);
|
|
|
|
// Map to DTO with IsProcessing
|
|
var dto = MapMapInfoToDto(mapInfo);
|
|
dto.IsProcessing = isProcessing;
|
|
|
|
return dto;
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SLAMHub: Failed to get map info and subscribe: {MapName}", mapName);
|
|
return null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Hủy đăng ký client khỏi group nhận cập nhật trạng thái xử lý
|
|
/// </summary>
|
|
/// <param name="mapName">Tên map</param>
|
|
public async Task UnsubscribeMapProcessing(string mapName)
|
|
{
|
|
try
|
|
{
|
|
await Groups.RemoveFromGroupAsync(Context.ConnectionId, mapName);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SLAMHub: Failed to unsubscribe from map processing: {MapName}", mapName);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Xóa map
|
|
/// </summary>
|
|
public async Task<bool> DeleteMap(string mapName)
|
|
{
|
|
try
|
|
{
|
|
return await _slamService.DeleteMapAsync(mapName);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to delete map");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Transform map origin to a new pose
|
|
/// </summary>
|
|
/// <param name="mapName">Name of the map to transform</param>
|
|
/// <param name="newOriginDto">New origin pose (position and orientation)</param>
|
|
/// <returns>True if transform was successful</returns>
|
|
public async Task<bool> TransformMapOrigin(string mapName, PoseDto newOriginDto)
|
|
{
|
|
try
|
|
{
|
|
var newOrigin = MapPoseFromDto(newOriginDto);
|
|
return await _slamService.TransformMapOriginAsync(mapName, newOrigin);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to transform map origin");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
|
|
/// This is an async operation - returns true if processing started successfully.
|
|
/// Clients subscribed to the map group will receive OnMapProcessingChanged notifications.
|
|
/// </summary>
|
|
/// <param name="mapName">Name of the map to rerender</param>
|
|
/// <param name="configDto">Custom occupancy grid configuration</param>
|
|
/// <returns>True if rerender was started successfully</returns>
|
|
public async Task<bool> RerenderMapWithConfig(string mapName, OccupancyGridConfigurationDto configDto)
|
|
{
|
|
try
|
|
{
|
|
return await _slamService.RerenderMapWithConfigAsync(mapName, configDto);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "SLAMHub: Failed to rerender map with config");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
return false;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set initial pose cho localization
|
|
/// </summary>
|
|
public async Task SetInitialPose(PoseDto poseDto)
|
|
{
|
|
try
|
|
{
|
|
var pose = MapPoseFromDto(poseDto);
|
|
_slamService.SetInitialPose(pose);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerHub: Failed to set initial pose");
|
|
await Clients.Caller.SendAsync("OnError", new ErrorDto
|
|
{
|
|
Message = ex.Message,
|
|
Timestamp = DateTime.UtcNow
|
|
});
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy occupancy grid mới hơn thời gian chỉ định
|
|
/// </summary>
|
|
/// <param name="since">Thời gian để so sánh</param>
|
|
/// <returns>OccupancyGridDto nếu có update mới hơn, null nếu không có</returns>
|
|
public OccupancyGridDto? GetOccupancyGrid(DateTime since)
|
|
{
|
|
var lastUpdated = _slamService.LastUpdatedOccupancyGrid;
|
|
var grid = _slamService.GetOccupancyGrid(since);
|
|
var trajectoryNodes = _slamService.GetTrajectoryNodes();
|
|
|
|
if (grid == null)
|
|
{
|
|
_logger.LogDebug(
|
|
"SLAMHub.GetOccupancyGrid: No grid available (since={Since}, lastUpdated={LastUpdated}, state={State})",
|
|
since, lastUpdated, _slamService.State);
|
|
return null;
|
|
}
|
|
|
|
_logger.LogDebug(
|
|
"SLAMHub.GetOccupancyGrid: Returning grid {W}x{H}, lastUpdated={LastUpdated}",
|
|
grid.Width, grid.Height, lastUpdated);
|
|
|
|
return MapOccupancyGridToDto(grid, lastUpdated, lastUpdated, versionTicks: lastUpdated.Ticks, trajectoryNodes);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy pose hiện tại của robot
|
|
/// Trả về pose từ ScanMappingService nếu đang ScanMapping, hoặc từ CartographerService nếu đang Localizing
|
|
/// </summary>
|
|
/// <returns>PoseDto nếu có pose, null nếu không có</returns>
|
|
public PoseDto GetCurrentPose()
|
|
{
|
|
var state = _slamService.State;
|
|
|
|
// Khi đang ScanMapping, lấy pose từ ScanMappingService
|
|
if (state == SLAMState.ScanMapping)
|
|
{
|
|
return MapPoseToDto(_slamService.CurrentPose, 0);
|
|
}
|
|
// Khi đang Localizing, lấy pose từ CartographerService
|
|
else if (state == SLAMState.Localizing)
|
|
{
|
|
return MapPoseToDto(_slamService.CurrentPose, _slamService.LocalizationScore ?? 0, _slamService.PoseCovariance);
|
|
}
|
|
|
|
return MapPoseToDto(new Pose(), 0);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Lấy sample point cloud từ tất cả lidar devices (đã transform sang pose-graph global frame).
|
|
/// Pipeline: LocalTrajectoryBuilder2D → trajectory-with-global-orientation, GlobalTrajectoryBuilder2D → GetLocalToGlobalTransform → global.
|
|
/// </summary>
|
|
/// <returns>Danh sách Point32 trong global (map) frame, empty list nếu không có</returns>
|
|
public Vector3[] GetSamplePointCloud()
|
|
{
|
|
var points = _slamService.GetAggregatedSamplePointCloud();
|
|
return points?.Count > 0 ? [.. points] : [];
|
|
}
|
|
|
|
// Mapping methods (convert server types to DTOs)
|
|
|
|
private static SLAMState MapState(SLAMState state)
|
|
{
|
|
return state switch
|
|
{
|
|
SLAMState.Idle => SLAMState.Idle,
|
|
SLAMState.Initializing => SLAMState.Initializing,
|
|
SLAMState.Ready => SLAMState.Ready,
|
|
SLAMState.Relocalizing => SLAMState.Relocalizing,
|
|
SLAMState.Localizing => SLAMState.Localizing,
|
|
SLAMState.ScanMapping => SLAMState.ScanMapping,
|
|
SLAMState.SavingMap => SLAMState.SavingMap,
|
|
SLAMState.Error => SLAMState.Error,
|
|
_ => SLAMState.Idle
|
|
};
|
|
}
|
|
|
|
private static Pose MapPoseFromDto(PoseDto dto)
|
|
{
|
|
return new Pose
|
|
{
|
|
Position = dto.Position,
|
|
Orientation = dto.Orientation
|
|
};
|
|
}
|
|
|
|
private static PoseDto MapPoseToDto(Pose pose, double score, Matrix3x3? covariance = null)
|
|
{
|
|
var dto = new PoseDto
|
|
{
|
|
Position = pose.Position,
|
|
Orientation = pose.Orientation,
|
|
Score = score,
|
|
Timestamp = DateTime.UtcNow
|
|
};
|
|
|
|
// Convert covariance matrix to flattened array if available
|
|
if (covariance.HasValue)
|
|
{
|
|
var cov = covariance.Value;
|
|
// Matrix3x3 uses indexer [row, col] where:
|
|
// row 0 = x, row 1 = y, row 2 = theta
|
|
// col 0 = x, col 1 = y, col 2 = theta
|
|
dto.Covariance =
|
|
[
|
|
cov[0, 0], cov[0, 1], cov[0, 2], // XX, XY, XT
|
|
cov[1, 0], cov[1, 1], cov[1, 2], // YX, YY, YT
|
|
cov[2, 0], cov[2, 1], cov[2, 2] // TX, TY, TT
|
|
];
|
|
}
|
|
|
|
return dto;
|
|
}
|
|
|
|
private OccupancyGridDto MapOccupancyGridToDto(OccupancyGrid grid, DateTime lastBaseUpdated, DateTime lastUpdated, long versionTicks, List<(int NodeId, Pose Pose)>? trajectoryNodes = null)
|
|
{
|
|
if (grid == null)
|
|
{
|
|
_logger.LogWarning("CartographerHub: MapOccupancyGridToDto called with null grid");
|
|
throw new ArgumentNullException(nameof(grid));
|
|
}
|
|
|
|
// Convert to sparse format: only include known cells (value >= 0)
|
|
// Format: byte[] với mỗi cell = 5 bytes: [index_byte0, index_byte1, index_byte2, index_byte3, value]
|
|
// Index: 4 bytes little-endian (32-bit unsigned integer)
|
|
// Value: 1 byte (0-100)
|
|
var knownCells = new List<byte>();
|
|
|
|
int knownCellCount = 0;
|
|
for (int i = 0; i < grid.Data.Length; i++)
|
|
{
|
|
var value = grid.Data[i];
|
|
if (value >= 0) // Only include known cells (skip unknown = -1)
|
|
{
|
|
knownCellCount++;
|
|
// Encode index as 4 bytes (little-endian)
|
|
knownCells.Add((byte)(i & 0xFF)); // Byte 0: LSB
|
|
knownCells.Add((byte)((i >> 8) & 0xFF)); // Byte 1
|
|
knownCells.Add((byte)((i >> 16) & 0xFF)); // Byte 2
|
|
knownCells.Add((byte)((i >> 24) & 0xFF)); // Byte 3: MSB
|
|
|
|
// Encode value as 1 byte (0-100)
|
|
knownCells.Add((byte)value);
|
|
}
|
|
}
|
|
|
|
TrajectoryNodeDto[]? trajectoryDtos = null;
|
|
if (trajectoryNodes != null && trajectoryNodes.Count > 0)
|
|
{
|
|
trajectoryDtos = [.. trajectoryNodes.Select(node => new TrajectoryNodeDto
|
|
{
|
|
NodeId = node.NodeId,
|
|
Pose = node.Pose,
|
|
Timestamp = DateTime.UtcNow
|
|
})];
|
|
}
|
|
|
|
return new OccupancyGridDto
|
|
{
|
|
Resolution = grid.Resolution,
|
|
Width = grid.Width,
|
|
Height = grid.Height,
|
|
Origin = grid.Origin,
|
|
KnownCells = [.. knownCells],
|
|
Version = versionTicks,
|
|
LastBaseUpdated = lastBaseUpdated,
|
|
LastUpdated = lastUpdated,
|
|
TrajectoryNodes = trajectoryDtos
|
|
};
|
|
}
|
|
|
|
private static MapInfoDto MapMapInfoToDto(MapInfo mapInfo)
|
|
{
|
|
return new MapInfoDto
|
|
{
|
|
Name = mapInfo.Name,
|
|
CreatedDate = mapInfo.CreatedDate,
|
|
Resolution = mapInfo.Resolution,
|
|
Width = mapInfo.Size.Width,
|
|
Height = mapInfo.Size.Height,
|
|
TrajectoryNodeCount = mapInfo.TrajectoryNodeCount,
|
|
OriginX = mapInfo.Origin.Position.X,
|
|
OriginY = mapInfo.Origin.Position.Y,
|
|
};
|
|
}
|
|
}
|