Add blocked state

src/isar/state_machine/state_machine.py

@@ -18,13 +18,14 @@
 )
 from isar.models.communication.message import StartMissionMessage
 from isar.models.communication.queues.queues import Queues
+from isar.state_machine.states.blocked import Blocked
+from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
 from isar.state_machine.states.idle import Idle
 from isar.state_machine.states.initialize import Initialize
 from isar.state_machine.states.initiate import Initiate
 from isar.state_machine.states.monitor import Monitor
 from isar.state_machine.states.off import Off
 from isar.state_machine.states.offline import Offline
-from isar.state_machine.states.blocked_protective_stop import BlockedProtectiveStop
 from isar.state_machine.states.paused import Paused
 from isar.state_machine.states.stop import Stop
 from isar.state_machine.states_enum import States
@@ -36,8 +37,8 @@
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.telemetry.payloads import (
-    RobotStatusPayload,
     MissionPayload,
+    RobotStatusPayload,
     TaskPayload,
 )
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
@@ -93,6 +94,7 @@ def __init__(
         self.initiate_state: State = Initiate(self)
         self.off_state: State = Off(self)
         self.offline_state: State = Offline(self)
+        self.blocked: State = Blocked(self)
         self.blocked_protective_stop: State = BlockedProtectiveStop(self)
 
         self.states: List[State] = [
@@ -104,6 +106,7 @@ def __init__(
             self.stop_state,
             self.paused_state,
             self.offline_state,
+            self.blocked,
             self.blocked_protective_stop,
         ]
 
@@ -230,6 +233,18 @@ def __init__(
                     "dest": self.idle_state,
                     "before": self._online,
                 },
+                {
+                    "trigger": "robot_blocked",
+                    "source": self.idle_state,
+                    "dest": self.blocked,
+                    "before": self._blocked,
+                },
+                {
+                    "trigger": "robot_unblocked",
+                    "source": self.blocked,
+                    "dest": self.idle_state,
+                    "before": self._unblocked,
+                },
                 {
                     "trigger": "robot_protective_stop_engaged",
                     "source": [self.idle_state],
@@ -290,6 +305,12 @@ def _offline(self) -> None:
     def _online(self) -> None:
         return
 
+    def _blocked(self) -> None:
+        return
+
+    def _unblocked(self) -> None:
+        return
+
     def _protective_stop_engaged(self) -> None:
         return
 
@@ -582,6 +603,8 @@ def _current_status(self) -> RobotStatus:
             return RobotStatus.Available
         elif self.current_state == States.Offline:
             return RobotStatus.Offline
+        elif self.current_state == States.Blocked:
+            return RobotStatus.Blocked
         elif self.current_state == States.BlockedProtectiveStop:
             return RobotStatus.BlockedProtectiveStop
         else:

src/isar/state_machine/states/blocked.py

@@ -0,0 +1,63 @@
+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 Blocked(State):
+    def __init__(self, state_machine: "StateMachine") -> None:
+        super().__init__(name="blocked", 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 Blocked 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.Blocked:
+                transition = self.state_machine.robot_unblocked  # type: ignore
+                break
+
+            self.robot_status_thread = None
+            time.sleep(settings.ROBOT_API_STATUS_POLL_INTERVAL)
+
+        transition()

src/isar/state_machine/states/idle.py

@@ -94,6 +94,9 @@ def _run(self) -> None:
             if robot_status == RobotStatus.Offline:
                 transition = self.state_machine.robot_turned_offline  # type: ignore
                 break
+            elif robot_status == RobotStatus.Blocked:
+                transition = self.state_machine.robot_blocked  # type: ignore
+                break
             elif robot_status == RobotStatus.BlockedProtectiveStop:
                 transition = self.state_machine.robot_protective_stop_engaged  # type: ignore
                 break

src/isar/state_machine/states_enum.py

@@ -10,6 +10,7 @@ class States(str, Enum):
     Paused = "paused"
     Stop = "stop"
     Offline = "offline"
+    Blocked = "blocked"
     BlockedProtectiveStop = "blocked_protective_stop"
 
     def __repr__(self):

tests/isar/state_machine/test_state_machine.py

@@ -25,8 +25,9 @@
 from tests.mocks.pose import MockPose
 from tests.mocks.robot_interface import (
     MockRobot,
-    MockRobotIdleToOfflineToIdleTest,
     MockRobotIdleToBlockedProtectiveStopToIdleTest,
+    MockRobotIdleToBlockedToIdleTest,
+    MockRobotIdleToOfflineToIdleTest,
 )
 from tests.mocks.task import MockTask
 
@@ -309,6 +310,21 @@ def test_state_machine_idle_to_offline_to_idle(mocker, state_machine_thread) ->
     )
 
 
+def test_state_machine_idle_to_blocked_to_idle(mocker, state_machine_thread) -> None:
+
+    # Robot status check happens every 5 seconds by default, so we mock the behavior
+    # to poll for status imediately
+    mocker.patch.object(Idle, "_is_ready_to_poll_for_status", return_value=True)
+
+    state_machine_thread.state_machine.robot = MockRobotIdleToBlockedToIdleTest()
+    state_machine_thread.start()
+    time.sleep(0.11)  # Slightly more than the StateMachine sleep time
+
+    assert state_machine_thread.state_machine.transitions_list == deque(
+        [States.Idle, States.Blocked, States.Idle]
+    )
+
+
 def test_state_machine_idle_to_blocked_protective_stop_to_idle(
     mocker, state_machine_thread
 ) -> None:

tests/mocks/robot_interface.py

@@ -110,6 +110,18 @@ def robot_status(self) -> RobotStatus:
         return RobotStatus.Available
 
 
+class MockRobotIdleToBlockedToIdleTest(MockRobot):
+    def __init__(self):
+        self.first = True
+
+    def robot_status(self) -> RobotStatus:
+        if self.first:
+            self.first = False
+            return RobotStatus.Blocked
+
+        return RobotStatus.Available
+
+
 class MockRobotIdleToBlockedProtectiveStopToIdleTest(MockRobot):
     def __init__(self):
         self.first = True