Initial commit
This commit is contained in:
@@ -0,0 +1,772 @@
|
||||
using CartographerSharp.Mapping;
|
||||
using Microsoft.AspNetCore.SignalR;
|
||||
using RobotNet10.RobotApp.Client.Shared.SLAM;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.RobotApp.Hubs;
|
||||
using RobotNet10.RobotApp.Shared;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
using RobotNet10.Shared.Numbers;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
|
||||
public partial class CartographerService
|
||||
{
|
||||
#region Map Listing and Info
|
||||
|
||||
/// <summary>
|
||||
/// Liệt kê các maps có sẵn trong thư mục maps
|
||||
/// </summary>
|
||||
public IReadOnlyList<MapInfo> ListMaps()
|
||||
{
|
||||
var mapsDirectory = Path.GetFullPath(_config.MapStorage.Directory);
|
||||
if (!Directory.Exists(mapsDirectory))
|
||||
return [];
|
||||
|
||||
try
|
||||
{
|
||||
var maps = new List<MapInfo>();
|
||||
var mapDirectories = Directory.GetDirectories(mapsDirectory);
|
||||
|
||||
foreach (var mapDir in mapDirectories)
|
||||
{
|
||||
try
|
||||
{
|
||||
var metadataPath = MapPathHelper.ResolveMetadataPath(mapDir);
|
||||
if (metadataPath != null)
|
||||
{
|
||||
var jsonContent = File.ReadAllText(metadataPath);
|
||||
var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
|
||||
if (metadata != null)
|
||||
{
|
||||
metadata = MapNameHelper.EnsureMapSize(metadata);
|
||||
maps.Add(metadata);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Create basic metadata from directory name
|
||||
var mapName = Path.GetFileName(mapDir);
|
||||
maps.Add(new MapInfo
|
||||
{
|
||||
Name = mapName,
|
||||
FolderPath = mapDir,
|
||||
CreatedDate = Directory.GetCreationTime(mapDir),
|
||||
Resolution = _config.MapStorage.OccupancyGridResolution
|
||||
});
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning(ex, "CartographerService: Failed to read metadata for map in: {MapDir}", mapDir);
|
||||
}
|
||||
}
|
||||
|
||||
return maps;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Failed to list maps");
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Lấy thông tin chi tiết của một map theo tên
|
||||
/// </summary>
|
||||
public MapInfo? GetMapInfo(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
|
||||
if (!Directory.Exists(mapPath))
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
|
||||
return null;
|
||||
}
|
||||
|
||||
var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
|
||||
|
||||
if (metadataPath == null)
|
||||
{
|
||||
// Create basic metadata from directory name
|
||||
var sanitizedMapName = MapNameHelper.Sanitize(mapName);
|
||||
return new MapInfo
|
||||
{
|
||||
Name = sanitizedMapName,
|
||||
FolderPath = mapPath,
|
||||
CreatedDate = Directory.GetCreationTime(mapPath),
|
||||
Resolution = _config.MapStorage.OccupancyGridResolution
|
||||
};
|
||||
}
|
||||
|
||||
var jsonContent = File.ReadAllText(metadataPath);
|
||||
var metadata = System.Text.Json.JsonSerializer.Deserialize<MapInfo>(jsonContent);
|
||||
if (metadata == null)
|
||||
return null;
|
||||
|
||||
metadata = MapNameHelper.EnsureMapSize(metadata);
|
||||
|
||||
return metadata;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Failed to get map info: {MapName}", mapName);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Lấy trạng thái xử lý của map
|
||||
/// </summary>
|
||||
/// <param name="mapName">Tên map</param>
|
||||
/// <returns>True nếu map đang được xử lý, false nếu không</returns>
|
||||
public bool GetMapProcessingStatus(string mapName)
|
||||
{
|
||||
return _mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing;
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Map Operations
|
||||
|
||||
/// <summary>
|
||||
/// Lấy đường dẫn đến file ảnh PNG của map
|
||||
/// </summary>
|
||||
public string? GetMapImagePath(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
|
||||
if (!Directory.Exists(mapPath))
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
|
||||
return null;
|
||||
}
|
||||
|
||||
var imagePath = MapPathHelper.ResolveImagePath(mapPath);
|
||||
if (imagePath != null)
|
||||
return imagePath;
|
||||
|
||||
_logger.LogWarning("CartographerService: Map image not found for: {MapName}", mapName);
|
||||
return null;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Failed to get map image path: {MapName}", mapName);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Xóa map folder
|
||||
/// </summary>
|
||||
public Task<bool> DeleteMapAsync(string mapName, CancellationToken cancellationToken = default)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
|
||||
if (!Directory.Exists(mapPath))
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Map directory not found: {MapPath}", mapPath);
|
||||
return Task.FromResult(false);
|
||||
}
|
||||
|
||||
Directory.Delete(mapPath, recursive: true);
|
||||
return Task.FromResult(true);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Failed to delete map: {MapName}", mapName);
|
||||
return Task.FromResult(false);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Transform map để chọn lại gốc tọa độ.
|
||||
/// Kiểm tra nếu map đang được xử lý thì return false.
|
||||
/// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
|
||||
/// </summary>
|
||||
public Task<bool> TransformMapOriginAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
|
||||
{
|
||||
// Check if map is already being processed
|
||||
if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
|
||||
return Task.FromResult(false);
|
||||
}
|
||||
|
||||
// Set processing status to true
|
||||
_mapProcessingStatus[mapName] = true;
|
||||
|
||||
// Notify clients that processing has started
|
||||
_ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);
|
||||
|
||||
// Start processing on a separate thread
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
await TransformMapOriginInternalAsync(mapName, newOrigin, cancellationToken);
|
||||
}
|
||||
finally
|
||||
{
|
||||
// Set processing status to false
|
||||
_mapProcessingStatus[mapName] = false;
|
||||
|
||||
// Notify clients that processing has completed
|
||||
await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
|
||||
}
|
||||
}, cancellationToken);
|
||||
|
||||
// Return true immediately to indicate processing has started
|
||||
return Task.FromResult(true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Internal method that performs the actual map transformation.
|
||||
/// Based on xloc.cc ChangeMapOrigin logic: updates TransformToMap in the PoseGraph proto
|
||||
/// while preserving all other data (trajectories, submaps, options, etc.).
|
||||
/// </summary>
|
||||
private async Task<bool> TransformMapOriginInternalAsync(string mapName, Pose newOrigin, CancellationToken cancellationToken = default)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
|
||||
var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
|
||||
|
||||
if (metadataPath == null)
|
||||
{
|
||||
_logger.LogError("TransformMapOrigin: Metadata file not found in: {MapPath}", mapPath);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (pbstreamPath == null)
|
||||
{
|
||||
_logger.LogError("TransformMapOrigin: Pbstream file not found in: {MapPath}", mapPath);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Load metadata
|
||||
var jsonContent = await File.ReadAllTextAsync(metadataPath, cancellationToken);
|
||||
var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);
|
||||
|
||||
if (metadata == null)
|
||||
{
|
||||
_logger.LogError("TransformMapOrigin: Failed to deserialize metadata");
|
||||
return false;
|
||||
}
|
||||
|
||||
// === Update pbstream file using shared helper ===
|
||||
// This preserves all original data (AllTrajectoryBuilderOptions, submaps, nodes, etc.)
|
||||
PbstreamTransformHelper.TransformPbstreamOrigin(pbstreamPath, newOrigin, _logger);
|
||||
|
||||
// Reload transformed map to regenerate occupancy grid and image files
|
||||
var transformedLoadResult = LoadMapFromDirectory(mapName);
|
||||
if (transformedLoadResult == null)
|
||||
{
|
||||
_logger.LogError("TransformMapOrigin: Failed to load transformed map for file regeneration");
|
||||
return false;
|
||||
}
|
||||
|
||||
MapBuilder transformedMapBuilder;
|
||||
try
|
||||
{
|
||||
transformedMapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
|
||||
transformedLoadResult,
|
||||
_config,
|
||||
_logger,
|
||||
loadFrozenState: false);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "TransformMapOrigin: Failed to create MapBuilder from transformed pbstream");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// Use MapSaveProcessor to regenerate occupancy grid files and update metadata
|
||||
if (_mapSaveProcessor == null)
|
||||
{
|
||||
_logger.LogError("TransformMapOrigin: MapSaveProcessor is not available");
|
||||
return false;
|
||||
}
|
||||
|
||||
await _mapSaveProcessor.SaveMapMetadataAsync(
|
||||
mapName,
|
||||
transformedMapBuilder,
|
||||
mapPath,
|
||||
cancellationToken);
|
||||
|
||||
return true;
|
||||
}
|
||||
finally
|
||||
{
|
||||
transformedMapBuilder.Dispose();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "TransformMapOrigin: Failed for map {MapName}", mapName);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Rerender map image files (PNG, JPG, PGM) with custom OccupancyGridConfiguration.
|
||||
/// Kiểm tra nếu map đang được xử lý thì return false.
|
||||
/// Nếu không, bắt đầu xử lý trên thread riêng và return true ngay lập tức.
|
||||
/// </summary>
|
||||
public Task<bool> RerenderMapWithConfigAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
|
||||
{
|
||||
// Check if map is already being processed
|
||||
if (_mapProcessingStatus.TryGetValue(mapName, out var isProcessing) && isProcessing)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Map {MapName} is already being processed", mapName);
|
||||
return Task.FromResult(false);
|
||||
}
|
||||
|
||||
// Set processing status to true
|
||||
_mapProcessingStatus[mapName] = true;
|
||||
|
||||
// Notify clients that processing has started
|
||||
_ = _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, true, cancellationToken);
|
||||
|
||||
// Start processing on a separate thread
|
||||
_ = Task.Run(async () =>
|
||||
{
|
||||
try
|
||||
{
|
||||
await RerenderMapWithConfigInternalAsync(mapName, configDto, cancellationToken);
|
||||
}
|
||||
finally
|
||||
{
|
||||
// Set processing status to false
|
||||
_mapProcessingStatus[mapName] = false;
|
||||
|
||||
// Notify clients that processing has completed
|
||||
await _hubContext.Clients.Group(mapName).SendAsync("OnMapProcessingChanged", mapName, false, cancellationToken);
|
||||
}
|
||||
}, cancellationToken);
|
||||
|
||||
// Return true immediately to indicate processing has started
|
||||
return Task.FromResult(true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Internal method that performs the actual map rerendering.
|
||||
/// Loads pbstream, generates occupancy grid with custom config, and saves image files.
|
||||
/// </summary>
|
||||
private async Task<bool> RerenderMapWithConfigInternalAsync(string mapName, OccupancyGridConfigurationDto configDto, CancellationToken cancellationToken = default)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
|
||||
|
||||
if (pbstreamPath == null)
|
||||
{
|
||||
_logger.LogError("RerenderMapWithConfig: Pbstream file not found in: {MapPath}", mapPath);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Load map from pbstream
|
||||
var loadResult = LoadMapFromDirectory(mapName);
|
||||
if (loadResult == null)
|
||||
{
|
||||
_logger.LogError("RerenderMapWithConfig: Failed to load map from directory");
|
||||
return false;
|
||||
}
|
||||
|
||||
MapBuilder mapBuilder;
|
||||
try
|
||||
{
|
||||
mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
|
||||
loadResult,
|
||||
_config,
|
||||
_logger,
|
||||
loadFrozenState: false);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "RerenderMapWithConfig: Failed to create MapBuilder from pbstream");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
// === DEBUG: Log PoseGraph state after loading ===
|
||||
var poseGraph = mapBuilder.PoseGraph;
|
||||
var transformToMap = poseGraph.GetTransformToMap();
|
||||
_logger.LogWarning(
|
||||
"RerenderMapWithConfig DEBUG: TransformToMap = T[{TX:F4}, {TY:F4}, {TZ:F4}], R[{RW:F4}, {RX:F4}, {RY:F4}, {RZ:F4}]",
|
||||
transformToMap.Translation.X, transformToMap.Translation.Y, transformToMap.Translation.Z,
|
||||
transformToMap.Rotation.W, transformToMap.Rotation.X, transformToMap.Rotation.Y, transformToMap.Rotation.Z);
|
||||
|
||||
var allSubmapData = poseGraph.GetAllSubmapData();
|
||||
_logger.LogWarning("RerenderMapWithConfig DEBUG: Total submaps = {Count}", allSubmapData?.Count ?? 0);
|
||||
|
||||
// Log LocalToGlobalTransform for each trajectory
|
||||
try
|
||||
{
|
||||
var trajectoryStates = poseGraph.GetTrajectoryStates();
|
||||
_logger.LogWarning("RerenderMapWithConfig DEBUG: Found {Count} trajectories", trajectoryStates?.Count ?? 0);
|
||||
if (trajectoryStates != null)
|
||||
{
|
||||
foreach (var trajState in trajectoryStates)
|
||||
{
|
||||
var trajId = trajState.Key;
|
||||
var localToGlobal = poseGraph.GetLocalToGlobalTransform(trajId);
|
||||
_logger.LogWarning(
|
||||
"RerenderMapWithConfig DEBUG: Trajectory[{TrajId}] State={State}, LocalToGlobalTransform = " +
|
||||
"T[{TX:F4},{TY:F4},{TZ:F4}], R[{RW:F4},{RX:F4},{RY:F4},{RZ:F4}]",
|
||||
trajId, trajState.Value,
|
||||
localToGlobal.Translation.X, localToGlobal.Translation.Y, localToGlobal.Translation.Z,
|
||||
localToGlobal.Rotation.W, localToGlobal.Rotation.X, localToGlobal.Rotation.Y, localToGlobal.Rotation.Z);
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning("RerenderMapWithConfig DEBUG: Failed to get trajectory states: {Error}", ex.Message);
|
||||
}
|
||||
|
||||
int submapIdx = 0;
|
||||
foreach (var idDataRef in allSubmapData)
|
||||
{
|
||||
var submapId = idDataRef.Id;
|
||||
var submapData = idDataRef.Data;
|
||||
var submapPose = submapData.Pose;
|
||||
|
||||
// Calculate globalPose as OccupancyGridGenerator does
|
||||
var transformToMapInverse = transformToMap.Inverse();
|
||||
var globalPose = transformToMapInverse * submapPose;
|
||||
|
||||
if (submapData.Submap is CartographerSharp.Mapping.D2D.Submap2D submap2D)
|
||||
{
|
||||
var localPose = submap2D.LocalPose;
|
||||
var grid = submap2D.Grid;
|
||||
var gridInfo = grid != null
|
||||
? $"cells={grid.Limits.CellLimits.NumXCells}x{grid.Limits.CellLimits.NumYCells}, res={grid.Limits.Resolution:F4}"
|
||||
: "null";
|
||||
|
||||
// Calculate yaw from quaternion components
|
||||
static double GetYawDegreesFromComponents(double qw, double qx, double qy, double qz)
|
||||
{
|
||||
var siny_cosp = 2.0 * (qw * qz + qx * qy);
|
||||
var cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
|
||||
return Math.Atan2(siny_cosp, cosy_cosp) * 180.0 / Math.PI;
|
||||
}
|
||||
|
||||
var localYaw = GetYawDegreesFromComponents(
|
||||
localPose.Rotation.W, localPose.Rotation.X, localPose.Rotation.Y, localPose.Rotation.Z);
|
||||
var submapPoseYaw = GetYawDegreesFromComponents(
|
||||
submapPose.Rotation.W, submapPose.Rotation.X, submapPose.Rotation.Y, submapPose.Rotation.Z);
|
||||
var globalPoseYaw = GetYawDegreesFromComponents(
|
||||
globalPose.Rotation.W, globalPose.Rotation.X, globalPose.Rotation.Y, globalPose.Rotation.Z);
|
||||
|
||||
_logger.LogWarning(
|
||||
"RerenderMapWithConfig DEBUG: Submap[{Idx}] Id={TrajId}:{SubIdx}, " +
|
||||
"LocalPose=T[{LPX:F4},{LPY:F4}] Yaw={LPYaw:F2}°, " +
|
||||
"SubmapData.Pose=T[{SPX:F4},{SPY:F4}] Yaw={SPYaw:F2}°, " +
|
||||
"GlobalPose=T[{GPX:F4},{GPY:F4}] Yaw={GPYaw:F2}°, " +
|
||||
"Grid={GridInfo}",
|
||||
submapIdx, submapId.TrajectoryId, submapId.SubmapIndex,
|
||||
localPose.Translation.X, localPose.Translation.Y, localYaw,
|
||||
submapPose.Translation.X, submapPose.Translation.Y, submapPoseYaw,
|
||||
globalPose.Translation.X, globalPose.Translation.Y, globalPoseYaw,
|
||||
gridInfo);
|
||||
}
|
||||
submapIdx++;
|
||||
}
|
||||
// === END DEBUG ===
|
||||
|
||||
// Convert DTO to OccupancyGridConfiguration
|
||||
var config = ConvertDtoToConfig(configDto);
|
||||
|
||||
// Generate occupancy grid with custom config
|
||||
var resolution = _config.MapStorage.OccupancyGridResolution;
|
||||
var padding = _config.MapStorage.MapPadding;
|
||||
var occupancyGrid = OccupancyGridGenerator.Generate(mapBuilder, resolution, padding, _logger, config);
|
||||
|
||||
if (occupancyGrid == null)
|
||||
{
|
||||
_logger.LogError("RerenderMapWithConfig: Failed to generate occupancy grid");
|
||||
return false;
|
||||
}
|
||||
|
||||
// === DEBUG: Log OccupancyGrid result ===
|
||||
_logger.LogWarning(
|
||||
"RerenderMapWithConfig DEBUG: OccupancyGrid size={W}x{H}, resolution={Res:F4}, " +
|
||||
"Origin=T[{OX:F4},{OY:F4}]",
|
||||
occupancyGrid.Width, occupancyGrid.Height, occupancyGrid.Resolution,
|
||||
occupancyGrid.Origin.Position.X, occupancyGrid.Origin.Position.Y);
|
||||
// === END DEBUG ===
|
||||
|
||||
// Save all image formats (PGM, PNG, JPG, YAML)
|
||||
await OccupancyGridFileHelper.SaveAllFormatsAsync(occupancyGrid, mapPath, cancellationToken, _logger);
|
||||
|
||||
_logger.LogInformation("RerenderMapWithConfig: Successfully rerendered map {MapName} with custom config", mapName);
|
||||
return true;
|
||||
}
|
||||
finally
|
||||
{
|
||||
mapBuilder.Dispose();
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "RerenderMapWithConfig: Failed for map {MapName}", mapName);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Convert OccupancyGridConfigurationDto to OccupancyGridConfiguration
|
||||
/// </summary>
|
||||
private static OccupancyGridConfiguration ConvertDtoToConfig(OccupancyGridConfigurationDto dto)
|
||||
{
|
||||
return new OccupancyGridConfiguration
|
||||
{
|
||||
MergeStrategy = dto.MergeStrategy switch
|
||||
{
|
||||
SubmapMergeStrategyDto.PorterDuff => SubmapMergeStrategy.PorterDuff,
|
||||
SubmapMergeStrategyDto.LogOddsSum => SubmapMergeStrategy.LogOddsSum,
|
||||
SubmapMergeStrategyDto.MaxProbability => SubmapMergeStrategy.MaxProbability,
|
||||
_ => SubmapMergeStrategy.LogOddsSum
|
||||
},
|
||||
LogOddsClamp = dto.LogOddsClamp,
|
||||
UseLogOddsAverage = dto.UseLogOddsAverage,
|
||||
FreeSpaceThreshold = dto.FreeSpaceThreshold,
|
||||
OccupiedSpaceThreshold = dto.OccupiedSpaceThreshold,
|
||||
UseBinaryOutput = dto.UseBinaryOutput,
|
||||
EnableWallThinning = dto.EnableWallThinning,
|
||||
WallThinningIterations = dto.WallThinningIterations,
|
||||
MinWallThicknessPixels = dto.MinWallThicknessPixels,
|
||||
AmbiguousCellValue = dto.AmbiguousCellValue,
|
||||
AmbiguousRangeLower = dto.AmbiguousRangeLower,
|
||||
AmbiguousRangeUpper = dto.AmbiguousRangeUpper,
|
||||
EnableMedianFilter = dto.EnableMedianFilter,
|
||||
MedianFilterKernelSize = dto.MedianFilterKernelSize
|
||||
};
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Map Loading
|
||||
|
||||
/// <summary>
|
||||
/// Load map files directly from map directory
|
||||
/// </summary>
|
||||
private MapCartographerLoadResult? LoadMapFromDirectory(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
|
||||
if (!Directory.Exists(mapPath))
|
||||
{
|
||||
_logger.LogError("CartographerService: Map directory not found: {MapPath}", mapPath);
|
||||
return null;
|
||||
}
|
||||
|
||||
// Find pbstream file (map.pbstream or legacy)
|
||||
var pbstreamPath = MapPathHelper.ResolvePbstreamPath(mapPath);
|
||||
if (pbstreamPath == null)
|
||||
{
|
||||
_logger.LogError("CartographerService: No pbstream file found in: {MapPath}", mapPath);
|
||||
return null;
|
||||
}
|
||||
|
||||
if (Path.GetFileName(pbstreamPath) != "map.pbstream")
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Using legacy pbstream file: {Path}", pbstreamPath);
|
||||
}
|
||||
|
||||
// Load metadata to get saved config (map.json)
|
||||
MapCartographerConfigSnapshot? savedConfig = null;
|
||||
var metadataPath = MapPathHelper.ResolveMetadataPath(mapPath);
|
||||
if (metadataPath != null)
|
||||
{
|
||||
if (Path.GetFileName(metadataPath) == "metadata.json")
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Using legacy metadata.json file");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var jsonContent = File.ReadAllText(metadataPath);
|
||||
var metadata = System.Text.Json.JsonSerializer.Deserialize<MapCartographerInfo>(jsonContent);
|
||||
savedConfig = metadata?.MapConfig;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogWarning(ex, "CartographerService: Failed to load metadata, will use current config");
|
||||
}
|
||||
}
|
||||
|
||||
return new MapCartographerLoadResult
|
||||
{
|
||||
PbstreamPath = pbstreamPath,
|
||||
MapPath = mapPath,
|
||||
SavedConfig = savedConfig
|
||||
};
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Failed to load map: {MapName}", mapName);
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private bool MapExists(string mapName)
|
||||
{
|
||||
try
|
||||
{
|
||||
var mapPath = MapPathHelper.GetMapPath(_config.MapStorage.Directory, mapName);
|
||||
return Directory.Exists(mapPath);
|
||||
}
|
||||
catch
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region Wall Alignment
|
||||
|
||||
/// <summary>
|
||||
/// Calculate wall-aligned pose by detecting walls from lidar scans.
|
||||
/// This method should be called AFTER sensors are resumed to ensure fresh lidar data.
|
||||
/// The aligned pose should be set on LocalTrajectoryBuilder2D using SetInitialPose()
|
||||
/// BEFORE resuming sensors for scan mapping.
|
||||
/// </summary>
|
||||
/// <returns>Wall-aligned pose if successful, null otherwise</returns>
|
||||
private Pose CalculateWallAlignedPoseAsync()
|
||||
{
|
||||
_logger.LogInformation("CartographerService: Starting wall alignment calculation");
|
||||
|
||||
// Step 1: Get all lidar devices
|
||||
var allDevices = _deviceProvider.GetDevicesByType(RobotNet10.RobotApp.Client.Shared.Devices.DeviceType.Lidar);
|
||||
var lidarList = allDevices.OfType<ILidar>().ToList();
|
||||
|
||||
if (lidarList.Count == 0)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: No lidar devices found for wall alignment");
|
||||
return new Pose();
|
||||
}
|
||||
|
||||
_logger.LogInformation("CartographerService: Found {Count} lidar device(s)", lidarList.Count);
|
||||
|
||||
// Step 2: Collect point clouds from all lidars and transform to base_link frame
|
||||
var allPoints = new List<Vector3>();
|
||||
|
||||
foreach (var lidar in lidarList)
|
||||
{
|
||||
try
|
||||
{
|
||||
var deviceId = (lidar as DeviceBase)?.DeviceId;
|
||||
if (string.IsNullOrEmpty(deviceId))
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Lidar device has no ID, skipping");
|
||||
continue;
|
||||
}
|
||||
|
||||
var laserScan = lidar.CurrentMeasurementData;
|
||||
if (!laserScan.HasValue)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: No scan data from lidar {DeviceId}", deviceId);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Get sensor configuration and transform
|
||||
var sensorConfig = _config.Sensors.Lidars
|
||||
.FirstOrDefault(cfg => cfg.DeviceId == deviceId && cfg.Enabled);
|
||||
|
||||
if (sensorConfig == null)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: No configuration found for lidar {DeviceId}", deviceId);
|
||||
continue;
|
||||
}
|
||||
|
||||
var scan = laserScan.Value;
|
||||
|
||||
// Convert LaserScan to point cloud in base_link frame
|
||||
var (baseLink, _, _, _, _) = SensorDataTransformHelper.ToTimedPointCloudDataBaseAndSensorFrame(
|
||||
scan.Header.Stamp,
|
||||
scan,
|
||||
sensorConfig.Transform,
|
||||
sensorConfig.AngleMin,
|
||||
sensorConfig.AngleMax
|
||||
);
|
||||
|
||||
// Extract points in base_link frame (use .Ranges instead of .Points)
|
||||
int pointsAdded = 0;
|
||||
foreach (var point in baseLink.Ranges)
|
||||
{
|
||||
allPoints.Add(point.Position);
|
||||
pointsAdded++;
|
||||
}
|
||||
|
||||
_logger.LogInformation(
|
||||
"CartographerService: Collected {Count} points from lidar {DeviceId}",
|
||||
pointsAdded, deviceId);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "CartographerService: Error processing lidar data for wall alignment");
|
||||
}
|
||||
}
|
||||
|
||||
if (allPoints.Count == 0)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: No points collected for wall alignment");
|
||||
return new Pose();
|
||||
}
|
||||
|
||||
_logger.LogInformation("CartographerService: Total points collected: {Count}", allPoints.Count);
|
||||
|
||||
// Step 3: Detect wall and calculate compensation angle
|
||||
var compensationAngle = WallAlignmentHelper.DetectWallAndCalculateCompensation(allPoints, _logger);
|
||||
|
||||
if (!compensationAngle.HasValue)
|
||||
{
|
||||
_logger.LogWarning("CartographerService: Could not detect wall for alignment");
|
||||
return new Pose();
|
||||
}
|
||||
|
||||
// Step 4: Calculate wall-aligned pose
|
||||
// Start with identity pose (robot at origin, facing along X-axis)
|
||||
// Then apply the compensation angle to align with detected wall
|
||||
var alignedOrientation = WallAlignmentHelper.CreateQuaternionFromYaw(compensationAngle.Value);
|
||||
|
||||
var alignedPose = new Pose
|
||||
{
|
||||
Position = Vector3.Zero, // Start at origin
|
||||
Orientation = alignedOrientation // Oriented to align map with wall
|
||||
};
|
||||
|
||||
_logger.LogInformation(
|
||||
"CartographerService: Calculated wall-aligned pose - " +
|
||||
"Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°), " +
|
||||
"Compensation angle: {CompAngle:F3}rad ({CompAngleDeg:F1}°)",
|
||||
alignedPose.Position.X,
|
||||
alignedPose.Position.Y,
|
||||
alignedPose.Position.Z,
|
||||
WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation),
|
||||
WallAlignmentHelper.GetYawFromQuaternion(alignedPose.Orientation) * 180.0 / Math.PI,
|
||||
compensationAngle.Value,
|
||||
compensationAngle.Value * 180.0 / Math.PI);
|
||||
|
||||
return alignedPose;
|
||||
}
|
||||
|
||||
#endregion
|
||||
}
|
||||
Reference in New Issue
Block a user