Initial commit
This commit is contained in:
476
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/SLAMHub.cs
Normal file
476
srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Hubs/SLAMHub.cs
Normal file
@@ -0,0 +1,476 @@
|
||||
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,
|
||||
};
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user