From 52c6bb7119d4a2b52c26f182851fd0b7af1eb94f Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Thu, 21 Mar 2024 15:19:54 +0100 Subject: [PATCH] Improve localization when safe zone button pressed --- .../api/EventHandlers/MissionEventHandler.cs | 47 +++++++------------ backend/api/Services/MissionRunService.cs | 20 ++++++++ .../api/Services/MissionSchedulingService.cs | 6 +-- 3 files changed, 41 insertions(+), 32 deletions(-) diff --git a/backend/api/EventHandlers/MissionEventHandler.cs b/backend/api/EventHandlers/MissionEventHandler.cs index f192fa27e..6347ab7a8 100644 --- a/backend/api/EventHandlers/MissionEventHandler.cs +++ b/backend/api/EventHandlers/MissionEventHandler.cs @@ -159,17 +159,17 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut try { await MissionScheduling.FreezeMissionRunQueueForRobot(e.RobotId); } catch (RobotNotFoundException) { return; } - var area = await FindRobotArea(robot.Id); + var area = await FindRelevantRobotAreaForSafePositionMission(robot.Id); if (area == null) { return; } - try { await MissionScheduling.ScheduleMissionToReturnToSafePosition(e.RobotId, area.Id); } + 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"); } - if (await MissionService.OngoingLocalizationMissionRunExists(e.RobotId)) { 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) @@ -210,29 +213,13 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); } catch (RobotNotFoundException) { return; } - if (!await LocalizationService.RobotIsLocalized(robot.Id)) - { - _logger.LogError($"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised."); - SignalRService.ReportSafeZoneFailureToSignalR(robot, $"Robot {robot.Name} could not be sent from safe zone as it is not correctly localised."); - 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} could not be sent from safe zone as it is not correctly localised"); - } - - if (await MissionScheduling.OngoingMission(robot.Id)) - { - _logger.LogInformation("Robot {RobotName} was unfrozen but the mission to return to safe zone will be completed before further missions are started", robot.Id); - } - - MissionScheduling.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id)); + _startMissionSemaphore.WaitOne(); + try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); } + catch (MissionRunNotFoundException) { return; } + finally { _startMissionSemaphore.Release(); } } - private async Task FindRobotArea(string robotId) + private async Task FindRelevantRobotAreaForSafePositionMission(string robotId) { var robot = await RobotService.ReadById(robotId); if (robot == null) @@ -244,18 +231,20 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB if (!await LocalizationService.RobotIsLocalized(robotId)) { - if (await MissionService.OngoingLocalizationMissionRunExists(robotId)) + if (await MissionService.PendingOrOngoingLocalizationMissionRunExists(robotId)) { - var localizationMission = await MissionService.ReadAll( + var missionRuns = await MissionService.ReadAll( new MissionRunQueryStringParameters { - Statuses = [MissionStatus.Ongoing], + Statuses = [MissionStatus.Ongoing, MissionStatus.Pending], RobotId = robot.Id, OrderBy = "DesiredStartTime", PageSize = 100 }); - return localizationMission.FirstOrDefault()?.Area ?? null; + 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); diff --git a/backend/api/Services/MissionRunService.cs b/backend/api/Services/MissionRunService.cs index 259b61278..bab67d3d7 100644 --- a/backend/api/Services/MissionRunService.cs +++ b/backend/api/Services/MissionRunService.cs @@ -36,6 +36,8 @@ public interface IMissionRunService public Task OngoingLocalizationMissionRunExists(string robotId); + public Task PendingOrOngoingLocalizationMissionRunExists(string robotId); + public Task PendingOrOngoingReturnToHomeMissionRunExists(string robotId); public Task Update(MissionRun mission); @@ -185,6 +187,24 @@ public async Task OngoingLocalizationMissionRunExists(string robotId) return false; } + public async Task 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 PendingOrOngoingReturnToHomeMissionRunExists(string robotId) { var pendingMissionRuns = await ReadMissionRunQueue(robotId); diff --git a/backend/api/Services/MissionSchedulingService.cs b/backend/api/Services/MissionSchedulingService.cs index 6b113ccd8..9790cb0a6 100644 --- a/backend/api/Services/MissionSchedulingService.cs +++ b/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) @@ -443,7 +443,7 @@ private async Task 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;