Introduce offline state

Co-authored-by: Anders Chirico Indrebø <[email protected]>

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

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

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()

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()

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()

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

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,

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