diff --git a/backend/api/EventHandlers/MissionEventHandler.cs b/backend/api/EventHandlers/MissionEventHandler.cs index a2eef32f9..7b6cfcea4 100644 --- a/backend/api/EventHandlers/MissionEventHandler.cs +++ b/backend/api/EventHandlers/MissionEventHandler.cs @@ -176,7 +176,15 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut return; } - await MissionSchedulingService.ScheduleMissionToReturnToSafePosition(e.RobotId, area.Id); + try + { + await MissionSchedulingService.ScheduleMissionToReturnToSafePosition(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); + await MissionSchedulingService.UnfreezeMissionRunQueueForRobot(e.RobotId); + } MqttEventHandlerService.TriggerRobotAvailable(new RobotAvailableEventArgs(robot.Id)); diff --git a/backend/api/EventHandlers/MissionScheduling.cs b/backend/api/EventHandlers/MissionScheduling.cs index 748b68d76..c5050a85f 100644 --- a/backend/api/EventHandlers/MissionScheduling.cs +++ b/backend/api/EventHandlers/MissionScheduling.cs @@ -185,7 +185,6 @@ public async Task StopCurrentMissionRun(string robotId) return; } - var ongoingMissions = await GetOngoingMission(robot.Id); if (ongoingMissions == null) @@ -196,6 +195,8 @@ public async Task StopCurrentMissionRun(string robotId) { foreach (var mission in ongoingMissions) { + if (mission.MissionRunPriority == MissionRunPriority.Emergency) continue; + var newMission = new MissionRun { Name = mission.Name, diff --git a/backend/api/Services/MissionSchedulingService.cs b/backend/api/Services/MissionSchedulingService.cs index 6cefc3fd5..fbbe9f6d2 100644 --- a/backend/api/Services/MissionSchedulingService.cs +++ b/backend/api/Services/MissionSchedulingService.cs @@ -76,7 +76,8 @@ public Pose ClosestSafePosition(Pose robotPose, IList safePosition { if (safePositions == null || !safePositions.Any()) { - throw new ArgumentException("List of safe positions cannot be null or empty."); + string message = "No safe position for area the robot is localized in"; + throw new SafeZoneException(message); } var closestPose = safePositions[0].Pose; diff --git a/backend/api/Utilities/Exceptions.cs b/backend/api/Utilities/Exceptions.cs index 63012a333..f96d81710 100644 --- a/backend/api/Utilities/Exceptions.cs +++ b/backend/api/Utilities/Exceptions.cs @@ -80,4 +80,9 @@ public class DeckExistsException : Exception { public DeckExistsException(string message) : base(message) { } } + + public class SafeZoneException : Exception + { + public SafeZoneException(string message) : base(message) { } + } }