Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/CartographerService.ApiMethods.cs

@@ -0,0 +1,166 @@
+using CartographerSharp.Mapping;
+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;
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer;
+
+public partial class CartographerService
+{
+    #region ISLAMService -- Localization
+
+    public void StartLocalization(string mapName, Pose? initialPose = null)
+    {
+        // Validate state
+        var currentState = State;
+        if (currentState != SLAMState.Ready)
+        {
+            throw new InvalidOperationException($"Cannot start localization from state: {currentState}");
+        }
+
+        // Dispose old MapBuilder if exists
+        lock (_lock)
+        {
+            if (_mapBuilder != null)
+            {
+                _logger.LogInformation("CartographerService: Disposing existing MapBuilder before starting localization");
+                (_mapBuilder as IDisposable)?.Dispose();
+                _mapBuilder = null;
+                _trajectoryBuilder = null;
+                _trajectoryId = -1;
+                _currentMapName = null;
+            }
+        }
+
+        // Store pending data for InitializingLocalizing state
+        lock (_localizationLock)
+        {
+            _pendingLocalizationMapName = mapName;
+            _pendingLocalizationInitialPose = initialPose;
+        }
+
+        // Fire StartLocalization trigger
+        // Actual work will be done in InitializingLocalizing.ExecuteOnEntry
+        FireStateMachine(CartographerTrigger.StartLocalization);
+
+        _logger.LogInformation("CartographerService: StartLocalization trigger fired for map: {MapName}", mapName);
+    }
+
+    public void SetInitialPose(Pose pose)
+    {
+        // Validate state and get current map name
+        string? currentMapName;
+        lock (_lock)
+        {
+            if (_currentState != SLAMState.Localizing && _currentState != SLAMState.Relocalizing)
+            {
+                throw new InvalidOperationException(
+                    $"SetInitialPose is only allowed during Localizing or InitializingLocalizing. Current state: {_currentState}");
+            }
+            if (_mapBuilder == null)
+            {
+                if (_config.Enable)
+                {
+                    FireStateMachine(CartographerTrigger.ErrorOccurred);
+                    throw new InvalidOperationException(
+                        "Localization not started. Call StartLocalization first.");
+                }
+            }
+            currentMapName = _currentMapName;
+        }
+
+        if (string.IsNullOrEmpty(currentMapName))
+        {
+            throw new InvalidOperationException("No map name available for SetInitialPose");
+        }
+
+        // Store pending data for InitializingLocalizing state
+        lock (_localizationLock)
+        {
+            _pendingLocalizationMapName = currentMapName; // Reuse current map
+            _pendingLocalizationInitialPose = pose;
+            _isSetInitialPoseFlow = true; // Mark as SetInitialPose flow
+        }
+
+        _logger.LogInformation("CartographerService: SetInitialPose called with pose [{X}, {Y}, {Z}], transitioning to InitializingLocalizing",
+            pose.Position.X, pose.Position.Y, pose.Position.Z);
+
+        // Fire StartLocalization trigger to transition to InitializingLocalizing
+        // The InitializingLocalizing state handler will process the restart with new pose
+        FireStateMachine(CartographerTrigger.StartLocalization);
+    }
+
+    public void StopLocalization() => FireStateMachine(CartographerTrigger.StopLocalization);
+
+    #endregion
+
+    #region ISLAMService -- ScanMapping
+
+    public void StartScanMapping(string mapName)
+    {
+        // Validate that SLAM is enabled
+        if (!_config.Enable) return;
+
+        lock (_lock)
+        {
+            _currentMapName = mapName;
+        }
+        FireStateMachine(CartographerTrigger.StartScanMapping);
+    }
+
+    public void SaveScanMap()
+    {
+        // Validate that SLAM is enabled
+        if (!_config.Enable) return;
+
+        FireStateMachine(CartographerTrigger.SaveMap);
+    }
+
+    #endregion
+
+    #region ISLAMService -- Query
+
+    public List<(int NodeId, Pose Pose)>? GetTrajectoryNodes()
+    {
+        var trajectoryNodePoses = _mapBuilder?.PoseGraph.GetTrajectoryNodePoses() ?? null;
+
+        if (trajectoryNodePoses == null || trajectoryNodePoses.IsEmpty)
+        {
+            return [];
+        }
+
+        var result = new List<(int NodeId, Pose Pose)>();
+        var trajectoryId = _trajectoryId;
+
+        foreach (var kvp in trajectoryNodePoses)
+        {
+            if (kvp.Id.TrajectoryId == trajectoryId)
+            {
+                var globalPose = kvp.Data.GlobalPose;
+                var pose = Helpers.PoseConverter.ToPose(globalPose);
+                result.Add((kvp.Id.NodeIndex, pose));
+            }
+        }
+        return result;
+    }
+
+    public IReadOnlyList<Vector3> GetAggregatedSamplePointCloud()
+    {
+        lock (_pointCloudLock)
+        {
+            var count = _completedAccumulatedSamplePointClouds.Count;
+            return [.. _completedAccumulatedSamplePointClouds];
+        }
+    }
+
+    public OccupancyGrid? GetOccupancyGrid() => _occupancyGridManager.OccupancyGrid;
+
+    public OccupancyGrid? GetOccupancyGrid(DateTime since) => _occupancyGridManager.GetGrid(since);
+
+    public DateTime LastUpdatedOccupancyGrid => _occupancyGridManager.LastUpdated;
+
+    #endregion
+}