Run full mission through state machine

src/isar/config/settings.env

@@ -1,5 +1,7 @@
 ISAR_ROBOT_PACKAGE = isar_robot
 
+ISAR_RUN_MISSION_STEPWISE = true
+
 ISAR_STORAGE_LOCAL_ENABLED = true
 ISAR_STORAGE_BLOB_ENABLED = false
 ISAR_STORAGE_SLIMM_ENABLED = false

src/isar/state_machine/state_machine.py

@@ -165,6 +165,12 @@ def __init__(
                     "dest": self.initiate_state,
                     "before": self._step_finished,
                 },
+                {
+                    "trigger": "full_mission_finished",
+                    "source": self.monitor_state,
+                    "dest": self.initiate_state,
+                    "before": self._full_mission_finished,
+                },
                 {
                     "trigger": "mission_paused",
                     "source": self.stop_state,
@@ -228,21 +234,27 @@ def _initialization_successful(self) -> None:
         self.publish_mission_status()
         self.current_task = self.task_selector.next_task()
         self.current_task.status = TaskStatus.InProgress
-        self.publish_task_status()
+        self.publish_task_status(task=self.current_task)
         self.update_current_step()
 
     def _initialization_failed(self) -> None:
         self.queues.start_mission.output.put(False)
         self._finalize()
 
     def _initiated(self) -> None:
-        self.current_step.status = StepStatus.InProgress
-        self.publish_step_status()
-        self.logger.info(
-            f"Successfully initiated "
-            f"{type(self.current_step).__name__} "
-            f"step: {str(self.current_step.id)[:8]}"
-        )
+        if self.stepwise_mission:
+            self.current_step.status = StepStatus.InProgress
+            self.publish_step_status(step=self.current_step)
+            self.logger.info(
+                f"Successfully initiated "
+                f"{type(self.current_step).__name__} "
+                f"step: {str(self.current_step.id)[:8]}"
+            )
+        else:
+            self.logger.info(
+                f"Successfully initiated full mission with ID: "
+                f"{str(self.current_mission.id)[:8]}"
+            )
 
     def _pause(self) -> None:
         return
@@ -255,7 +267,7 @@ def _resume(self) -> None:
         self.current_mission.status = MissionStatus.InProgress
         self.current_task.status = TaskStatus.InProgress
         self.publish_mission_status()
-        self.publish_task_status()
+        self.publish_task_status(task=self.current_task)
 
         resume_mission_response: ControlMissionResponse = (
             self._make_control_mission_response()
@@ -287,10 +299,35 @@ def _mission_started(self) -> None:
         return
 
     def _step_finished(self) -> None:
-        self.publish_step_status()
+        self.publish_step_status(step=self.current_step)
         self.update_current_task()
         self.update_current_step()
 
+    def _full_mission_finished(self) -> None:
+        self.current_task = None
+        step_status: StepStatus = StepStatus.Failed
+        task_status: TaskStatus = TaskStatus.Failed
+
+        if self.current_mission.status == MissionStatus.Failed:
+            step_status = StepStatus.Failed
+            task_status = TaskStatus.Failed
+        elif self.current_mission.status == MissionStatus.Cancelled:
+            step_status = StepStatus.Cancelled
+            task_status = TaskStatus.Cancelled
+        elif (
+            self.current_mission.status == MissionStatus.Successful
+            or self.current_mission.status == MissionStatus.PartiallySuccessful
+        ):
+            step_status = StepStatus.Successful
+            task_status = TaskStatus.Successful
+
+        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)
+
     def _mission_paused(self) -> None:
         self.logger.info(f"Pausing mission: {self.current_mission.id}")
         self.current_mission.status = MissionStatus.Paused
@@ -303,8 +340,8 @@ def _mission_paused(self) -> None:
         self.queues.pause_mission.output.put(paused_mission_response)
 
         self.publish_mission_status()
-        self.publish_task_status()
-        self.publish_step_status()
+        self.publish_task_status(task=self.current_task)
+        self.publish_step_status(step=self.current_step)
 
     def _stop(self) -> None:
         self.stopped = True
@@ -313,15 +350,16 @@ 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()
-        self.publish_task_status()
+        self.publish_step_status(step=self.current_step)
+        self.publish_task_status(task=self.current_task)
         self._finalize()
 
     def _initiate_infeasible(self) -> None:
-        self.current_step.status = StepStatus.Failed
-        self.publish_step_status()
-        self.update_current_task()
-        self.update_current_step()
+        if self.stepwise_mission:
+            self.current_step.status = StepStatus.Failed
+            self.publish_step_status(step=self.current_step)
+            self.update_current_task()
+            self.update_current_step()
 
     def _mission_stopped(self) -> None:
         self.current_mission.status = MissionStatus.Cancelled
@@ -341,8 +379,8 @@ def _mission_stopped(self) -> None:
         )
         self.queues.stop_mission.output.put(stopped_mission_response)
 
-        self.publish_task_status()
-        self.publish_step_status()
+        self.publish_task_status(task=self.current_task)
+        self.publish_step_status(step=self.current_step)
         self._finalize()
 
     #################################################################################
@@ -370,11 +408,11 @@ def begin(self):
     def update_current_task(self):
         if self.current_task.is_finished():
             self.current_task.update_task_status()
-            self.publish_task_status()
+            self.publish_task_status(task=self.current_task)
             try:
                 self.current_task = self.task_selector.next_task()
                 self.current_task.status = TaskStatus.InProgress
-                self.publish_task_status()
+                self.publish_task_status(task=self.current_task)
             except TaskSelectorStop:
                 # Indicates that all tasks are finished
                 self.current_task = None
@@ -458,17 +496,17 @@ def publish_mission_status(self) -> None:
             retain=False,
         )
 
-    def publish_task_status(self) -> None:
-        """Publishes the current task status to the MQTT Broker"""
+    def publish_task_status(self, task: Task) -> None:
+        """Publishes the task status to the MQTT Broker"""
         if not self.mqtt_publisher:
             return
         payload: str = json.dumps(
             {
                 "isar_id": settings.ISAR_ID,
                 "robot_name": settings.ROBOT_NAME,
                 "mission_id": self.current_mission.id if self.current_mission else None,
-                "task_id": self.current_task.id if self.current_task else None,
-                "status": self.current_task.status if self.current_task else None,
+                "task_id": task.id if task else None,
+                "status": task.status if task else None,
                 "timestamp": datetime.utcnow(),
             },
             cls=EnhancedJSONEncoder,
@@ -480,8 +518,8 @@ def publish_task_status(self) -> None:
             retain=False,
         )
 
-    def publish_step_status(self) -> None:
-        """Publishes the current step status to the MQTT Broker"""
+    def publish_step_status(self, step: Step) -> None:
+        """Publishes the step status to the MQTT Broker"""
         if not self.mqtt_publisher:
             return
         payload: str = json.dumps(
@@ -490,11 +528,9 @@ def publish_step_status(self) -> None:
                 "robot_name": settings.ROBOT_NAME,
                 "mission_id": self.current_mission.id if self.current_mission else None,
                 "task_id": self.current_task.id if self.current_task else None,
-                "step_id": self.current_step.id if self.current_step else None,
-                "step_type": self.current_step.__class__.__name__
-                if self.current_step
-                else None,
-                "status": self.current_step.status if self.current_step else None,
+                "step_id": step.id if step else None,
+                "step_type": step.__class__.__name__ if step else None,
+                "status": step.status if step else None,
                 "timestamp": datetime.utcnow(),
             },
             cls=EnhancedJSONEncoder,

src/isar/state_machine/states/initiate.py

@@ -11,13 +11,11 @@
 )
 from robot_interface.models.exceptions import (
     RobotException,
+    RobotInfeasibleMissionException,
     RobotInfeasibleStepException,
     RobotLowBatteryException,
     RobotLowPressureException,
 )
-from robot_interface.models.exceptions.robot_exceptions import (
-    RobotInfeasibleMissionException,
-)
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine

src/isar/state_machine/states/monitor.py

@@ -1,7 +1,7 @@
 import logging
 import time
 from copy import deepcopy
-from typing import Callable, Optional, Sequence, TYPE_CHECKING, Tuple
+from typing import Callable, Optional, Sequence, TYPE_CHECKING, Tuple, Union
 
 from injector import inject
 from transitions import State
@@ -13,6 +13,8 @@
 )
 from robot_interface.models.exceptions import RobotException
 from robot_interface.models.inspection.inspection import Inspection
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.status import MissionStatus
 from robot_interface.models.mission.step import InspectionStep, Step, StepStatus
 
 if TYPE_CHECKING:
@@ -49,46 +51,77 @@ def _run(self) -> None:
                 break
 
             if not self.step_status_thread:
-                self.step_status_thread = ThreadedRequest(
-                    self.state_machine.robot.step_status
-                )
-                self.step_status_thread.start_thread(
-                    name="State Machine Monitor Current Status"
-                )
+                if self.state_machine.stepwise_mission:
+                    self._run_get_status_thread(
+                        status_function=self.state_machine.robot.step_status,
+                        thread_name="State Machine Monitor Get Step Status",
+                    )
+                else:
+                    self._run_get_status_thread(
+                        status_function=self.state_machine.robot.mission_status,
+                        thread_name="State Machine Monitor Get Mission Status",
+                    )
 
             try:
-                step_status: StepStatus = self.step_status_thread.get_output()
+                status: Union[
+                    StepStatus, MissionStatus
+                ] = self.step_status_thread.get_output()
             except ThreadedRequestNotFinishedError:
                 time.sleep(self.state_machine.sleep_time)
                 continue
             except RobotException:
-                step_status = StepStatus.Failed
+                status = StepStatus.Failed
 
-            self.state_machine.current_step.status = step_status
+            if self.state_machine.stepwise_mission and isinstance(status, StepStatus):
+                self.state_machine.current_step.status = status
+            elif isinstance(status, MissionStatus):
+                self.state_machine.current_mission.status = status
 
-            if self._step_finished(step=self.state_machine.current_step):
-                get_inspections_thread = ThreadedRequest(self._process_finished_step)
+            if self._should_upload_inspections():
+                get_inspections_thread = ThreadedRequest(
+                    self._queue_inspections_for_upload
+                )
                 get_inspections_thread.start_thread(
                     self.state_machine.current_step,
                     name="State Machine Get Inspections",
                 )
-                transition = self.state_machine.step_finished  # type: ignore
-                break
+
+            if self.state_machine.stepwise_mission:
+                if self._step_finished(self.state_machine.current_step):
+                    transition = self.state_machine.step_finished  # type: ignore
+                    break
+            else:
+                if self._mission_finished(self.state_machine.current_mission):
+                    transition = self.state_machine.full_mission_finished  # type: ignore
+                    break
 
             self.step_status_thread = None
             time.sleep(self.state_machine.sleep_time)
 
         transition()
 
+    def _run_get_status_thread(
+        self, status_function: Callable, thread_name: str
+    ) -> None:
+        self.step_status_thread = ThreadedRequest(request_func=status_function)
+        self.step_status_thread.start_thread(name=thread_name)
+
     def _queue_inspections_for_upload(self, current_step: InspectionStep) -> None:
         try:
             inspections: Sequence[
                 Inspection
             ] = self.state_machine.robot.get_inspections(step=current_step)
         except Exception as e:
-            self.logger.error(
-                f"Error getting inspections for step {str(current_step.id)[:8]}: {e}"
-            )
+            if self.state_machine.stepwise_mission:
+                self.logger.error(
+                    f"Error getting inspections for step "
+                    f"{str(current_step.id)[:8]}: {e}"
+                )
+            else:
+                self.logger.error(
+                    f"Error getting inspections for mission "
+                    f"{str(self.state_machine.current_mission.id)[:8]}: {e}"
+                )
             return
 
         if not inspections:
@@ -124,6 +157,29 @@ def _step_finished(self, step: Step) -> bool:
             finished = True
         return finished
 
-    def _process_finished_step(self, step: Step) -> None:
-        if step.status == StepStatus.Successful and isinstance(step, InspectionStep):
-            self._queue_inspections_for_upload(current_step=step)
+    @staticmethod
+    def _mission_finished(mission: Mission) -> bool:
+        if (
+            mission.status == MissionStatus.Successful
+            or mission.status == MissionStatus.PartiallySuccessful
+            or mission.status == MissionStatus.Failed
+        ):
+            return True
+        return False
+
+    def _should_upload_inspections(self) -> bool:
+        if self.state_machine.stepwise_mission:
+            step: Step = self.state_machine.current_step
+            return (
+                self._step_finished(step)
+                and step.status == StepStatus.Successful
+                and isinstance(step, InspectionStep)
+            )
+        else:
+            mission_status: MissionStatus = self.state_machine.current_mission.status
+            if (
+                mission_status == MissionStatus.Successful
+                or mission_status == MissionStatus.PartiallySuccessful
+            ):
+                return True
+            return False

src/robot_interface/models/exceptions/__init__.py

@@ -1,6 +1,7 @@
 from .robot_exceptions import (
     RobotCommunicationException,
     RobotException,
+    RobotInfeasibleMissionException,
     RobotInfeasibleStepException,
     RobotInvalidResponseException,
     RobotLowBatteryException,