From 6b8b6458d1ca3f567b8240a24a67e3a91fc2e352 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Vetle=20Kopperg=C3=A5rd?= Date: Mon, 30 May 2022 11:08:14 +0200 Subject: [PATCH] Add initialization of robot to state machine --- .../apis/schedule/scheduling_controller.py | 14 ++++- src/isar/state_machine/state_machine.py | 52 +++++++++++++--- src/isar/state_machine/states/__init__.py | 1 + src/isar/state_machine/states/initialize.py | 61 +++++++++++++++++++ src/isar/state_machine/states_enum.py | 1 + .../utilities/test_scheduling_utilities.py | 4 ++ .../isar/state_machine/test_state_machine.py | 11 ++-- tests/mocks/robot_interface.py | 3 + 8 files changed, 128 insertions(+), 19 deletions(-) create mode 100644 src/isar/state_machine/states/initialize.py diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py index 16bae443..c0a61d12 100644 --- a/src/isar/apis/schedule/scheduling_controller.py +++ b/src/isar/apis/schedule/scheduling_controller.py @@ -53,6 +53,7 @@ def start_mission( return if state in [ + States.Initialize, States.InitiateStep, States.StopStep, States.Monitor, @@ -104,7 +105,7 @@ def pause_mission(self, response: Response): response.status_code = HTTPStatus.REQUEST_TIMEOUT.value return - if state in [States.Idle, States.StopStep, States.Paused]: + if state in [States.Idle, States.StopStep, States.Paused, States.Initialize]: self.logger.info("Conflict - Pause command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -125,7 +126,13 @@ def resume_mission(self, response: Response): response.status_code = HTTPStatus.REQUEST_TIMEOUT.value return - if state in [States.Idle, States.InitiateStep, States.Monitor, States.StopStep]: + if state in [ + States.Idle, + States.InitiateStep, + States.Monitor, + States.StopStep, + States.Initialize, + ]: self.logger.info("Conflict - Resume command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -147,7 +154,7 @@ def stop_mission(self, response: Response): response.status_code = HTTPStatus.REQUEST_TIMEOUT.value return - if state in [States.Idle]: + if state in [States.Idle, States.Initialize]: self.logger.info("Conflict - Stop command received in invalid state") response.status_code = HTTPStatus.CONFLICT.value return @@ -191,6 +198,7 @@ def drive_to( return if state in [ + States.Initialize, States.InitiateStep, States.StopStep, States.Monitor, diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 56f0667b..821694c3 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -6,6 +6,7 @@ from typing import Deque, List, Optional from injector import Injector, inject +from this import d from transitions import Machine from transitions.core import State @@ -15,7 +16,15 @@ from isar.models.mission.status import MissionStatus, TaskStatus from isar.services.service_connections.mqtt.mqtt_client import MqttClientInterface from isar.services.utilities.json_service import EnhancedJSONEncoder -from isar.state_machine.states import Idle, InitiateStep, Monitor, Off, Paused, StopStep +from isar.state_machine.states import ( + Idle, + Initialize, + InitiateStep, + Monitor, + Off, + Paused, + StopStep, +) from isar.state_machine.states_enum import States from robot_interface.models.mission import StepStatus from robot_interface.models.mission.step import Step @@ -61,6 +70,7 @@ def __init__( self.stop_step_state: State = StopStep(self) self.paused_state: State = Paused(self) self.idle_state: State = Idle(self) + self.initialize_state: State = Initialize(self) self.monitor_state: State = Monitor(self) self.initiate_step_state: State = InitiateStep(self) self.off_state: State = Off(self) @@ -68,6 +78,7 @@ def __init__( self.states: List[State] = [ self.off_state, self.idle_state, + self.initialize_state, self.initiate_step_state, self.monitor_state, self.stop_step_state, @@ -112,9 +123,21 @@ def __init__( { "trigger": "mission_started", "source": self.idle_state, - "dest": self.initiate_step_state, + "dest": self.initialize_state, "before": self._mission_started, }, + { + "trigger": "initialization_successful", + "source": self.initialize_state, + "dest": self.initiate_step_state, + "before": self._initialization_successful, + }, + { + "trigger": "initialization_failed", + "source": self.initialize_state, + "dest": self.idle_state, + "before": self._initialization_failed, + }, { "trigger": "resume", "source": self.paused_state, @@ -171,6 +194,23 @@ def __init__( ################################################################################# # Transition Callbacks + def _initialization_successful(self) -> None: + self.queues.start_mission.output.put(True) + self.logger.info( + f"Initialization successful. Starting new mission: {self.current_mission.id}" + ) + self.log_step_overview(mission=self.current_mission) + self.current_mission.status = MissionStatus.InProgress + self.publish_mission_status() + self.current_task = self.current_mission.next_task() + self.current_task.status = TaskStatus.InProgress + self.publish_task_status() + self.update_current_step() + + def _initialization_failed(self) -> None: + self.queues.start_mission.output.put(False) + self._finalize() + def _step_initiated(self) -> None: self.current_step.status = StepStatus.InProgress self.publish_step_status() @@ -198,11 +238,7 @@ def _mission_finished(self) -> None: self._finalize() def _mission_started(self) -> None: - self.logger.info(f"Starting new mission: {self.current_mission.id}") - self.current_mission.status = MissionStatus.InProgress - self.current_task.status = TaskStatus.InProgress - self.log_step_overview(mission=self.current_mission) - self.update_current_step() + return def _step_finished(self) -> None: self.publish_step_status() @@ -308,8 +344,6 @@ def send_status(self): def start_mission(self, mission: Mission): """Starts a scheduled mission.""" self.current_mission = mission - self.current_task = mission.next_task() - self.queues.start_mission.output.put(True) def should_send_status(self) -> bool: try: diff --git a/src/isar/state_machine/states/__init__.py b/src/isar/state_machine/states/__init__.py index 14080df5..23459eb4 100644 --- a/src/isar/state_machine/states/__init__.py +++ b/src/isar/state_machine/states/__init__.py @@ -1,4 +1,5 @@ from .idle import Idle +from .initialize import Initialize from .initiate_step import InitiateStep from .monitor import Monitor from .off import Off diff --git a/src/isar/state_machine/states/initialize.py b/src/isar/state_machine/states/initialize.py new file mode 100644 index 00000000..ec69599c --- /dev/null +++ b/src/isar/state_machine/states/initialize.py @@ -0,0 +1,61 @@ +import logging +import time +from typing import TYPE_CHECKING, Callable, Optional + +from injector import inject +from transitions import State + +from isar.services.utilities.threaded_request import ( + ThreadedRequest, + ThreadedRequestNotFinishedError, +) +from robot_interface.models.exceptions import RobotException + +if TYPE_CHECKING: + from isar.state_machine.state_machine import StateMachine + + +class Initialize(State): + @inject + def __init__(self, state_machine: "StateMachine"): + super().__init__(name="initialize", on_enter=self.start, on_exit=self.stop) + self.state_machine: "StateMachine" = state_machine + + self.logger = logging.getLogger("state_machine") + self.initialize_thread: Optional[ThreadedRequest] = None + + def start(self): + self.state_machine.update_state() + self._run() + + def stop(self): + if self.initialize_thread: + self.initialize_thread.wait_for_thread() + self.initialize_thread = None + + def _run(self): + transition: Callable + while True: + if self.state_machine.should_send_status(): + self.state_machine.send_status() + + if not self.initialize_thread: + self.initialize_thread = ThreadedRequest( + self.state_machine.robot.initialize + ) + self.initialize_thread.start_thread() + + try: + self.initialize_thread.get_output() + except ThreadedRequestNotFinishedError: + time.sleep(self.state_machine.sleep_time) + continue + except RobotException as e: + self.logger.error("Initialization of robot failed") + self.logger.error(e) + transition = self.state_machine.initialization_failed + break + + transition = self.state_machine.initialization_successful + break + transition() diff --git a/src/isar/state_machine/states_enum.py b/src/isar/state_machine/states_enum.py index 8f02614f..f01da215 100644 --- a/src/isar/state_machine/states_enum.py +++ b/src/isar/state_machine/states_enum.py @@ -6,6 +6,7 @@ class States(str, Enum): Off = "off" Idle = "idle" InitiateStep = "initiate_step" + Initialize = "initialize" Monitor = "monitor" Paused = "paused" StopStep = "stop_step" diff --git a/tests/isar/services/utilities/test_scheduling_utilities.py b/tests/isar/services/utilities/test_scheduling_utilities.py index f20c13a8..9a6a0703 100644 --- a/tests/isar/services/utilities/test_scheduling_utilities.py +++ b/tests/isar/services/utilities/test_scheduling_utilities.py @@ -11,3 +11,7 @@ def test_timeout_send_command(mocker, scheduling_utilities): with pytest.raises(QueueTimeoutError): scheduling_utilities._send_command(True, q) assert q.input.empty() + + +import queue +from queue import Queue diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 08453a68..94464d0d 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -92,13 +92,6 @@ def test_reset_state_machine(state_machine): empty_mission: Mission = Mission([], None) -def test_start_mission(state_machine): - mission: Mission = MockMissionDefinition.default_mission - state_machine.start_mission(mission=mission) - message = state_machine.queues.start_mission.output.get() - assert message - - def test_state_machine_transitions(injector, state_machine_thread): step: Step = DriveToPose(pose=MockPose.default_pose) mission: Mission = Mission(tasks=[Task(steps=[step])]) @@ -110,6 +103,7 @@ def test_state_machine_transitions(injector, state_machine_thread): expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -135,6 +129,7 @@ def test_state_machine_failed_dependency(injector, state_machine_thread, mocker) expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -160,6 +155,7 @@ def test_state_machine_with_successful_collection( expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, @@ -189,6 +185,7 @@ def test_state_machine_with_unsuccessful_collection( expected_transitions_list = deque( [ States.Idle, + States.Initialize, States.InitiateStep, States.Monitor, States.InitiateStep, diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index 7f647868..11178af8 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -44,6 +44,9 @@ def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: image.data = b"Some binary image data" return [image] + def initialize(self, **kwargs) -> None: + return + def mock_image_metadata() -> ImageMetadata: return ImageMetadata(