diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 4912faff..55d8e2ce 100644 --- a/src/isar/state_machine/state_machine.py +++ b/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 diff --git a/src/isar/state_machine/states/blocked_protective_stop.py b/src/isar/state_machine/states/blocked_protective_stop.py new file mode 100644 index 00000000..98bfc422 --- /dev/null +++ b/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() diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index 7ddae22c..d1e70813 100644 --- a/src/isar/state_machine/states/idle.py +++ b/src/isar/state_machine/states/idle.py @@ -24,6 +24,7 @@ def __init__(self, state_machine: "StateMachine") -> None: self.logger = logging.getLogger("state_machine") self.robot_status_thread: Optional[ThreadedRequest] = None self.last_robot_status_poll_time: float = time.time() + self.status_checked_at_least_once: bool = False def start(self) -> None: self.state_machine.update_state() @@ -33,8 +34,12 @@ def stop(self) -> None: if self.robot_status_thread: self.robot_status_thread.wait_for_thread() self.robot_status_thread = None + self.status_checked_at_least_once = False def _is_ready_to_poll_for_status(self) -> bool: + if not self.status_checked_at_least_once: + return True + time_since_last_robot_status_poll = ( time.time() - self.last_robot_status_poll_time ) @@ -47,17 +52,19 @@ def _run(self) -> None: if self.state_machine.should_stop_mission(): transition = self.state_machine.stop # type: ignore break - start_mission: Optional[StartMissionMessage] = ( - self.state_machine.should_start_mission() - ) - if start_mission: - self.state_machine.start_mission( - mission=start_mission.mission, - initial_pose=start_mission.initial_pose, + + if self.status_checked_at_least_once: + start_mission: Optional[StartMissionMessage] = ( + self.state_machine.should_start_mission() ) - transition = self.state_machine.mission_started # type: ignore - break - time.sleep(self.state_machine.sleep_time) + if start_mission: + self.state_machine.start_mission( + mission=start_mission.mission, + initial_pose=start_mission.initial_pose, + ) + transition = self.state_machine.mission_started # type: ignore + break + time.sleep(self.state_machine.sleep_time) if not self._is_ready_to_poll_for_status(): continue @@ -72,6 +79,7 @@ def _run(self) -> None: try: robot_status: RobotStatus = self.robot_status_thread.get_output() + self.status_checked_at_least_once = True except ThreadedRequestNotFinishedError: time.sleep(self.state_machine.sleep_time) continue @@ -86,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.BlockedProtectiveStop: + transition = self.state_machine.robot_protective_stop_engaged # type: ignore + break self.robot_status_thread = None diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index d5f4e72d..a027c69e 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" + BlockedProtectiveStop = "blocked_protective_stop" def __repr__(self): return self.value diff --git a/src/robot_interface/models/mission/status.py b/src/robot_interface/models/mission/status.py index c4c1237f..4f1e7f11 100644 --- a/src/robot_interface/models/mission/status.py +++ b/src/robot_interface/models/mission/status.py @@ -26,3 +26,4 @@ class RobotStatus(Enum): Busy = "busy" Offline = "offline" Blocked = "blocked" + BlockedProtectiveStop = "blockedprotectivestop" diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 75cbf04a..c4cd0203 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/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 @@ -228,18 +232,28 @@ def test_state_machine_with_unsuccessful_collection( def test_state_machine_with_successful_mission_stop( injector: Injector, + mocker: MockerFixture, state_machine_thread: StateMachineThread, ) -> None: state_machine_thread.start() mission: Mission = Mission(name="Dummy misson", tasks=[MockTask.take_image()]) + mocker.patch.object(MockRobot, "task_status", return_value=TaskStatus.InProgress) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission, initial_pose=None) + time.sleep(0.11) # Slightly more than the StateMachine sleep time scheduling_utilities.stop_mission() assert state_machine_thread.state_machine.transitions_list == deque( - [States.Idle, States.Initialize, States.Initiate, States.Stop, States.Idle] + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Stop, + States.Idle, + ] ) @@ -260,6 +274,7 @@ def test_state_machine_with_unsuccessful_mission_stop( state_machine_thread.start() scheduling_utilities.start_mission(mission=mission, initial_pose=None) + time.sleep(0.11) # Slightly more than the StateMachine sleep time scheduling_utilities.stop_mission() expected_log = ( @@ -268,7 +283,14 @@ def test_state_machine_with_unsuccessful_mission_stop( ) assert expected_log in caplog.text assert state_machine_thread.state_machine.transitions_list == deque( - [States.Idle, States.Initialize, States.Initiate, States.Stop, States.Idle] + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Stop, + States.Idle, + ] ) @@ -287,6 +309,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, diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index b3ba284f..054a8caf 100644 --- a/tests/mocks/robot_interface.py +++ b/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