From bc8028402d2a90e86f91c71872b82744170e63b5 Mon Sep 17 00:00:00 2001 From: oysand Date: Thu, 16 Jan 2025 11:07:52 +0100 Subject: [PATCH] Add blocked state --- src/isar/state_machine/state_machine.py | 27 +++++++- src/isar/state_machine/states/blocked.py | 63 +++++++++++++++++++ src/isar/state_machine/states/idle.py | 3 + src/isar/state_machine/states_enum.py | 1 + .../isar/state_machine/test_state_machine.py | 18 +++++- tests/mocks/robot_interface.py | 12 ++++ 6 files changed, 121 insertions(+), 3 deletions(-) create mode 100644 src/isar/state_machine/states/blocked.py diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 55d8e2ce..fef45e5e 100644 --- a/src/isar/state_machine/state_machine.py +++ b/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: diff --git a/src/isar/state_machine/states/blocked.py b/src/isar/state_machine/states/blocked.py new file mode 100644 index 00000000..e6c5ad90 --- /dev/null +++ b/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() diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index d1e70813..a0189497 100644 --- a/src/isar/state_machine/states/idle.py +++ b/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 diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index a027c69e..2e438229 100644 --- a/src/isar/state_machine/states_enum.py +++ b/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): diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index c4cd0203..163c2494 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/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: diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index 054a8caf..5106e105 100644 --- a/tests/mocks/robot_interface.py +++ b/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