diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index f6c9bd04..1486ad74 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -24,6 +24,7 @@ Initiate, Monitor, Off, + Offline, Paused, Stop, ) @@ -88,6 +89,7 @@ def __init__( self.monitor_state: State = Monitor(self) self.initiate_state: State = Initiate(self) self.off_state: State = Off(self) + self.offline_state: State = Offline(self) self.states: List[State] = [ self.off_state, @@ -97,6 +99,7 @@ def __init__( self.monitor_state, self.stop_state, self.paused_state, + self.offline_state, ] self.machine = Machine(self, states=self.states, initial="off", queued=True) @@ -194,6 +197,18 @@ def __init__( "dest": self.idle_state, "before": self._mission_stopped, }, + { + "trigger": "robot_turned_offline", + "source": [self.idle_state], + "dest": self.offline_state, + "before": self._offline, + }, + { + "trigger": "robot_turned_online", + "source": self.offline_state, + "dest": self.idle_state, + "before": self._online, + }, ] ) @@ -239,6 +254,12 @@ def _pause(self) -> None: def _off(self) -> None: return + def _offline(self) -> None: + return + + def _online(self) -> None: + return + def _resume(self) -> None: self.logger.info(f"Resuming mission: {self.current_mission.id}") self.current_mission.status = MissionStatus.InProgress diff --git a/src/isar/state_machine/states/__init__.py b/src/isar/state_machine/states/__init__.py index ec766345..034c06fe 100644 --- a/src/isar/state_machine/states/__init__.py +++ b/src/isar/state_machine/states/__init__.py @@ -3,5 +3,6 @@ from .initiate import Initiate from .monitor import Monitor from .off import Off +from .offline import Offline from .paused import Paused from .stop import Stop diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index 97d6bf41..b90495e5 100644 --- a/src/isar/state_machine/states/idle.py +++ b/src/isar/state_machine/states/idle.py @@ -1,10 +1,17 @@ import logging import time -from typing import Optional, TYPE_CHECKING +from typing import TYPE_CHECKING, Optional from transitions import State +from isar.config.settings import settings from isar.models.communication.message import StartMissionMessage +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions.robot_exceptions import RobotException +from robot_interface.models.mission.status import RobotStatus if TYPE_CHECKING: from isar.state_machine.state_machine import StateMachine @@ -15,13 +22,17 @@ def __init__(self, state_machine: "StateMachine") -> None: super().__init__(name="idle", on_enter=self.start, on_exit=self.stop) self.state_machine: "StateMachine" = state_machine self.logger = logging.getLogger("state_machine") + self.robot_status_thread: Optional[ThreadedRequest] = None + self.last_robot_status_poll_time: float = time.time() def start(self) -> None: self.state_machine.update_state() self._run() def stop(self) -> None: - pass + if self.robot_status_thread: + self.robot_status_thread.wait_for_thread() + self.robot_status_thread = None def _run(self) -> None: while True: @@ -37,4 +48,38 @@ def _run(self) -> None: break time.sleep(self.state_machine.sleep_time) + time_from_last_robot_status_poll = ( + time.time() - self.last_robot_status_poll_time + ) + if ( + time_from_last_robot_status_poll + < settings.ROBOT_API_STATUS_POLL_INTERVAL + ): + continue + + if not self.robot_status_thread: + self.robot_status_thread = ThreadedRequest( + request_func=self.state_machine.robot.robot_status + ) + self.robot_status_thread.start_thread( + name="State Machine Offline Get Robot Status" + ) + + try: + robot_status: RobotStatus = self.robot_status_thread.get_output() + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + + except (RobotException,) as e: + self.logger.error( + f"Failed to get robot status because: {e.error_description}" + ) + + self.last_robot_status_poll_time = time.time() + + if robot_status == RobotStatus.Offline: + transition = self.state_machine.robot_turned_offline # type: ignore + break + transition() diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 3e245781..8184a0af 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/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, Union +from typing import TYPE_CHECKING, Callable, Optional, Sequence, Tuple, Union from injector import inject from transitions import State @@ -14,11 +14,11 @@ ) from robot_interface.models.exceptions.robot_exceptions import ( ErrorMessage, + RobotCommunicationTimeoutException, RobotException, RobotMissionStatusException, RobotRetrieveInspectionException, RobotStepStatusException, - RobotCommunicationTimeoutException, ) from robot_interface.models.inspection.inspection import Inspection from robot_interface.models.mission.mission import Mission @@ -131,8 +131,6 @@ def _run(self) -> None: f"Retrieving the status failed because: {e.error_description}" ) - self.request_status_failure_counter = 0 - if isinstance(status, StepStatus): self.state_machine.current_step.status = status elif isinstance(status, MissionStatus): @@ -158,6 +156,7 @@ def _run(self) -> None: else: if isinstance(status, StepStatus): if self._step_finished(self.state_machine.current_step): + self.state_machine.update_current_step() self.state_machine.current_task.update_task_status() else: # If not all steps are done self.state_machine.current_task.status = TaskStatus.InProgress @@ -166,7 +165,6 @@ def _run(self) -> None: self.state_machine.current_task ) if self.state_machine.current_task.status == TaskStatus.Successful: - self.state_machine.update_remaining_steps() try: self.state_machine.current_task = ( self.state_machine.task_selector.next_task() diff --git a/src/isar/state_machine/states/offline.py b/src/isar/state_machine/states/offline.py new file mode 100644 index 00000000..683bc1d3 --- /dev/null +++ b/src/isar/state_machine/states/offline.py @@ -0,0 +1,62 @@ +import logging +import time +from typing import TYPE_CHECKING, Optional + +from transitions import State + +from isar.config.settings import settings +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions.robot_exceptions import RobotException +from robot_interface.models.mission.status import RobotStatus + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class Offline(State): + def __init__(self, state_machine: "StateMachine") -> None: + super().__init__(name="offline", on_enter=self.start, on_exit=self.stop) + self.state_machine: "StateMachine" = state_machine + self.logger = logging.getLogger("state_machine") + self.robot_status_thread: Optional[ThreadedRequest] = None + + def start(self) -> None: + self.state_machine.update_state() + self._run() + + def stop(self) -> None: + if self.robot_status_thread: + self.robot_status_thread.wait_for_thread() + self.robot_status_thread = None + + def _run(self) -> None: + while True: + if not self.robot_status_thread: + self.robot_status_thread = ThreadedRequest( + request_func=self.state_machine.robot.robot_status + ) + self.robot_status_thread.start_thread( + name="State Machine Offline Get Robot Status" + ) + + try: + robot_status: RobotStatus = self.robot_status_thread.get_output() + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + + except (RobotException,) as e: + self.logger.error( + f"Failed to get robot status because: {e.error_description}" + ) + + if robot_status != RobotStatus.Offline: + transition = self.state_machine.robot_turned_online # type: ignore + break + + time.sleep(settings.ROBOT_API_STATUS_POLL_INTERVAL) + + transition() diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index 55ccae83..d5f4e72d 100644 --- a/src/isar/state_machine/states_enum.py +++ b/src/isar/state_machine/states_enum.py @@ -9,6 +9,7 @@ class States(str, Enum): Monitor = "monitor" Paused = "paused" Stop = "stop" + Offline = "offline" def __repr__(self): return self.value diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 800744a0..c2e5a66c 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -25,7 +25,7 @@ from robot_interface.models.mission.task import Task from robot_interface.telemetry.mqtt_client import MqttClientInterface from tests.mocks.pose import MockPose -from tests.mocks.robot_interface import MockRobot +from tests.mocks.robot_interface import MockRobot, MockRobotIdleToOfflineToIdleTest from tests.mocks.step import MockStep @@ -320,6 +320,18 @@ def test_state_machine_with_unsuccessful_mission_stop( assert expected == actual +def test_state_machine_idle_to_offline_to_idle(state_machine_thread) -> None: + state_machine_thread.state_machine.robot = MockRobotIdleToOfflineToIdleTest() + + # Robot status check happens every 5 seconds by default + time.sleep(13) # Ensure that robot_status have been called again in Idle + + expected_transitions_list = deque([States.Idle, States.Offline, States.Idle]) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + def _mock_robot_exception_with_message() -> RobotException: raise RobotException( error_reason=ErrorReason.RobotUnknownErrorException, diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index c13f7c57..4be71215 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -78,3 +78,15 @@ def mock_image_metadata() -> ImageMetadata: ), file_type="jpg", ) + + +class MockRobotIdleToOfflineToIdleTest(MockRobot): + def __init__(self): + self.first = True + + def robot_status(self) -> RobotStatus: + if self.first: + self.first = False + return RobotStatus.Offline + + return RobotStatus.Available