Improve send robots to safe zone during localization mission

backend/api.test/EventHandlers/TestMissionEventHandler.cs

@@ -11,6 +11,7 @@
 using Api.Options;
 using Api.Services;
 using Api.Services.ActionServices;
+using Api.Services.Events;
 using Api.Test.Database;
 using Api.Test.Mocks;
 using Microsoft.AspNetCore.Builder;
@@ -31,6 +32,9 @@ public class TestMissionEventHandler : IDisposable
         private readonly MqttEventHandler _mqttEventHandler;
         private readonly MqttService _mqttService;
         private readonly DatabaseUtilities _databaseUtilities;
+        private readonly RobotService _robotService;
+        private readonly LocalizationService _localizationService;
+        private readonly EmergencyActionService _emergencyActionService;
 
         public TestMissionEventHandler(DatabaseFixture fixture)
         {
@@ -57,6 +61,7 @@ public TestMissionEventHandler(DatabaseFixture fixture)
             _mqttService = new MqttService(mqttServiceLogger, configuration);
             _missionRunService = new MissionRunService(context, signalRService, missionLogger, accessRoleService);
 
+
             var echoServiceMock = new MockEchoService();
             var stidServiceMock = new MockStidService(context);
             var customMissionServiceMock = new MockCustomMissionService();
@@ -69,15 +74,17 @@ public TestMissionEventHandler(DatabaseFixture fixture)
             var plantService = new PlantService(context, installationService, accessRoleService);
             var deckService = new DeckService(context, defaultLocalizationPoseService, installationService, plantService, accessRoleService, signalRService);
             var areaService = new AreaService(context, installationService, plantService, deckService, defaultLocalizationPoseService, accessRoleService);
-            var robotService = new RobotService(context, robotServiceLogger, robotModelService, signalRService, accessRoleService, installationService, areaService, _missionRunService);
             var mapServiceMock = new MockMapService();
-            var localizationService = new LocalizationService(localizationServiceLogger, robotService, _missionRunService, installationService, areaService, mapServiceMock);
-            var returnToHomeService = new ReturnToHomeService(returnToHomeServiceLogger, robotService, _missionRunService, mapServiceMock);
-            var missionSchedulingService = new MissionSchedulingService(missionSchedulingServiceLogger, _missionRunService, robotService, areaService,
-                isarServiceMock, localizationService, returnToHomeService, signalRService);
+            _robotService = new RobotService(context, robotServiceLogger, robotModelService, signalRService, accessRoleService, installationService, areaService, _missionRunService);
+            _localizationService = new LocalizationService(localizationServiceLogger, _robotService, _missionRunService, installationService, areaService, mapServiceMock);
+
+            var returnToHomeService = new ReturnToHomeService(returnToHomeServiceLogger, _robotService, _missionRunService, mapServiceMock);
+            var missionSchedulingService = new MissionSchedulingService(missionSchedulingServiceLogger, _missionRunService, _robotService, areaService,
+                isarServiceMock, _localizationService, returnToHomeService, signalRService);
             var lastMissionRunService = new LastMissionRunService(lastMissionRunServiceLogger, missionDefinitionService, _missionRunService);
 
             _databaseUtilities = new DatabaseUtilities(context);
+            _emergencyActionService = new EmergencyActionService();
 
             var mockServiceProvider = new Mock<IServiceProvider>();
 
@@ -87,7 +94,7 @@ public TestMissionEventHandler(DatabaseFixture fixture)
                 .Returns(_missionRunService);
             mockServiceProvider
                 .Setup(p => p.GetService(typeof(IRobotService)))
-                .Returns(robotService);
+                .Returns(_robotService);
             mockServiceProvider
                 .Setup(p => p.GetService(typeof(IMissionSchedulingService)))
                 .Returns(missionSchedulingService);
@@ -96,7 +103,7 @@ public TestMissionEventHandler(DatabaseFixture fixture)
                 .Returns(context);
             mockServiceProvider
                 .Setup(p => p.GetService(typeof(ILocalizationService)))
-                .Returns(localizationService);
+                .Returns(_localizationService);
             mockServiceProvider
                 .Setup(p => p.GetService(typeof(IReturnToHomeService)))
                 .Returns(returnToHomeService);
@@ -109,6 +116,16 @@ public TestMissionEventHandler(DatabaseFixture fixture)
             mockServiceProvider
                 .Setup(p => p.GetService(typeof(ILastMissionRunService)))
                 .Returns(lastMissionRunService);
+            mockServiceProvider
+                .Setup(p => p.GetService(typeof(IAreaService)))
+                .Returns(areaService);
+            mockServiceProvider
+                .Setup(p => p.GetService(typeof(ISignalRService)))
+                .Returns(signalRService);
+            mockServiceProvider
+                .Setup(p => p.GetService(typeof(IEmergencyActionService)))
+                .Returns(_emergencyActionService);
+
 
             // Mock service injector
             var mockScope = new Mock<IServiceScope>();
@@ -316,5 +333,31 @@ public async void QueuedMissionsAreAbortedWhenLocalizationFails()
             Assert.Equal(MissionStatus.Aborted, postTestMissionRun!.Status);
         }
 
+        [Fact]
+        public async void LocalizationMissionCompletesAfterPressingSendToSafeZoneButton()
+        {
+            // Arrange
+            var installation = await _databaseUtilities.NewInstallation();
+            var plant = await _databaseUtilities.NewPlant(installation.InstallationCode);
+            var deck = await _databaseUtilities.NewDeck(installation.InstallationCode, plant.PlantCode);
+            var area = await _databaseUtilities.NewArea(installation.InstallationCode, plant.PlantCode, deck.Name);
+            var robot = await _databaseUtilities.NewRobot(RobotStatus.Busy, installation, area);
+            await _databaseUtilities.NewMissionRun(installation.InstallationCode, robot, area, true, MissionRunPriority.Localization, MissionStatus.Ongoing, Guid.NewGuid().ToString());
+
+            Thread.Sleep(100);
+
+            // Act
+            var eventArgs = new EmergencyButtonPressedForRobotEventArgs(robot.Id);
+            _emergencyActionService.RaiseEvent(nameof(EmergencyActionService.EmergencyButtonPressedForRobot), eventArgs);
+
+            Thread.Sleep(1000);
+
+            // Assert 
+            var updatedRobot = await _robotService.ReadById(robot.Id);
+            Assert.True(updatedRobot?.MissionQueueFrozen);
+
+            bool isRobotLocalized = await _localizationService.RobotIsLocalized(robot.Id);
+            Assert.True(isRobotLocalized);
+        }
     }
 }

backend/api/Controllers/EmergencyActionController.cs

@@ -30,7 +30,7 @@ public async Task<ActionResult<string>> AbortCurrentMissionAndSendAllRobotsToSaf
             [FromRoute] string installationCode)
         {
 
-            var robots = await robotService.ReadLocalizedRobotsForInstallation(installationCode);
+            var robots = await robotService.ReadRobotsForInstallation(installationCode);
 
             foreach (var robot in robots)
             {
@@ -58,7 +58,7 @@ public async Task<ActionResult<string>> AbortCurrentMissionAndSendAllRobotsToSaf
         public async Task<ActionResult<string>> ClearEmergencyStateForAllRobots(
             [FromRoute] string installationCode)
         {
-            var robots = await robotService.ReadLocalizedRobotsForInstallation(installationCode);
+            var robots = await robotService.ReadRobotsForInstallation(installationCode);
 
             foreach (var robot in robots)
             {

backend/api/EventHandlers/MissionEventHandler.cs

@@ -1,4 +1,5 @@
-using Api.Database.Models;
+using Api.Controllers.Models;
+using Api.Database.Models;
 using Api.Services;
 using Api.Services.Events;
 using Api.Utilities;
@@ -80,6 +81,11 @@ private async void OnMissionRunCreated(object? sender, MissionRunCreatedEventArg
                     _scheduleLocalizationSemaphore.Release();
                     return;
                 }
+                if (await MissionService.OngoingLocalizationMissionRunExists(missionRun.Robot.Id))
+                {
+                    _scheduleLocalizationSemaphore.Release();
+                    return;
+                }
                 try
                 {
                     var localizationMissionRun = await LocalizationService.CreateLocalizationMissionInArea(missionRun.Robot.Id, missionRun.Area.Id);
@@ -150,26 +156,20 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
                 return;
             }
 
-            var area = await AreaService.ReadById(robot.CurrentArea!.Id);
-            if (area == null)
-            {
-                _logger.LogError("Could not find area with ID {AreaId}", robot.CurrentArea!.Id);
-                SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} was not correctly localised. Could not find area {robot.CurrentArea.Name}");
-                return;
-            }
-
             try { await MissionScheduling.FreezeMissionRunQueueForRobot(e.RobotId); }
             catch (RobotNotFoundException) { return; }
 
-            try { await MissionScheduling.ScheduleMissionToReturnToSafePosition(e.RobotId, area.Id); }
+            var area = await FindRelevantRobotAreaForSafePositionMission(robot.Id);
+            if (area == null) { return; }
+
+            try { await MissionScheduling.ScheduleMissionToDriveToSafePosition(e.RobotId, area.Id); }
             catch (SafeZoneException ex)
             {
                 _logger.LogError(ex, "Failed to schedule return to safe zone mission on robot {RobotName} because: {ErrorMessage}", robot.Name, ex.Message);
                 SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Failed to send {robot.Name} to a safe zone");
-                try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); }
-                catch (RobotNotFoundException) { return; }
             }
 
+            if (await MissionService.PendingOrOngoingLocalizationMissionRunExists(e.RobotId)) { return; }
             try { await MissionScheduling.StopCurrentMissionRun(e.RobotId); }
             catch (RobotNotFoundException) { return; }
             catch (MissionRunNotFoundException)
@@ -194,7 +194,10 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
                 return;
             }
 
-            MissionScheduling.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id));
+            _startMissionSemaphore.WaitOne();
+            try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); }
+            catch (MissionRunNotFoundException) { return; }
+            finally { _startMissionSemaphore.Release(); }
         }
 
         private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyButtonPressedForRobotEventArgs e)
@@ -207,22 +210,57 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB
                 return;
             }
 
-            var area = await AreaService.ReadById(robot.CurrentArea!.Id);
-            if (area == null)
+            try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); }
+            catch (RobotNotFoundException) { return; }
+
+            _startMissionSemaphore.WaitOne();
+            try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); }
+            catch (MissionRunNotFoundException) { return; }
+            finally { _startMissionSemaphore.Release(); }
+        }
+
+        private async Task<Area?> FindRelevantRobotAreaForSafePositionMission(string robotId)
+        {
+            var robot = await RobotService.ReadById(robotId);
+            if (robot == null)
             {
-                _logger.LogError("Could not find area with ID {AreaId}", robot.CurrentArea!.Id);
-                SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised");
+                _logger.LogError("Robot with ID: {RobotId} was not found in the database", robotId);
+                return null;
             }
 
-            try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); }
-            catch (RobotNotFoundException) { return; }
+            if (!await LocalizationService.RobotIsLocalized(robotId))
+            {
+
+                if (await MissionService.PendingOrOngoingLocalizationMissionRunExists(robotId))
+                {
+                    var missionRuns = await MissionService.ReadAll(
+                        new MissionRunQueryStringParameters
+                        {
+                            Statuses = [MissionStatus.Ongoing, MissionStatus.Pending],
+                            RobotId = robot.Id,
+                            OrderBy = "DesiredStartTime",
+                            PageSize = 100
+                        });
+
+                    var localizationMission = missionRuns.Find(missionRun => missionRun.IsLocalizationMission());
+
+                    return localizationMission?.Area ?? null;
+                }
+
+                _logger.LogError("Robot {RobotName} is not localized and no localization mission is ongoing.", robot.Name);
+                SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} has not been localised.");
+                return null;
+            }
 
-            if (await MissionScheduling.OngoingMission(robot.Id))
+            var area = await AreaService.ReadById(robot.CurrentArea!.Id);
+            if (area == null)
             {
-                _logger.LogInformation("Robot {RobotName} was unfrozen but the mission to return to safe zone will be completed before further missions are started", robot.Id);
+                _logger.LogError("Could not find area with ID {AreaId}", robot.CurrentArea!.Id);
+                SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} was not correctly localised. Could not find area {robot.CurrentArea.Name}");
+                return null;
             }
 
-            MissionScheduling.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id));
+            return area;
         }
     }
 }

backend/api/Services/MissionRunService.cs

@@ -36,6 +36,8 @@ public interface IMissionRunService
 
         public Task<bool> OngoingLocalizationMissionRunExists(string robotId);
 
+        public Task<bool> PendingOrOngoingLocalizationMissionRunExists(string robotId);
+
         public Task<bool> PendingOrOngoingReturnToHomeMissionRunExists(string robotId);
 
         public Task<MissionRun> Update(MissionRun mission);
@@ -185,6 +187,24 @@ public async Task<bool> OngoingLocalizationMissionRunExists(string robotId)
             return false;
         }
 
+        public async Task<bool> PendingOrOngoingLocalizationMissionRunExists(string robotId)
+        {
+            var pendingMissionRuns = await ReadMissionRunQueue(robotId);
+            foreach (var pendingMissionRun in pendingMissionRuns)
+            {
+                if (pendingMissionRun.IsLocalizationMission()) { return true; }
+            }
+            var ongoingMissionRuns = await GetMissionRunsWithSubModels()
+                .Where(missionRun => missionRun.Robot.Id == robotId && missionRun.Status == MissionStatus.Ongoing)
+                .OrderBy(missionRun => missionRun.DesiredStartTime)
+                .ToListAsync();
+            foreach (var ongoingMissionRun in ongoingMissionRuns)
+            {
+                if (ongoingMissionRun.IsLocalizationMission()) { return true; }
+            }
+            return false;
+        }
+
         public async Task<bool> PendingOrOngoingReturnToHomeMissionRunExists(string robotId)
         {
             var pendingMissionRuns = await ReadMissionRunQueue(robotId);

backend/api/Services/MissionSchedulingService.cs

@@ -18,7 +18,7 @@ public interface IMissionSchedulingService
 
         public Task AbortAllScheduledMissions(string robotId, string? abortReason = null);
 
-        public Task ScheduleMissionToReturnToSafePosition(string robotId, string areaId);
+        public Task ScheduleMissionToDriveToSafePosition(string robotId, string areaId);
 
         public Task UnfreezeMissionRunQueueForRobot(string robotId);
 
@@ -212,7 +212,7 @@ public async Task AbortAllScheduledMissions(string robotId, string? abortReason)
             }
         }
 
-        public async Task ScheduleMissionToReturnToSafePosition(string robotId, string areaId)
+        public async Task ScheduleMissionToDriveToSafePosition(string robotId, string areaId)
         {
             var area = await areaService.ReadById(areaId);
             if (area == null)
@@ -296,6 +296,12 @@ private async Task MoveInterruptedMissionsToQueue(IEnumerable<string> interrupte
                     continue;
                 }
 
+                if (missionRun.IsReturnHomeMission())
+                {
+                    logger.LogWarning("Return to home mission will not be added back to the queue.");
+                    return;
+                }
+
                 var newMissionRun = new MissionRun
                 {
                     Name = missionRun.Name,
@@ -305,7 +311,7 @@ private async Task MoveInterruptedMissionsToQueue(IEnumerable<string> interrupte
                     Area = missionRun.Area,
                     Status = MissionStatus.Pending,
                     DesiredStartTime = DateTime.UtcNow,
-                    Tasks = missionRun.Tasks,
+                    Tasks = missionRun.Tasks.Select(t => new MissionTask(t)).ToList(),
                     Map = new MapMetadata()
                 };
 
@@ -437,7 +443,7 @@ private async Task<bool> TheSystemIsAvailableToRunAMission(string robotId, strin
                 throw new MissionRunNotFoundException(errorMessage);
             }
 
-            if (robot.MissionQueueFrozen && missionRun.MissionRunPriority != MissionRunPriority.Emergency)
+            if (robot.MissionQueueFrozen && missionRun.MissionRunPriority != MissionRunPriority.Emergency && missionRun.MissionRunPriority != MissionRunPriority.Localization)
             {
                 logger.LogInformation("Mission run {MissionRunId} was not started as the mission run queue for robot {RobotName} is frozen", missionRun.Id, robot.Name);
                 return false;