From 7924a601d1a180a4db4ad60cdd4c0a6704d27fc3 Mon Sep 17 00:00:00 2001
From: oysand <outb@equinor.com>
Date: Sat, 22 Jun 2024 22:00:01 +0200
Subject: [PATCH] Update is localization mission

---
 src/isar_robot/robotinterface.py |  5 +++++
 src/isar_robot/utilities.py      | 11 ++++-------
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/isar_robot/robotinterface.py b/src/isar_robot/robotinterface.py
index 9676281a..94dccae4 100644
--- a/src/isar_robot/robotinterface.py
+++ b/src/isar_robot/robotinterface.py
@@ -65,6 +65,11 @@ def initiate_step(self, step: Step) -> None:
         time.sleep(settings.STEP_DURATION_IN_SECONDS)
 
     def step_status(self) -> StepStatus:
+        if self.current_mission:
+            if is_localization_mission(self.current_mission):
+                if settings.SHOULD_FAIL_LOCALIZATION_MISSION:
+                    return StepStatus.Failed
+
         if is_localization_step(self.current_step):
             self.current_step = None
             if settings.SHOULD_FAIL_LOCALIZATION_STEP:
diff --git a/src/isar_robot/utilities.py b/src/isar_robot/utilities.py
index 5ddbe4de..564c6fd3 100644
--- a/src/isar_robot/utilities.py
+++ b/src/isar_robot/utilities.py
@@ -3,13 +3,10 @@
 
 
 def is_localization_mission(mission: Mission):
-    if len(mission.tasks) != 1:
-        return False
-    if len(mission.tasks[0].steps) != 1:
-        return False
-    if not isinstance(mission.tasks[0].steps[0], Localize):
-        return False
-    return True
+    if mission.start_pose is not None:
+        return True
+
+    return False
 
 
 def is_localization_step(step: Step):