Implement pause mission

src/isar/state_machine/states/monitor.py

@@ -5,6 +5,7 @@
 
 from injector import inject
 from transitions import State
+from isar.config.settings import RobotSettings
 
 from isar.services.utilities.threaded_request import (
     ThreadedRequest,
@@ -34,6 +35,7 @@ def __init__(self, state_machine: "StateMachine") -> None:
 
         self.logger = logging.getLogger("state_machine")
         self.step_status_thread: Optional[ThreadedRequest] = None
+        self.pause_mission_thread: Optional[ThreadedRequest] = None
 
     def start(self) -> None:
         self.state_machine.update_state()
@@ -44,6 +46,10 @@ def stop(self) -> None:
             self.step_status_thread.wait_for_thread()
         self.step_status_thread = None
 
+        if self.pause_mission_thread:
+            self.pause_mission_thread.wait_for_thread()
+        self.pause_mission_thread = None
+
     def _run(self) -> None:
         transition: Callable
         while True:
@@ -53,6 +59,11 @@ def _run(self) -> None:
 
             if self.state_machine.should_pause_mission():
                 transition = self.state_machine.pause  # type: ignore
+                if "pause_mission" in RobotSettings.CAPABILITIES:
+                    self._run_pause_mission_thread(
+                        pause_mission_function=self.state_machine.robot.pause,
+                        thread_name="State Machine Monitor Pause Mission",
+                    )
                 break
 
             if not self.step_status_thread:
@@ -141,6 +152,12 @@ def _run_get_status_thread(
         self.step_status_thread = ThreadedRequest(request_func=status_function)
         self.step_status_thread.start_thread(name=thread_name)
 
+    def _run_pause_mission_thread(
+        self, pause_mission_function: Callable, thread_name: str
+    ) -> None:
+        self.pause_mission_thread = ThreadedRequest(request_func=pause_mission_function)
+        self.pause_mission_thread.start_thread(name=thread_name)
+
     def _queue_inspections_for_upload(
         self, mission: Mission, current_step: InspectionStep
     ) -> None:

src/isar/state_machine/states/paused.py

@@ -1,23 +1,32 @@
 import logging
 import time
-from typing import Callable, TYPE_CHECKING
+from typing import Callable, TYPE_CHECKING, Optional
 
 from transitions import State
 
+from isar.config.settings import RobotSettings
+from isar.services.utilities.threaded_request import ThreadedRequest
+
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
 
 
 class Paused(State):
     def __init__(self, state_machine: "StateMachine") -> None:
-        super().__init__(name="paused", on_enter=self.start)
+        super().__init__(name="paused", on_enter=self.start, on_exit=self.stop)
         self.state_machine: "StateMachine" = state_machine
         self.logger = logging.getLogger("state_machine")
+        self.resume_mission_thread: Optional[ThreadedRequest] = None
 
     def start(self) -> None:
         self.state_machine.update_state()
         self._run()
 
+    def stop(self) -> None:
+        if self.resume_mission_thread:
+            self.resume_mission_thread.wait_for_thread()
+        self.resume_mission_thread = None
+
     def _run(self) -> None:
         transition: Callable
         while True:
@@ -27,8 +36,21 @@ def _run(self) -> None:
 
             if self.state_machine.should_resume_mission():
                 transition = self.state_machine.resume  # type: ignore
+                if "pause_mission" in RobotSettings.CAPABILITIES:
+                    self._run_resume_mission_thread(
+                        resume_mission_function=self.state_machine.robot.resume,
+                        thread_name="State Machine Paused Resume Mission",
+                    )
                 break
 
             time.sleep(self.state_machine.sleep_time)
 
         transition()
+
+    def _run_resume_mission_thread(
+        self, resume_mission_function: Callable, thread_name: str
+    ) -> None:
+        self.resume_mission_thread = ThreadedRequest(
+            request_func=resume_mission_function
+        )
+        self.resume_mission_thread.start_thread(name=thread_name)

src/robot_interface/robot_interface.py

@@ -142,6 +142,44 @@ def stop(self) -> None:
         """
         raise NotImplementedError
 
+    @abstractmethod
+    def pause(self) -> None:
+        """Pauses the execution of the current step and stops the movement of the robot.
+
+        Returns
+        -------
+        None
+
+        Raises
+        ------
+        RobotActionException
+            If the robot fails to perform the requested action to pause mission execution
+            the action to pause will be attempted again until a certain number of retries
+        RobotException
+            Will catch other RobotExceptions and retry to pause the mission
+
+        """
+        raise NotImplementedError
+
+    @abstractmethod
+    def resume(self) -> None:
+        """Resumes the execution of the current step and continues the rest of the mission.
+
+        Returns
+        -------
+        None
+
+        Raises
+        ------
+        RobotActionException
+            If the robot fails to perform the requested action to resume mission execution
+            the action to resume will be attempted again until a certain number of retries
+        RobotException
+            Will catch other RobotExceptions and retry to resume the mission
+
+        """
+        raise NotImplementedError
+
     @abstractmethod
     def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
         """Return the inspections connected to the given step.

tests/mocks/robot_interface.py

@@ -51,6 +51,12 @@ def step_status(self) -> StepStatus:
     def stop(self) -> None:
         return
 
+    def pause(self) -> None:
+        return
+
+    def resume(self) -> None:
+        return
+
     def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
         image: Image = Image(mock_image_metadata())
         image.data = b"Some binary image data"