diff --git a/backend/api/Services/MissionSchedulingService.cs b/backend/api/Services/MissionSchedulingService.cs index 0094beff6..09d0ab66f 100644 --- 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 safePositions) + private static Pose? ClosestSafePosition(Pose robotPose, IList 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;