diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 428ac469..f43c5073 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/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: diff --git a/src/isar/state_machine/states/paused.py b/src/isar/state_machine/states/paused.py index ee68864d..ca396074 100644 --- a/src/isar/state_machine/states/paused.py +++ b/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) diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 55663606..2d00d641 100644 --- a/src/robot_interface/robot_interface.py +++ b/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. diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index c13f7c57..ac9f3927 100644 --- a/tests/mocks/robot_interface.py +++ b/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"