740 lines
31 KiB
C#
740 lines
31 KiB
C#
using CartographerSharp.Mapping;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using Microsoft.AspNetCore.SignalR;
|
|
using Microsoft.Extensions.Options;
|
|
using RobotNet10.RobotApp.Shared;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
public partial class CartographerService
|
|
{
|
|
#region Idle State
|
|
|
|
private void OnIdleStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Idle;
|
|
_lastError = null;
|
|
_lastException = null;
|
|
}
|
|
_ = NotifyStateChangedAsync(SLAMState.Idle);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Initializing State
|
|
|
|
private async void OnInitializingStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Initializing;
|
|
_lastError = null;
|
|
}
|
|
_ = NotifyStateChangedAsync(SLAMState.Initializing);
|
|
|
|
try
|
|
{
|
|
if (_config.Enable)
|
|
{
|
|
// Full SLAM mode: initialize devices and sensor pipeline
|
|
// Wait for devices to be loaded and connected
|
|
var devicesLoaded = await _deviceProvider.WaitForDevicesLoadedAsync(TimeSpan.FromSeconds(30));
|
|
if (!devicesLoaded)
|
|
{
|
|
_logger.LogError("CartographerService: Timeout waiting for devices to be loaded");
|
|
throw new TimeoutException("Timeout waiting for devices to be loaded");
|
|
}
|
|
|
|
var devicesConnected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(60));
|
|
if (!devicesConnected)
|
|
{
|
|
_logger.LogWarning("Some devices are not connected, but continuing initialization");
|
|
}
|
|
|
|
// Initialize sensor manager (subscribes to all sensors)
|
|
if (_sensorPipeline != null)
|
|
{
|
|
await _sensorPipeline.InitializeAsync();
|
|
}
|
|
|
|
// Create MapBuilder and add trajectory builder using helper
|
|
_mapBuilder = MapBuilderHelper.CreateMapBuilder(_config);
|
|
(_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);
|
|
|
|
// Check for auto-resume: if CurrentMap.name exists with valid map and pose, auto-start localization
|
|
var savedMapName = LoadCurrentMapName();
|
|
if (!string.IsNullOrEmpty(savedMapName) && MapExists(savedMapName) && PoseFileExists(savedMapName))
|
|
{
|
|
var savedPose = LoadPoseFromFile(savedMapName);
|
|
if (savedPose.HasValue)
|
|
{
|
|
_logger.LogInformation(
|
|
"CartographerService: Auto-resume detected - Map: {MapName}, Pose: ({X:F3}, {Y:F3}, {Z:F3})",
|
|
savedMapName, savedPose.Value.Position.X, savedPose.Value.Position.Y, savedPose.Value.Position.Z);
|
|
|
|
// Set pending localization data
|
|
lock (_localizationLock)
|
|
{
|
|
_pendingLocalizationMapName = savedMapName;
|
|
_pendingLocalizationInitialPose = savedPose;
|
|
_isSetInitialPoseFlow = false;
|
|
}
|
|
|
|
// Fire InitializationComplete to transition to Ready, then StartLocalization to auto-resume
|
|
FireStateMachine(CartographerTrigger.InitializationComplete);
|
|
FireStateMachine(CartographerTrigger.StartLocalization);
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
FireStateMachine(CartographerTrigger.InitializationComplete);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Initialization failed");
|
|
lock (_lock)
|
|
{
|
|
_lastError = ex.Message;
|
|
_lastException = ex;
|
|
}
|
|
FireStateMachine(CartographerTrigger.InitializationFailed);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Ready State
|
|
|
|
private void OnReadyStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Ready;
|
|
}
|
|
|
|
// Pause sensor subscriptions to stop receiving new data
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
|
|
// Wait for in-flight sensor data to be processed
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
|
|
// Notify state change (fire-and-forget)
|
|
_ = NotifyStateChangedAsync(SLAMState.Ready);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Relocalizing State
|
|
|
|
private void OnInitializingLocalizingStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Relocalizing;
|
|
}
|
|
|
|
// Notify state change
|
|
_ = NotifyStateChangedAsync(SLAMState.Relocalizing);
|
|
|
|
if (_config.Enable)
|
|
{
|
|
// Start background task to handle localization setup
|
|
_ = Task.Run(async () =>
|
|
{
|
|
try
|
|
{
|
|
// Get pending data and determine flow type
|
|
string? mapName;
|
|
Pose initialPose;
|
|
bool isSetInitialPoseFlow;
|
|
|
|
lock (_localizationLock)
|
|
{
|
|
mapName = _pendingLocalizationMapName;
|
|
initialPose = _pendingLocalizationInitialPose ?? new();
|
|
isSetInitialPoseFlow = _isSetInitialPoseFlow;
|
|
_isSetInitialPoseFlow = false; // Reset flag after reading
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(mapName))
|
|
{
|
|
throw new InvalidOperationException("No map name set for localization");
|
|
}
|
|
|
|
_logger.LogInformation("CartographerService: Background localization task started for map: {MapName}, isSetInitialPoseFlow: {IsSetInitialPoseFlow}", mapName, isSetInitialPoseFlow);
|
|
|
|
IMapBuilder? mapBuilder;
|
|
bool useMcl = _config.Mcl.Enabled && _mcl != null;
|
|
|
|
// =================================================================================
|
|
// STEP 1: Prepare MapBuilder
|
|
// =================================================================================
|
|
if (isSetInitialPoseFlow)
|
|
{
|
|
// SetInitialPose flow: Finish old trajectory, reuse existing MapBuilder
|
|
int oldTrajectoryId;
|
|
lock (_lock)
|
|
{
|
|
mapBuilder = _mapBuilder ?? throw new InvalidOperationException("MapBuilder is null during SetInitialPose flow");
|
|
|
|
oldTrajectoryId = _trajectoryId;
|
|
|
|
// Finish old trajectory if exists
|
|
if (oldTrajectoryId >= 0)
|
|
{
|
|
_logger.LogInformation("CartographerService: Finishing old trajectory {TrajectoryId} before restarting with new pose", oldTrajectoryId);
|
|
mapBuilder.FinishTrajectory(oldTrajectoryId);
|
|
_trajectoryBuilder = null;
|
|
_trajectoryId = -1;
|
|
}
|
|
}
|
|
|
|
// Wait for trajectory to finish
|
|
if (oldTrajectoryId >= 0)
|
|
{
|
|
await mapBuilder.WaitForTrajectoryFinishedAsync(oldTrajectoryId, CancellationToken.None);
|
|
|
|
// Verify trajectory is finished (defensive check)
|
|
int currentTrajId;
|
|
lock (_lock)
|
|
{
|
|
currentTrajId = _trajectoryId;
|
|
}
|
|
|
|
if (currentTrajId != -1)
|
|
{
|
|
_logger.LogError("CartographerService: Trajectory {OldId} not finished after wait, current trajectory: {CurrentId}",
|
|
oldTrajectoryId, currentTrajId);
|
|
throw new InvalidOperationException($"Old trajectory {oldTrajectoryId} not finished properly");
|
|
}
|
|
|
|
_logger.LogInformation("CartographerService: Verified trajectory {TrajectoryId} is finished", oldTrajectoryId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// StartLocalization flow: Load map from storage
|
|
_logger.LogInformation("CartographerService: Loading map from storage for map: {MapName}", mapName);
|
|
|
|
// Load map files directly from configuration
|
|
var loadResult = LoadMapFromDirectory(mapName)
|
|
?? throw new InvalidOperationException($"CartographerService: Failed to load map files: {mapName}");
|
|
|
|
mapBuilder = MapBuilderHelper.CreateMapBuilderFromLoadResult(
|
|
loadResult,
|
|
_config,
|
|
_logger,
|
|
loadFrozenState: true);
|
|
|
|
lock (_lock)
|
|
{
|
|
// Reset trajectory from old MapBuilder (created in Initializing state)
|
|
// before assigning new MapBuilder from pbstream
|
|
_trajectoryId = -1;
|
|
_trajectoryBuilder = null;
|
|
|
|
_mapBuilder = mapBuilder;
|
|
_currentMapName = mapName;
|
|
}
|
|
|
|
// Load occupancy grid from PGM
|
|
_occupancyGridManager.LoadFromPgm(mapName);
|
|
}
|
|
|
|
// =================================================================================
|
|
// STEP 2: Determine if we can use MCL
|
|
// =================================================================================
|
|
bool canUseMcl = useMcl && _occupancyGridManager.OccupancyGridMcl != null;
|
|
_logger.LogInformation("CartographerService: MCL check - useMcl: {UseMcl}, OccupancyGridMcl != null: {GridNotNull}, canUseMcl: {CanUseMcl}",
|
|
useMcl, _occupancyGridManager.OccupancyGridMcl != null, canUseMcl);
|
|
|
|
// =================================================================================
|
|
// STEP 3: Resume sensors AFTER map is prepared (only if sensor pipeline exists)
|
|
// =================================================================================
|
|
_sensorPipeline?.ResumeSubscriptions();
|
|
|
|
// =================================================================================
|
|
// STEP 4: Start localization - MCL path or non-MCL path
|
|
// =================================================================================
|
|
if (canUseMcl && _mcl != null && _mclProcessor != null)
|
|
{
|
|
if (_occupancyGridManager.OccupancyGridMcl is null)
|
|
{
|
|
throw new InvalidOperationException("OccupancyGridMcl is null despite canUseMcl being true");
|
|
}
|
|
// MCL path: Start MCL processor, trajectory will be added when MCL converges
|
|
var grid = _occupancyGridManager.OccupancyGridMcl;
|
|
var seedPose = initialPose;
|
|
|
|
_mcl.SetMap(grid);
|
|
_mcl.SetInitialPose(seedPose, useTightNoise: true);
|
|
|
|
var effectivePrimary = GetEffectiveMclPrimaryLidarIdAndLog();
|
|
SetMclBaseLinkToLaser(effectivePrimary, _mcl);
|
|
|
|
// Start MCL processor (will run until convergence, then fire MclConverged trigger in RunMclOnScan)
|
|
_mclProcessor.Start(seedPose, effectivePrimary, grid);
|
|
|
|
_logger.LogInformation(
|
|
"CartographerService: MCL started for map {MapName} with pose [{X}, {Y}, {Z}]; trajectory will be added when MCL converges.",
|
|
mapName, seedPose.Position.X, seedPose.Position.Y, seedPose.Position.Z);
|
|
|
|
OnPoseUpdated(seedPose, null);
|
|
|
|
// Note: MclConverged trigger will be fired by RunMclOnScan when MCL converges
|
|
}
|
|
else
|
|
{
|
|
// Non-MCL path: Add trajectory immediately, then fire MclSkipped
|
|
lock (_lock)
|
|
{
|
|
if (_mapBuilder != null)
|
|
{
|
|
_logger.LogInformation("CartographerService: Adding trajectory without MCL (MCL disabled or no occupancy grid) with initial pose: [{X}, {Y}, {Z}]",
|
|
initialPose.Position.X, initialPose.Position.Y, initialPose.Position.Z);
|
|
var (trajId, trajBuilder, poseInMapFrame) = _mapBuilder.AddLocalizationTrajectoryBuilder(_config, initialPose);
|
|
_trajectoryId = trajId;
|
|
_trajectoryBuilder = trajBuilder;
|
|
// NOTE: Don't call StartRelocalization - AddLocalizationTrajectoryBuilder already
|
|
// sets InitialTrajectoryPose with the correct pose. StartRelocalization would
|
|
// trigger redundant constraint-based relocalization and cause excessive optimization.
|
|
|
|
OnPoseUpdated(initialPose, null);
|
|
}
|
|
}
|
|
|
|
// Fire MclSkipped to transition to Localizing
|
|
_logger.LogInformation("CartographerService: Trajectory added without MCL; firing MclSkipped to transition to Localizing");
|
|
FireStateMachine(CartographerTrigger.MclSkipped);
|
|
}
|
|
|
|
_logger.LogInformation("CartographerService: Background localization task completed successfully for map: {MapName}", mapName);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Background localization task failed");
|
|
|
|
// Update error tracking
|
|
lock (_lock)
|
|
{
|
|
_lastError = ex.Message;
|
|
_lastException = ex;
|
|
_mapBuilder = null;
|
|
_trajectoryBuilder = null;
|
|
_trajectoryId = -1;
|
|
_currentMapName = null;
|
|
}
|
|
|
|
// Fire ErrorOccurred trigger to transition to Error state
|
|
FireStateMachine(CartographerTrigger.ErrorOccurred);
|
|
}
|
|
});
|
|
}
|
|
else
|
|
{
|
|
if (string.IsNullOrEmpty(_pendingLocalizationMapName))
|
|
{
|
|
FireStateMachine(CartographerTrigger.ErrorOccurred);
|
|
_logger.LogError("Start localization with no map name");
|
|
}
|
|
else
|
|
{
|
|
_currentMapName = _pendingLocalizationMapName;
|
|
if (_pendingLocalizationInitialPose.HasValue)
|
|
{
|
|
var initPose = _pendingLocalizationInitialPose ?? new();
|
|
OnPoseUpdated(initPose, null);
|
|
}
|
|
// Load occupancy grid from PGM
|
|
_occupancyGridManager.LoadFromPgm(_pendingLocalizationMapName);
|
|
FireStateMachine(CartographerTrigger.MclSkipped);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void OnInitializingLocalizingStateExit()
|
|
{
|
|
// Pause sensors when leaving this state (will be resumed again in Localizing if transitioning there)
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Localizing State
|
|
|
|
private void OnLocalizingStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Localizing;
|
|
}
|
|
|
|
// Log trajectory builder status for debugging
|
|
_logger.LogInformation(
|
|
"CartographerService: Entered Localizing state - trajectoryBuilder={TbNotNull}, trajectoryId={TrajId}",
|
|
_trajectoryBuilder != null, _trajectoryId);
|
|
|
|
try
|
|
{
|
|
// Save current map name to file for auto-resume on next startup
|
|
var mapName = _currentMapName;
|
|
if (!string.IsNullOrEmpty(mapName))
|
|
{
|
|
SaveCurrentMapName(mapName);
|
|
}
|
|
|
|
// Reset drift detector for new localization session
|
|
_driftDetector.Reset();
|
|
|
|
// Start pose sync stopwatch for periodic pose saving
|
|
_poseSyncStopwatch.Restart();
|
|
|
|
// Start point cloud update stopwatch for throttled point cloud updates
|
|
_pointCloudUpdateStopwatch.Restart();
|
|
|
|
// Just resume subscriptions - no special handling needed
|
|
// (SetInitialPose flow is handled in OnInitializingLocalizingStateEntry)
|
|
_logger.LogInformation("CartographerService: Resuming sensor subscriptions for Localizing state");
|
|
_sensorPipeline?.ResumeSubscriptions();
|
|
_logger.LogInformation("CartographerService: Sensor subscriptions resumed successfully");
|
|
|
|
// Start background pose extrapolation thread at 100Hz
|
|
StartPoseThread(10);
|
|
_logger.LogInformation("CartographerService: Pose thread started for Localizing state");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Error during OnLocalizingStateEntry");
|
|
throw;
|
|
}
|
|
|
|
_ = NotifyStateChangedAsync(SLAMState.Localizing);
|
|
}
|
|
|
|
private void OnLocalizingStateExit()
|
|
{
|
|
// Pause sensor subscriptions when leaving Localizing state
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
|
|
// Stop background pose thread before cleanup
|
|
StopPoseThread();
|
|
|
|
// Stop MCL processor if running
|
|
_mclProcessor?.Stop();
|
|
|
|
// Stop pose sync stopwatch and save final pose
|
|
_poseSyncStopwatch.Stop();
|
|
var mapName = _currentMapName;
|
|
if (!string.IsNullOrEmpty(mapName))
|
|
{
|
|
SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
|
|
}
|
|
|
|
// Check if this is SetInitialPose flow (transition back to InitializingLocalizing)
|
|
bool isSetInitialPoseFlow;
|
|
lock (_localizationLock)
|
|
{
|
|
isSetInitialPoseFlow = _isSetInitialPoseFlow;
|
|
}
|
|
|
|
// Cleanup localization resources when transitioning to Ready (StopLocalization trigger)
|
|
// But keep resources when transitioning back to InitializingLocalizing (SetInitialPose) or self-transitioning
|
|
var nextState = State; // Will be updated after this exits
|
|
if (!isSetInitialPoseFlow && (nextState == SLAMState.Ready || _currentState == SLAMState.Ready))
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_mapBuilder?.FinishTrajectoryAndDispose(_trajectoryId);
|
|
_trajectoryBuilder = null;
|
|
_trajectoryId = -1;
|
|
_mapBuilder = null;
|
|
_currentMapName = null;
|
|
|
|
_poseCovariance = null;
|
|
|
|
_constraintCache.Invalidate();
|
|
}
|
|
|
|
Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);
|
|
|
|
// Clear localization data from processors
|
|
_slamResultProcessor?.ClearLocalizationData();
|
|
|
|
_logger.LogInformation("CartographerService: Localization resources cleaned up");
|
|
}
|
|
else if (isSetInitialPoseFlow)
|
|
{
|
|
_logger.LogInformation("CartographerService: SetInitialPose flow detected, keeping MapBuilder for restart");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region ScanMapping State
|
|
|
|
private void OnScanMappingStateEntry()
|
|
{
|
|
if (!_config.Enable) return;
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.ScanMapping;
|
|
}
|
|
Volatile.Write(ref _poseSnapshot, PoseSnapshot.Empty);
|
|
Volatile.Write(ref _lastMatchingScore, -1.0);
|
|
_ = NotifyStateChangedAsync(SLAMState.ScanMapping);
|
|
|
|
_ = Task.Run(async () =>
|
|
{
|
|
// IMPORTANT: Sensors are already PAUSED from previous state (OnScanMappingStateExit or OnReadyStateEntry)
|
|
// This ensures no sensor data is processed before we set the initial pose
|
|
|
|
// Wait for in-flight sensor data to be processed
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
|
|
// Reset processors for new scan mapping session
|
|
_slamResultProcessor?.ResetForNewSession();
|
|
_occupancyGridManager.ResetCounter();
|
|
|
|
// Reset MapBuilder if there are existing trajectory states (theo xloc flow)
|
|
if (_mapBuilder != null && _mapBuilder.DisposeIfHasTrajectories())
|
|
{
|
|
_trajectoryBuilder = null;
|
|
_trajectoryId = -1;
|
|
_mapBuilder = null;
|
|
}
|
|
|
|
// STEP 2: Perform wall alignment to calculate aligned pose
|
|
Pose wallAlignedPose = CalculateWallAlignedPoseAsync();
|
|
lock (_wallAlignedPoseLock)
|
|
{
|
|
_wallAlignedPose = wallAlignedPose; // Store for use when saving map
|
|
}
|
|
|
|
// STEP 3: Pause sensors again before creating trajectory
|
|
// This prevents sensor data from being processed before we set initial pose
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
_logger.LogInformation("CartographerService: Sensors paused before creating trajectory");
|
|
|
|
// Wait for in-flight sensor data to be processed
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
|
|
// STEP 4: Create MapBuilder and add trajectory builder
|
|
if (_mapBuilder == null || _trajectoryBuilder == null)
|
|
{
|
|
_mapBuilder ??= MapBuilderHelper.CreateMapBuilder(_config);
|
|
|
|
// Add trajectory builder (without sensor data, extrapolator not initialized yet)
|
|
(_trajectoryId, _trajectoryBuilder) = _mapBuilder.AddTrajectoryBuilder(_config);
|
|
|
|
// Log wall-aligned pose (will be applied when saving map via TransformPbstreamOrigin)
|
|
_logger.LogInformation(
|
|
"CartographerService: Wall alignment calculated for trajectory {TrajectoryId} - " +
|
|
"Position: [{X:F3}, {Y:F3}, {Z:F3}], Yaw: {Yaw:F3}rad ({YawDeg:F1}°). Will be applied when saving map.",
|
|
_trajectoryId,
|
|
wallAlignedPose.Position.X,
|
|
wallAlignedPose.Position.Y,
|
|
wallAlignedPose.Position.Z,
|
|
WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation),
|
|
WallAlignmentHelper.GetYawFromQuaternion(wallAlignedPose.Orientation) * 180.0 / Math.PI);
|
|
}
|
|
|
|
if (_trajectoryBuilder == null)
|
|
{
|
|
_logger.LogWarning("CartographerService: Trajectory builder is null when entering ScanMapping state");
|
|
}
|
|
|
|
// STEP 6: Now resume sensors - extrapolator will be initialized with correct initial pose
|
|
_sensorPipeline?.ResumeSubscriptions();
|
|
_logger.LogInformation("CartographerService: Sensors resumed for scan mapping");
|
|
|
|
// Start background pose extrapolation thread at 20Hz
|
|
StartPoseThread(50);
|
|
|
|
// Start pose sync stopwatch for periodic pose saving
|
|
_poseSyncStopwatch.Restart();
|
|
|
|
// Start point cloud update stopwatch for throttled point cloud updates
|
|
_pointCloudUpdateStopwatch.Restart();
|
|
});
|
|
|
|
}
|
|
|
|
private void OnScanMappingStateExit()
|
|
{
|
|
if (!_config.Enable) return;
|
|
// Pause sensor subscriptions immediately when leaving ScanMapping state
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
|
|
// Stop background pose thread before cleanup
|
|
StopPoseThread();
|
|
|
|
// Note: Do NOT manually update _currentState here - state machine will handle state transition
|
|
// The state will be updated when ExecuteOnEntry of next state is called
|
|
|
|
// Stop pose sync stopwatch and save final pose
|
|
_poseSyncStopwatch.Stop();
|
|
var mapName = _currentMapName;
|
|
if (!string.IsNullOrEmpty(mapName))
|
|
{
|
|
SavePoseToFile(mapName, Volatile.Read(ref _poseSnapshot).Pose);
|
|
}
|
|
|
|
// Wait for in-flight sensor data to be processed
|
|
_sensorPipeline?.WaitForProcessingComplete(200);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region SavingMap State
|
|
|
|
private void OnSavingMapStateEntry()
|
|
{
|
|
if (!_config.Enable) return;
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.SavingMap;
|
|
}
|
|
|
|
// Pause sensor subscriptions to stop receiving new data
|
|
_sensorPipeline?.PauseSubscriptions();
|
|
|
|
// Wait for in-flight sensor data to be processed
|
|
// This prevents "Cannot add node to finished trajectory" errors
|
|
_sensorPipeline?.WaitForProcessingComplete(2000);
|
|
|
|
// Notify state change (fire-and-forget)
|
|
_ = NotifyStateChangedAsync(SLAMState.SavingMap);
|
|
|
|
// Create CancellationTokenSource for save operation (allows cancellation if needed)
|
|
_savingMapCts?.Cancel();
|
|
_savingMapCts?.Dispose();
|
|
_savingMapCts = new CancellationTokenSource();
|
|
var cancellationToken = _savingMapCts.Token;
|
|
|
|
// Start background save task to handle async save workflow
|
|
_ = Task.Run(async () =>
|
|
{
|
|
try
|
|
{
|
|
string? mapName;
|
|
lock (_lock)
|
|
{
|
|
mapName = _currentMapName;
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(mapName))
|
|
{
|
|
throw new InvalidOperationException("No map name set for save operation");
|
|
}
|
|
|
|
var mapBuilder = MapBuilder ?? throw new InvalidOperationException("MapBuilder is not available for save operation");
|
|
var trajectoryId = TrajectoryId;
|
|
|
|
// Get wall-aligned pose with proper synchronization
|
|
Pose? wallAlignedPose;
|
|
lock (_wallAlignedPoseLock)
|
|
{
|
|
wallAlignedPose = _wallAlignedPose;
|
|
}
|
|
|
|
_logger.LogInformation("CartographerService: Background save task started for map: {MapName}", mapName);
|
|
|
|
// Execute save map logic using MapSaveProcessor
|
|
// This handles: finish trajectory, optimization, pbstream, PGM, PNG, JPG, metadata
|
|
if (_mapSaveProcessor == null)
|
|
{
|
|
throw new InvalidOperationException("MapSaveProcessor is not available (CartographerConfiguration.Enable = false)");
|
|
}
|
|
|
|
// Create progress callback to send updates via SignalR
|
|
async Task ProgressCallback(int workItemsAdded, int workItemsCompleted, int percentComplete)
|
|
{
|
|
try
|
|
{
|
|
await _hubContext.Clients.All.SendAsync("OnMapSaveProgress",
|
|
workItemsAdded, workItemsCompleted, percentComplete, cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogWarning(ex, "CartographerService: Failed to send map save progress update");
|
|
}
|
|
}
|
|
|
|
var mapPath = await _mapSaveProcessor.SaveMapAsync(mapName, mapBuilder, trajectoryId, ProgressCallback, wallAlignedPose, cancellationToken);
|
|
|
|
_logger.LogInformation("CartographerService: Background save task completed successfully for map: {MapName}, path: {MapPath}", mapName, mapPath);
|
|
|
|
// Save current map name to file for auto-resume on next startup
|
|
SaveCurrentMapName(mapName);
|
|
|
|
// Fire MapSaved trigger to transition to Ready state
|
|
FireStateMachine(CartographerTrigger.MapSaved);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
_logger.LogWarning("CartographerService: Background save task was cancelled");
|
|
|
|
// Update error tracking
|
|
lock (_lock)
|
|
{
|
|
_lastError = "Save operation was cancelled";
|
|
_lastException = null;
|
|
}
|
|
|
|
// Fire ErrorOccurred trigger to transition to Error state
|
|
FireStateMachine(CartographerTrigger.ErrorOccurred);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Background save task failed");
|
|
|
|
// Update error tracking
|
|
lock (_lock)
|
|
{
|
|
_lastError = ex.Message;
|
|
_lastException = ex;
|
|
}
|
|
|
|
// Fire ErrorOccurred trigger to transition to Error state
|
|
FireStateMachine(CartographerTrigger.ErrorOccurred);
|
|
}
|
|
}, cancellationToken);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Error State
|
|
|
|
private void OnErrorStateEntry()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_currentState = SLAMState.Error;
|
|
_logger.LogError(
|
|
"CartographerService: Entered Error state. Error: {Error}. " +
|
|
"Call Reset() to return to Idle state and reinitialize.",
|
|
_lastError);
|
|
|
|
// Log exception details if available
|
|
if (_lastException != null)
|
|
{
|
|
_logger.LogError(_lastException,
|
|
"CartographerService: Exception details for Error state");
|
|
}
|
|
}
|
|
_ = NotifyStateChangedAsync(SLAMState.Error);
|
|
}
|
|
|
|
#endregion
|
|
}
|