Initial commit
This commit is contained in:
@@ -0,0 +1,739 @@
|
||||
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
|
||||
}
|
||||
Reference in New Issue
Block a user