Update is localization mission

equinor · Jun 22, 2024 · 7924a60 · 7924a60
1 parent 0d8b9be
commit 7924a60
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 7 deletions.
diff --git 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
@@ -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):