Use localization pose when no safezone available

equinor · Oct 25, 2024 · ede56fd · ede56fd
1 parent b0199b7
commit ede56fd
Showing 1 changed file with 14 additions and 3 deletions.
diff --git a/backend/api/Services/MissionSchedulingService.cs b/backend/api/Services/MissionSchedulingService.cs
@@ -288,13 +288,25 @@ public async Task ScheduleMissionToDriveToSafePosition(string robotId, string ar
                 logger.LogError("Could not find area with ID {AreaId}", areaId);
                 return;
             }
+
             var robot = await robotService.ReadById(robotId, readOnly: true);
             if (robot == null)
             {
                 logger.LogError("Robot with ID: {RobotId} was not found in the database", robotId);
                 return;
             }
+
             var closestSafePosition = ClosestSafePosition(robot.Pose, area.SafePositions);
+            if (closestSafePosition == null)
+            {
+                logger.LogWarning("Robot with ID: {RobotId} did not have a safe position for area it is localized in, using localization position instead", robotId);
+                if (robot.CurrentArea?.Deck.DefaultLocalizationPose == null)
+                {
+                    throw new SafeZoneException($"Robot with ID: {robotId} has no available safezone or localization poses in its current area");
+                }
+                closestSafePosition = robot.CurrentArea.Deck.DefaultLocalizationPose.Pose;
+            }
+
             // Cloning to avoid tracking same object
             var clonedPose = ObjectCopier.Clone(closestSafePosition);
             var customTaskQuery = new CustomTaskQuery
@@ -471,12 +483,11 @@ private async Task StartMissionRun(MissionRun queuedMissionRun)
             logger.LogInformation("Started mission run '{Id}'", queuedMissionRun.Id);
         }
 
-        private static Pose ClosestSafePosition(Pose robotPose, IList<SafePosition> safePositions)
+        private static Pose? ClosestSafePosition(Pose robotPose, IList<SafePosition> safePositions)
         {
             if (safePositions == null || !safePositions.Any())
             {
-                string message = "No safe position for area the robot is localized in";
-                throw new SafeZoneException(message);
+                return null;
             }
 
             var closestPose = safePositions[0].Pose;