Initial commit

2026-07-13 09:25:40 +07:00
parent c08ff54676
commit bccfb156d7
1938 changed files with 641646 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.StateMachine.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.StateMachine.cs
@@ -0,0 +1,127 @@
+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
+}