From 0fd49e5fc6d38bac6b57f68e06c743508daa1b39 Mon Sep 17 00:00:00 2001
From: Afonso Luz <avsluz@gmail.com>
Date: Thu, 22 Feb 2024 18:03:51 +0100
Subject: [PATCH] Handle initiate infeasible for full missions

Initate infeasible for full missions
should just fail as there are no next steps
to be tried
---
 src/isar/state_machine/state_machine.py   | 26 +++++++++++++++++------
 src/isar/state_machine/states/initiate.py |  2 +-
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py
index 5cf77909..71422dfe 100644
--- a/src/isar/state_machine/state_machine.py
+++ b/src/isar/state_machine/state_machine.py
@@ -349,12 +349,26 @@ def _stop(self) -> None:
         self.stopped = True
 
     def _initiate_failed(self) -> None:
-        self.current_step.status = StepStatus.Failed
-        self.current_task.update_task_status()
-        self.current_mission.status = MissionStatus.Failed
-        self.publish_step_status(step=self.current_step)
-        self.publish_task_status(task=self.current_task)
-        self._finalize()
+        if self.stepwise_mission:
+            self.current_step.status = StepStatus.Failed
+            self.current_task.update_task_status()
+            self.current_mission.status = MissionStatus.Failed
+            self.publish_step_status(step=self.current_step)
+            self.publish_task_status(task=self.current_task)
+            self._finalize()
+
+        else:
+            self.current_task = None
+            step_status: StepStatus = StepStatus.Cancelled
+            task_status: TaskStatus = TaskStatus.Cancelled
+
+            for task in self.current_mission.tasks:
+                task.status = task_status
+                for step in task.steps:
+                    step.status = step_status
+                    self.publish_step_status(step=step)
+                self.publish_task_status(task=task)
+            self._finalize()
 
     def _initiate_infeasible(self) -> None:
         if self.stepwise_mission:
diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py
index 2aa429d3..0b333c0e 100644
--- a/src/isar/state_machine/states/initiate.py
+++ b/src/isar/state_machine/states/initiate.py
@@ -103,7 +103,7 @@ def _run(self) -> None:
                     f"{str(self.state_machine.current_mission.id)[:8]} because: "
                     f"{e.error_description}"
                 )
-                transition = self.state_machine.initiate_infeasible  # type: ignore
+                transition = self.state_machine.initiate_failed  # type: ignore
                 break
 
             except RobotException as e: