Add blocked protective stop state

src/isar/state_machine/state_machine.py

@@ -24,6 +24,7 @@
 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
@@ -92,6 +93,7 @@ def __init__(
         self.initiate_state: State = Initiate(self)
         self.off_state: State = Off(self)
         self.offline_state: State = Offline(self)
+        self.blocked_protective_stop: State = BlockedProtectiveStop(self)
 
         self.states: List[State] = [
             self.off_state,
@@ -102,6 +104,7 @@ def __init__(
             self.stop_state,
             self.paused_state,
             self.offline_state,
+            self.blocked_protective_stop,
         ]
 
         self.machine = Machine(self, states=self.states, initial="off", queued=True)
@@ -227,6 +230,18 @@ def __init__(
                     "dest": self.idle_state,
                     "before": self._online,
                 },
+                {
+                    "trigger": "robot_protective_stop_engaged",
+                    "source": [self.idle_state],
+                    "dest": self.blocked_protective_stop,
+                    "before": self._protective_stop_engaged,
+                },
+                {
+                    "trigger": "robot_protective_stop_disengaged",
+                    "source": self.blocked_protective_stop,
+                    "dest": self.idle_state,
+                    "before": self._protective_stop_disengaged,
+                },
             ]
         )
 
@@ -275,6 +290,12 @@ def _offline(self) -> None:
     def _online(self) -> None:
         return
 
+    def _protective_stop_engaged(self) -> None:
+        return
+
+    def _protective_stop_disengaged(self) -> None:
+        return
+
     def _resume(self) -> None:
         self.logger.info(f"Resuming mission: {self.current_mission.id}")
         self.current_mission.status = MissionStatus.InProgress
@@ -561,6 +582,8 @@ def _current_status(self) -> RobotStatus:
             return RobotStatus.Available
         elif self.current_state == States.Offline:
             return RobotStatus.Offline
+        elif self.current_state == States.BlockedProtectiveStop:
+            return RobotStatus.BlockedProtectiveStop
         else:
             return RobotStatus.Busy
 

src/isar/state_machine/states/blocked_protective_stop.py

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

src/isar/state_machine/states/idle.py

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

src/isar/state_machine/states_enum.py

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

src/robot_interface/models/mission/status.py

@@ -26,3 +26,4 @@ class RobotStatus(Enum):
     Busy = "busy"
     Offline = "offline"
     Blocked = "blocked"
+    BlockedProtectiveStop = "blockedprotectivestop"

tests/isar/state_machine/test_state_machine.py

@@ -23,7 +23,11 @@
 from robot_interface.models.mission.task import ReturnToHome, TakeImage, Task
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from tests.mocks.pose import MockPose
-from tests.mocks.robot_interface import MockRobot, MockRobotIdleToOfflineToIdleTest
+from tests.mocks.robot_interface import (
+    MockRobot,
+    MockRobotIdleToOfflineToIdleTest,
+    MockRobotIdleToBlockedProtectiveStopToIdleTest,
+)
 from tests.mocks.task import MockTask
 
 
@@ -287,6 +291,25 @@ def test_state_machine_idle_to_offline_to_idle(mocker, state_machine_thread) ->
     )
 
 
+def test_state_machine_idle_to_blocked_protective_stop_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 = (
+        MockRobotIdleToBlockedProtectiveStopToIdleTest()
+    )
+    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.BlockedProtectiveStop, States.Idle]
+    )
+
+
 def _mock_robot_exception_with_message() -> RobotException:
     raise RobotException(
         error_reason=ErrorReason.RobotUnknownErrorException,

tests/mocks/robot_interface.py

@@ -108,3 +108,15 @@ def robot_status(self) -> RobotStatus:
             return RobotStatus.Offline
 
         return RobotStatus.Available
+
+
+class MockRobotIdleToBlockedProtectiveStopToIdleTest(MockRobot):
+    def __init__(self):
+        self.first = True
+
+    def robot_status(self) -> RobotStatus:
+        if self.first:
+            self.first = False
+            return RobotStatus.BlockedProtectiveStop
+
+        return RobotStatus.Available