128 lines
4.7 KiB
C#
128 lines
4.7 KiB
C#
using Appccelerate.StateMachine;
|
|
using Appccelerate.StateMachine.Machine;
|
|
using Microsoft.AspNetCore.SignalR;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
public partial class CartographerService
|
|
{
|
|
#region State Machine Definition
|
|
|
|
private PassiveStateMachine<SLAMState, CartographerTrigger> BuildStateMachine()
|
|
{
|
|
var builder = new StateMachineDefinitionBuilder<SLAMState, CartographerTrigger>();
|
|
|
|
// Idle state
|
|
builder.In(SLAMState.Idle)
|
|
.ExecuteOnEntry(OnIdleStateEntry)
|
|
.On(CartographerTrigger.Start)
|
|
.Goto(SLAMState.Initializing);
|
|
|
|
// Initializing state
|
|
builder.In(SLAMState.Initializing)
|
|
.ExecuteOnEntry(OnInitializingStateEntry)
|
|
.On(CartographerTrigger.InitializationComplete)
|
|
.Goto(SLAMState.Ready)
|
|
.On(CartographerTrigger.InitializationFailed)
|
|
.Goto(SLAMState.Error);
|
|
|
|
// Ready state
|
|
builder.In(SLAMState.Ready)
|
|
.ExecuteOnEntry(OnReadyStateEntry)
|
|
.On(CartographerTrigger.StartLocalization)
|
|
.Goto(SLAMState.Relocalizing)
|
|
.On(CartographerTrigger.StartScanMapping)
|
|
.Goto(SLAMState.ScanMapping)
|
|
.On(CartographerTrigger.Shutdown)
|
|
.Goto(SLAMState.Idle);
|
|
|
|
// InitializingLocalizing state - with background task for async localization setup
|
|
// Background task will load map, start MCL (if enabled), or add trajectory (if MCL disabled), then fire MclConverged/MclSkipped trigger
|
|
builder.In(SLAMState.Relocalizing)
|
|
.ExecuteOnEntry(OnInitializingLocalizingStateEntry)
|
|
.ExecuteOnExit(OnInitializingLocalizingStateExit)
|
|
.On(CartographerTrigger.MclConverged)
|
|
.Goto(SLAMState.Localizing)
|
|
.On(CartographerTrigger.MclSkipped)
|
|
.Goto(SLAMState.Localizing)
|
|
.On(CartographerTrigger.StopLocalization)
|
|
.Goto(SLAMState.Ready)
|
|
.On(CartographerTrigger.ErrorOccurred)
|
|
.Goto(SLAMState.Error);
|
|
|
|
// Localizing state
|
|
builder.In(SLAMState.Localizing)
|
|
.ExecuteOnEntry(OnLocalizingStateEntry)
|
|
.ExecuteOnExit(OnLocalizingStateExit)
|
|
.On(CartographerTrigger.MclConverged)
|
|
.Goto(SLAMState.Localizing) // Self-transition: handle MCL convergence after it converges
|
|
.On(CartographerTrigger.StartLocalization)
|
|
.Goto(SLAMState.Relocalizing) // Allow transition back to InitializingLocalizing for SetInitialPose
|
|
.On(CartographerTrigger.StopLocalization)
|
|
.Goto(SLAMState.Ready)
|
|
.On(CartographerTrigger.ErrorOccurred)
|
|
.Goto(SLAMState.Error);
|
|
|
|
// ScanMapping state
|
|
builder.In(SLAMState.ScanMapping)
|
|
.ExecuteOnEntry(OnScanMappingStateEntry)
|
|
.ExecuteOnExit(OnScanMappingStateExit)
|
|
.On(CartographerTrigger.SaveMap)
|
|
.Goto(SLAMState.SavingMap)
|
|
.On(CartographerTrigger.ErrorOccurred)
|
|
.Goto(SLAMState.Error);
|
|
|
|
// SavingMap state - with background task for async save workflow
|
|
builder.In(SLAMState.SavingMap)
|
|
.ExecuteOnEntry(OnSavingMapStateEntry)
|
|
.On(CartographerTrigger.MapSaved)
|
|
.Goto(SLAMState.Ready)
|
|
.On(CartographerTrigger.ErrorOccurred)
|
|
.Goto(SLAMState.Error);
|
|
|
|
// Error state
|
|
builder.In(SLAMState.Error)
|
|
.ExecuteOnEntry(OnErrorStateEntry)
|
|
.On(CartographerTrigger.Reset)
|
|
.Goto(SLAMState.Idle);
|
|
|
|
return builder
|
|
.WithInitialState(SLAMState.Idle)
|
|
.Build()
|
|
.CreatePassiveStateMachine();
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region State Machine Helpers
|
|
|
|
/// <summary>
|
|
/// Thread-safe wrapper for state machine Fire() (Fix 7).
|
|
/// PassiveStateMachine is not thread-safe; all Fire/Start/Stop calls must be synchronized.
|
|
/// </summary>
|
|
private void FireStateMachine(CartographerTrigger trigger)
|
|
{
|
|
lock (_stateMachineLock)
|
|
{
|
|
_stateMachine.Fire(trigger);
|
|
}
|
|
}
|
|
|
|
private async Task NotifyStateChangedAsync(SLAMState state)
|
|
{
|
|
try
|
|
{
|
|
await _hubContext.Clients.All.SendAsync("OnStateChanged", state);
|
|
_logger.LogDebug("CartographerService: Notified state change to {State}", state);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Failed to notify state change to {State}", state);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
}
|