diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 4912faff..234359de 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -19,15 +19,19 @@ from isar.models.communication.message import StartMissionMessage from isar.models.communication.queues.queues import Queues 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.paused import Paused from isar.state_machine.states.stop import Stop from isar.state_machine.states_enum import States -from robot_interface.models.exceptions.robot_exceptions import ErrorMessage +from robot_interface.models.exceptions.robot_exceptions import ( + ErrorMessage, + RobotException, + RobotInfeasibleMissionException, + RobotInfeasibleTaskException, + RobotInitializeException, +) from robot_interface.models.initialize.initialize_params import InitializeParams from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.status import MissionStatus, RobotStatus, TaskStatus @@ -87,17 +91,13 @@ def __init__( self.stop_state: State = Stop(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_state: State = Initiate(self) self.off_state: State = Off(self) self.offline_state: State = Offline(self) self.states: List[State] = [ self.off_state, self.idle_state, - self.initialize_state, - self.initiate_state, self.monitor_state, self.stop_state, self.paused_state, @@ -113,64 +113,42 @@ def __init__( "dest": self.idle_state, "before": self._off, }, - { - "trigger": "initiated", - "source": self.initiate_state, - "dest": self.monitor_state, - "before": self._initiated, - }, { "trigger": "pause_full_mission", - "source": [self.initiate_state, self.monitor_state], + "source": self.monitor_state, "dest": self.paused_state, "before": self._mission_paused, }, { "trigger": "pause", - "source": [self.initiate_state, self.monitor_state], + "source": self.monitor_state, "dest": self.stop_state, "before": self._pause, }, { "trigger": "stop", "source": [ - self.initiate_state, self.monitor_state, self.idle_state, ], "dest": self.stop_state, "before": self._stop, }, - { - "trigger": "mission_finished", - "source": [ - self.initiate_state, - ], - "dest": self.idle_state, - "before": self._mission_finished, - }, { "trigger": "mission_started", "source": self.idle_state, - "dest": self.initialize_state, - "before": self._mission_started, - }, - { - "trigger": "initialization_successful", - "source": self.initialize_state, - "dest": self.initiate_state, - "before": self._initialization_successful, + "dest": self.monitor_state, + "conditions": self._try_start_mission, }, { - "trigger": "initialization_failed", - "source": self.initialize_state, + "trigger": "mission_started", + "source": self.idle_state, "dest": self.idle_state, - "before": self._initialization_failed, }, { "trigger": "resume", "source": self.paused_state, - "dest": self.initiate_state, + "dest": self.monitor_state, "before": self._resume, }, { @@ -182,13 +160,19 @@ def __init__( { "trigger": "task_finished", "source": self.monitor_state, - "dest": self.initiate_state, - "before": self._task_finished, + "dest": self.monitor_state, + "conditions": self._start_task, + }, + { + "trigger": "task_finished", + "source": self.monitor_state, + "dest": self.idle_state, + "before": self._full_mission_finished, }, { "trigger": "full_mission_finished", "source": self.monitor_state, - "dest": self.initiate_state, + "dest": self.idle_state, "before": self._full_mission_finished, }, { @@ -197,18 +181,6 @@ def __init__( "dest": self.paused_state, "before": self._mission_paused, }, - { - "trigger": "initiate_infeasible", - "source": self.initiate_state, - "dest": self.initiate_state, - "before": self._initiate_infeasible, - }, - { - "trigger": "initiate_failed", - "source": self.initiate_state, - "dest": self.idle_state, - "before": self._initiate_failed, - }, { "trigger": "mission_stopped", "source": [self.stop_state, self.paused_state], @@ -316,7 +288,41 @@ def _mission_finished(self) -> None: self.current_mission.status = MissionStatus.Successful self._finalize() - def _mission_started(self) -> None: + def _start_task(self) -> bool: + self._task_finished() + + if self.current_task is None: + return False + + if not self._try_initiate_task_or_mission(): + return False + + if self.run_mission_by_task: + self.current_task.status = TaskStatus.InProgress + self.current_mission.status = MissionStatus.InProgress + self.publish_task_status(task=self.current_task) + self.logger.info( + f"Successfully initiated " + f"{type(self.current_task).__name__} " + f"task: {str(self.current_task.id)[:8]}" + ) + return True + + def _initialize_robot(self) -> bool: + try: + self.robot.initialize(self.get_initialize_params()) + except (RobotInitializeException, RobotException) as e: + self.current_task.error_message = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.logger.error( + f"Failed to initialize robot because: {e.error_description}" + ) + self._initialization_failed() + return False + return True + + def _initiate_mission(self) -> bool: self.queues.start_mission.output.put(True) self.logger.info( f"Initialization successful. Starting new mission: " @@ -328,10 +334,93 @@ def _mission_started(self) -> None: self.publish_mission_status() self.current_task = self.task_selector.next_task() if self.current_task is None: - self._mission_finished() + return False else: self.current_task.status = TaskStatus.InProgress self.publish_task_status(task=self.current_task) + return True + + def _set_mission_to_in_progress(self) -> None: + if self.run_mission_by_task: + self.current_task.status = TaskStatus.InProgress + self.current_mission.status = MissionStatus.InProgress + self.publish_task_status(task=self.current_task) + self.logger.info( + f"Successfully initiated " + f"{type(self.current_task).__name__} " + f"task: {str(self.current_task.id)[:8]}" + ) + + def _try_initiate_task_or_mission(self) -> bool: + retries = 0 + started_mission = False + try: + while not started_mission: + try: + if self.run_mission_by_task: + self.robot.initiate_task(self.current_task) + else: + self.robot.initiate_mission(self.current_mission) + except RobotException as e: + retries += 1 + self.logger.warning( + f"Initiating failed #: {str(retries)} " + f"because: {e.error_description}" + ) + + if retries >= settings.INITIATE_FAILURE_COUNTER_LIMIT: + self.current_task.error_message = ErrorMessage( + error_reason=e.error_reason, + error_description=e.error_description, + ) + self.logger.error( + f"Mission will be cancelled after failing to initiate " + f"{self.initiate_failure_counter_limit} times because: " + f"{e.error_description}" + ) + self._initiate_failed() + return False + started_mission = True + except RobotInfeasibleTaskException as e: + self.current_task.error_message = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.logger.warning( + f"Failed to initiate task " + f"{str(self.current_task.id)[:8]} after retrying " + f"{retries} times because: " + f"{e.error_description}" + ) + self._initiate_infeasible() + # We only fail the transition back to monitor if we are unable to continue the mission + return True + + except RobotInfeasibleMissionException as e: + self.current_mission.error_message = ErrorMessage( + error_reason=e.error_reason, error_description=e.error_description + ) + self.logger.warning( + f"Failed to initiate mission " + f"{str(self.current_mission.id)[:8]} because: " + f"{e.error_description}" + ) + self._initiate_failed() + return False + return True + + def _try_start_mission(self) -> bool: + if not self._initiate_mission(): + return False + + if not self._initialize_robot(): + return False + + if not self._try_initiate_task_or_mission(): + return False + + self._set_mission_to_in_progress() + + return True def _task_finished(self) -> None: self.publish_task_status(task=self.current_task) @@ -339,6 +428,7 @@ def _task_finished(self) -> None: self.iterate_current_task() def _full_mission_finished(self) -> None: + self._mission_finished() self.current_task = None def _mission_paused(self) -> None: diff --git a/src/isar/state_machine/states/idle.py b/src/isar/state_machine/states/idle.py index 7ddae22c..18406e7b 100644 --- a/src/isar/state_machine/states/idle.py +++ b/src/isar/state_machine/states/idle.py @@ -1,6 +1,6 @@ import logging import time -from typing import TYPE_CHECKING, Optional +from typing import TYPE_CHECKING, Callable, Optional from transitions import State @@ -43,10 +43,11 @@ def _is_ready_to_poll_for_status(self) -> bool: ) def _run(self) -> None: + transition: Callable while True: if self.state_machine.should_stop_mission(): transition = self.state_machine.stop # type: ignore - break + continue start_mission: Optional[StartMissionMessage] = ( self.state_machine.should_start_mission() ) @@ -88,5 +89,4 @@ def _run(self) -> None: break self.robot_status_thread = None - transition() diff --git a/src/isar/state_machine/states/initialize.py b/src/isar/state_machine/states/initialize.py deleted file mode 100644 index e719484a..00000000 --- a/src/isar/state_machine/states/initialize.py +++ /dev/null @@ -1,71 +0,0 @@ -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.robot_exceptions import ( - ErrorMessage, - RobotException, - RobotInitializeException, -) - -if TYPE_CHECKING: - from isar.state_machine.state_machine import StateMachine - - -class Initialize(State): - @inject - def __init__(self, state_machine: "StateMachine") -> None: - 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) -> None: - self.state_machine.update_state() - self._run() - - def stop(self) -> None: - if self.initialize_thread: - self.initialize_thread.wait_for_thread() - self.initialize_thread = None - - def _run(self) -> None: - transition: Callable - while True: - if not self.initialize_thread: - self.initialize_thread = ThreadedRequest( - self.state_machine.robot.initialize - ) - self.initialize_thread.start_thread( - self.state_machine.get_initialize_params(), - name="State Machine Initialize Robot", - ) - - try: - self.initialize_thread.get_output() - - except ThreadedRequestNotFinishedError: - time.sleep(self.state_machine.sleep_time) - continue - - except (RobotInitializeException, RobotException) as e: - self.state_machine.current_task.error_message = ErrorMessage( - error_reason=e.error_reason, error_description=e.error_description - ) - self.logger.error( - f"Failed to initialize robot because: {e.error_description}" - ) - transition = self.state_machine.initialization_failed # type: ignore - break - - transition = self.state_machine.initialization_successful # type: ignore - break - transition() diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py deleted file mode 100644 index 243e76e8..00000000 --- a/src/isar/state_machine/states/initiate.py +++ /dev/null @@ -1,142 +0,0 @@ -import logging -import time -from typing import TYPE_CHECKING, Any, Callable, 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 ( - ErrorMessage, - RobotException, - RobotInfeasibleMissionException, - RobotInfeasibleTaskException, -) - -if TYPE_CHECKING: - from isar.state_machine.state_machine import StateMachine - - -class Initiate(State): - def __init__(self, state_machine: "StateMachine") -> None: - super().__init__(name="initiate", on_enter=self.start, on_exit=self.stop) - self.state_machine: "StateMachine" = state_machine - self.initiate_failure_counter: int = 0 - self.initiate_failure_counter_limit: int = ( - settings.INITIATE_FAILURE_COUNTER_LIMIT - ) - self.logger = logging.getLogger("state_machine") - - self.initiate_thread: Optional[ThreadedRequest] = None - - def start(self) -> None: - self.state_machine.update_state() - self._run() - - def stop(self) -> None: - self.initiate_failure_counter = 0 - if self.initiate_thread: - self.initiate_thread.wait_for_thread() - self.initiate_thread = None - - def _run(self) -> None: - transition: Callable - while True: - if self.state_machine.should_stop_mission(): - transition = self.state_machine.stop # type: ignore - break - - if self.state_machine.should_pause_mission(): - transition = self.state_machine.pause # type: ignore - break - - if not self.state_machine.current_task: - self.logger.info( - f"Completed mission: {self.state_machine.current_mission.id}" - ) - transition = self.state_machine.mission_finished # type: ignore - break - - if not self.initiate_thread: - if self.state_machine.run_mission_by_task: - self._run_initiate_thread( - initiate_function=self.state_machine.robot.initiate_task, - function_argument=self.state_machine.current_task, - thread_name="State Machine Initiate Task", - ) - else: - self._run_initiate_thread( - initiate_function=self.state_machine.robot.initiate_mission, - function_argument=self.state_machine.current_mission, - thread_name="State Machine Initiate Mission", - ) - - try: - self.initiate_thread.get_output() - transition = self.state_machine.initiated # type: ignore - break - except ThreadedRequestNotFinishedError: - time.sleep(self.state_machine.sleep_time) - continue - except RobotInfeasibleTaskException as e: - self.state_machine.current_task.error_message = ErrorMessage( - error_reason=e.error_reason, error_description=e.error_description - ) - self.logger.warning( - f"Failed to initiate task " - f"{str(self.state_machine.current_task.id)[:8]} after retrying " - f"{self.initiate_failure_counter} times because: " - f"{e.error_description}" - ) - transition = self.state_machine.initiate_infeasible # type: ignore - break - - except RobotInfeasibleMissionException as e: - self.state_machine.current_mission.error_message = ErrorMessage( - error_reason=e.error_reason, error_description=e.error_description - ) - self.logger.warning( - f"Failed to initiate mission " - f"{str(self.state_machine.current_mission.id)[:8]} because: " - f"{e.error_description}" - ) - transition = self.state_machine.initiate_failed # type: ignore - break - - except RobotException as e: - self.initiate_thread = None - self.initiate_failure_counter += 1 - self.logger.warning( - f"Initiating failed #: {str(self.initiate_failure_counter)} " - f"because: {e.error_description}" - ) - - if self.initiate_failure_counter >= self.initiate_failure_counter_limit: - self.state_machine.current_task.error_message = ErrorMessage( - error_reason=e.error_reason, - error_description=e.error_description, - ) - self.logger.error( - f"Mission will be cancelled after failing to initiate " - f"{self.initiate_failure_counter_limit} times because: " - f"{e.error_description}" - ) - transition = self.state_machine.initiate_failed # type: ignore - break - - time.sleep(self.state_machine.sleep_time) - - transition() - - def _run_initiate_thread( - self, initiate_function: Callable, function_argument: Any, thread_name: str - ) -> None: - self.initiate_thread = ThreadedRequest(request_func=initiate_function) - - self.initiate_thread.start_thread( - function_argument, - name=thread_name, - ) diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 57f577db..65734dba 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -136,6 +136,8 @@ def _run(self) -> None: if self.state_machine.run_mission_by_task: if self.state_machine.current_task.is_finished(): self._report_task_status(self.state_machine.current_task) + # TODO: consider having it so that the transition from idle does not run any tasks, and instead does it from monitor -> monitor only + # TODO: check that this transitions, if not, set it up so that it will try the next task transition = self.state_machine.task_finished # type: ignore break else: