Make robot go home when battery or pressure levels are outside safe limits

backend/api.test/EventHandlers/TestMissionEventHandler.cs

@@ -468,8 +468,8 @@ public async Task LocalizationMissionCompletesAfterPressingSendToSafeZoneButton(
             Thread.Sleep(100);
 
             // Act
-            var eventArgs = new EmergencyButtonPressedForRobotEventArgs(robot.Id);
-            _emergencyActionService.RaiseEvent(nameof(EmergencyActionService.EmergencyButtonPressedForRobot), eventArgs);
+            var eventArgs = new RobotEmergencyEventArgs(robot.Id, RobotFlotillaStatus.SafeZone);
+            _emergencyActionService.RaiseEvent(nameof(EmergencyActionService.SendRobotToSafezoneTriggered), eventArgs);
 
             Thread.Sleep(1000);
 

backend/api/Controllers/EmergencyActionController.cs

@@ -34,7 +34,7 @@ public async Task<ActionResult<string>> AbortCurrentMissionAndSendAllRobotsToSaf
 
             foreach (var robot in robots)
             {
-                emergencyActionService.TriggerEmergencyButtonPressedForRobot(new EmergencyButtonPressedForRobotEventArgs(robot.Id));
+                emergencyActionService.SendRobotToSafezone(new RobotEmergencyEventArgs(robot.Id, Database.Models.RobotFlotillaStatus.SafeZone));
 
             }
 
@@ -62,7 +62,7 @@ public async Task<ActionResult<string>> ClearEmergencyStateForAllRobots(
 
             foreach (var robot in robots)
             {
-                emergencyActionService.TriggerEmergencyButtonDepressedForRobot(new EmergencyButtonPressedForRobotEventArgs(robot.Id));
+                emergencyActionService.ReleaseRobotFromSafezone(new RobotEmergencyEventArgs(robot.Id, Database.Models.RobotFlotillaStatus.Normal));
             }
 
             return NoContent();

backend/api/Controllers/Models/RobotResponse.cs

@@ -32,7 +32,7 @@ public class RobotResponse
 
         public bool Deprecated { get; set; }
 
-        public bool MissionQueueFrozen { get; set; }
+        public RobotFlotillaStatus FlotillaStatus { get; set; }
 
         public RobotStatus Status { get; set; }
 
@@ -65,7 +65,7 @@ public RobotResponse(Robot robot)
             Port = robot.Port;
             IsarConnected = robot.IsarConnected;
             Deprecated = robot.Deprecated;
-            MissionQueueFrozen = robot.MissionQueueFrozen;
+            FlotillaStatus = robot.FlotillaStatus;
             Status = robot.Status;
             Pose = robot.Pose;
             CurrentMissionId = robot.CurrentMissionId;

backend/api/Database/Models/MissionRun.cs

@@ -195,6 +195,8 @@ var inspection in task.Inspections.Where(inspection => !inspection.IsCompleted)
         public bool IsLocalizationMission() { return MissionRunType == MissionRunType.Localization; }
 
         public bool IsReturnHomeMission() { return Tasks is [{ Type: MissionTaskType.ReturnHome }]; }
+
+        public bool IsEmergencyMission() { return MissionRunType == MissionRunType.Emergency; }
     }
 
     public enum MissionStatus

backend/api/Database/Models/Robot.cs

@@ -117,6 +117,9 @@ public bool IsRobotBatteryTooLow()
         [Required]
         public RobotStatus Status { get; set; }
 
+        [Required]
+        public RobotFlotillaStatus FlotillaStatus { get; set; } = RobotFlotillaStatus.Normal;
+
         [Required]
         public Pose Pose { get; set; }
 
@@ -146,6 +149,13 @@ public enum RobotStatus
         Blocked,
     }
 
+    public enum RobotFlotillaStatus
+    {
+        Normal,
+        SafeZone,
+        Recharging,
+    }
+
     public enum RobotCapabilitiesEnum
     {
         take_thermal_image,

backend/api/EventHandlers/MissionEventHandler.cs

@@ -43,17 +43,17 @@ public override void Subscribe()
             MissionRunService.MissionRunCreated += OnMissionRunCreated;
             MissionSchedulingService.RobotAvailable += OnRobotAvailable;
             MissionSchedulingService.LocalizationMissionSuccessful += OnLocalizationMissionSuccessful;
-            EmergencyActionService.EmergencyButtonPressedForRobot += OnEmergencyButtonPressedForRobot;
-            EmergencyActionService.EmergencyButtonDepressedForRobot += OnEmergencyButtonDepressedForRobot;
+            EmergencyActionService.SendRobotToSafezoneTriggered += OnSendRobotToSafezoneTriggered;
+            EmergencyActionService.ReleaseRobotFromSafezoneTriggered += OnReleaseRobotFromSafezoneTriggered;
         }
 
         public override void Unsubscribe()
         {
             MissionRunService.MissionRunCreated -= OnMissionRunCreated;
             MissionSchedulingService.RobotAvailable -= OnRobotAvailable;
             MissionSchedulingService.LocalizationMissionSuccessful -= OnLocalizationMissionSuccessful;
-            EmergencyActionService.EmergencyButtonPressedForRobot -= OnEmergencyButtonPressedForRobot;
-            EmergencyActionService.EmergencyButtonDepressedForRobot -= OnEmergencyButtonDepressedForRobot;
+            EmergencyActionService.SendRobotToSafezoneTriggered -= OnSendRobotToSafezoneTriggered;
+            EmergencyActionService.ReleaseRobotFromSafezoneTriggered -= OnReleaseRobotFromSafezoneTriggered;
         }
 
         protected override async Task ExecuteAsync(CancellationToken stoppingToken)
@@ -153,7 +153,7 @@ private async void OnLocalizationMissionSuccessful(object? sender, LocalizationM
             finally { _startMissionSemaphore.Release(); }
         }
 
-        private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyButtonPressedForRobotEventArgs e)
+        private async void OnSendRobotToSafezoneTriggered(object? sender, RobotEmergencyEventArgs e)
         {
             _logger.LogInformation("Triggered EmergencyButtonPressed event for robot ID: {RobotId}", e.RobotId);
             var robot = await RobotService.ReadById(e.RobotId);
@@ -163,6 +163,19 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
                 return;
             }
 
+            if (robot.FlotillaStatus == e.RobotFlotillaStatus)
+            {
+                _logger.LogInformation("Did not send robot to safezone since robot {RobotId} was already in the correct state", e.RobotId);
+                return;
+            }
+
+            try { await RobotService.UpdateFlotillaStatus(e.RobotId, e.RobotFlotillaStatus ?? RobotFlotillaStatus.Normal); }
+            catch (Exception ex)
+            {
+                _logger.LogError("Was not able to update Robot Flotilla status for robot {RobotId}, {ErrorMessage}", e.RobotId, ex.Message);
+                return;
+            }
+
             try { await MissionScheduling.FreezeMissionRunQueueForRobot(e.RobotId); }
             catch (RobotNotFoundException) { return; }
 
@@ -221,7 +234,7 @@ private async void OnEmergencyButtonPressedForRobot(object? sender, EmergencyBut
             finally { _startMissionSemaphore.Release(); }
         }
 
-        private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyButtonPressedForRobotEventArgs e)
+        private async void OnReleaseRobotFromSafezoneTriggered(object? sender, RobotEmergencyEventArgs e)
         {
             _logger.LogInformation("Triggered EmergencyButtonPressed event for robot ID: {RobotId}", e.RobotId);
             var robot = await RobotService.ReadById(e.RobotId);
@@ -231,9 +244,22 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB
                 return;
             }
 
+            if (robot.FlotillaStatus == e.RobotFlotillaStatus)
+            {
+                _logger.LogInformation("Did not release robot from safezone since robot {RobotId} was already in the correct state", e.RobotId);
+                return;
+            }
+
             try { await MissionScheduling.UnfreezeMissionRunQueueForRobot(e.RobotId); }
             catch (RobotNotFoundException) { return; }
 
+            try { await RobotService.UpdateFlotillaStatus(e.RobotId, e.RobotFlotillaStatus ?? RobotFlotillaStatus.Normal); }
+            catch (Exception ex)
+            {
+                _logger.LogError("Was not able to update Robot Flotilla status for robot {RobotId}, {ErrorMessage}", e.RobotId, ex.Message);
+                return;
+            }
+
             _startMissionSemaphore.WaitOne();
             try { await MissionScheduling.StartNextMissionRunIfSystemIsAvailable(robot.Id); }
             catch (MissionRunNotFoundException) { return; }
@@ -268,8 +294,6 @@ private async void OnEmergencyButtonDepressedForRobot(object? sender, EmergencyB
                     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;
             }
 

backend/api/EventHandlers/MqttEventHandler.cs

@@ -45,6 +45,7 @@ public MqttEventHandler(ILogger<MqttEventHandler> logger, IServiceScopeFactory s
         private ISignalRService SignalRService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<ISignalRService>();
         private ITaskDurationService TaskDurationService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<ITaskDurationService>();
         private ITeamsMessageService TeamsMessageService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<ITeamsMessageService>();
+        private IEmergencyActionService EmergencyActionService => _scopeFactory.CreateScope().ServiceProvider.GetRequiredService<IEmergencyActionService>();
 
         public override void Subscribe()
         {
@@ -424,13 +425,39 @@ private async void OnIsarStepUpdate(object? sender, MqttReceivedArgs mqttArgs)
         private async void OnIsarBatteryUpdate(object? sender, MqttReceivedArgs mqttArgs)
         {
             var batteryStatus = (IsarBatteryMessage)mqttArgs.Message;
-            await BatteryTimeseriesService.AddBatteryEntry(batteryStatus.BatteryLevel, batteryStatus.IsarId);
+            var robot = await BatteryTimeseriesService.AddBatteryEntry(batteryStatus.BatteryLevel, batteryStatus.IsarId);
+            if (robot == null) return;
+            robot.BatteryLevel = batteryStatus.BatteryLevel;
+
+            if (robot.FlotillaStatus == RobotFlotillaStatus.Normal && robot.IsRobotBatteryTooLow())
+            {
+                _logger.LogInformation("Sending robot '{RobotName}' to its safe zone as its battery level is too low.", robot.Name);
+                EmergencyActionService.SendRobotToSafezone(new RobotEmergencyEventArgs(robot.Id, RobotFlotillaStatus.Recharging));
+            }
+            else if (robot.FlotillaStatus == RobotFlotillaStatus.Recharging && !(robot.IsRobotBatteryTooLow() || robot.IsRobotPressureTooHigh() || robot.IsRobotPressureTooLow()))
+            {
+                _logger.LogInformation("Releasing robot '{RobotName}' from its safe zone as its battery and pressure levels are good enough to run missions.", robot.Name);
+                EmergencyActionService.ReleaseRobotFromSafezone(new RobotEmergencyEventArgs(robot.Id, RobotFlotillaStatus.Normal));
+            }
         }
 
         private async void OnIsarPressureUpdate(object? sender, MqttReceivedArgs mqttArgs)
         {
             var pressureStatus = (IsarPressureMessage)mqttArgs.Message;
-            await PressureTimeseriesService.AddPressureEntry(pressureStatus.PressureLevel, pressureStatus.IsarId);
+            var robot = await PressureTimeseriesService.AddPressureEntry(pressureStatus.PressureLevel, pressureStatus.IsarId);
+            if (robot == null) return;
+            robot.PressureLevel = pressureStatus.PressureLevel;
+
+            if (robot.FlotillaStatus == RobotFlotillaStatus.Normal && (robot.IsRobotPressureTooLow() || robot.IsRobotPressureTooHigh()))
+            {
+                _logger.LogInformation("Sending robot '{RobotName}' to its safe zone as its pressure is too low or high.", robot.Name);
+                EmergencyActionService.SendRobotToSafezone(new RobotEmergencyEventArgs(robot.Id, RobotFlotillaStatus.Recharging));
+            }
+            else if (robot.FlotillaStatus == RobotFlotillaStatus.Recharging && !(robot.IsRobotBatteryTooLow() || robot.IsRobotPressureTooHigh() || robot.IsRobotPressureTooLow()))
+            {
+                _logger.LogInformation("Releasing robot '{RobotName}' from its safe zone as its battery and pressure levels are good enough to run missions.", robot.Name);
+                EmergencyActionService.ReleaseRobotFromSafezone(new RobotEmergencyEventArgs(robot.Id, RobotFlotillaStatus.Normal));
+            }
         }
 
         private async void OnIsarPoseUpdate(object? sender, MqttReceivedArgs mqttArgs)