From e2c9adb78cba61874c1cc5e36c8c29cced15556c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Anders=20Chirico=20Indreb=C3=B8?= Date: Thu, 2 Jan 2025 14:35:53 +0100 Subject: [PATCH] WIP --- README.md | 2 - .../apis/schedule/scheduling_controller.py | 1 - src/isar/state_machine/state_machine.py | 196 +++++++++++++----- src/isar/state_machine/states/idle.py | 4 +- src/isar/state_machine/states/initialize.py | 71 ------- src/isar/state_machine/states/initiate.py | 142 ------------- src/isar/state_machine/states_enum.py | 2 - tests/conftest.py | 6 - .../apis/scheduler/test_scheduler_router.py | 2 - .../utilities/test_scheduling_utilities.py | 2 +- .../isar/state_machine/test_state_machine.py | 25 +-- 11 files changed, 151 insertions(+), 302 deletions(-) delete mode 100644 src/isar/state_machine/states/initialize.py delete mode 100644 src/isar/state_machine/states/initiate.py diff --git a/README.md b/README.md index d949657e..e78c4a90 100644 --- a/README.md +++ b/README.md @@ -211,8 +211,6 @@ In general the states ``` States.Off, -States.Initialize, -States.Initiate, States.Stop, States.Monitor, States.Paused, diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py index bcc4d61a..096c48d0 100644 --- a/src/isar/apis/schedule/scheduling_controller.py +++ b/src/isar/apis/schedule/scheduling_controller.py @@ -153,7 +153,6 @@ def pause_mission(self) -> ControlMissionResponse: if state not in [ States.Monitor, - States.Initiate, ]: error_message = ( f"Conflict - Pause command received in invalid state - State: {state}" diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 4912faff..59501150 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"{settings.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..70de7764 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,6 +43,7 @@ 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 @@ -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_enum.py b/src/isar/state_machine/states_enum.py index d5f4e72d..94c0e260 100644 --- a/src/isar/state_machine/states_enum.py +++ b/src/isar/state_machine/states_enum.py @@ -4,8 +4,6 @@ class States(str, Enum): Off = "off" Idle = "idle" - Initiate = "initiate" - Initialize = "initialize" Monitor = "monitor" Paused = "paused" Stop = "stop" diff --git a/tests/conftest.py b/tests/conftest.py index c6c9eb3b..2301a7c2 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -21,7 +21,6 @@ from isar.services.utilities.scheduling_utilities import SchedulingUtilities from isar.state_machine.state_machine import StateMachine from isar.state_machine.states.idle import Idle -from isar.state_machine.states.initiate import Initiate from isar.state_machine.states.monitor import Monitor from robot_interface.telemetry.mqtt_client import MqttClientInterface from tests.mocks.robot_interface import MockRobot @@ -117,11 +116,6 @@ def idle_state(state_machine): return Idle(state_machine) -@pytest.fixture() -def initiate(state_machine): - return Initiate(state_machine) - - @pytest.fixture() def monitor(state_machine): return Monitor(state_machine=state_machine) diff --git a/tests/isar/apis/scheduler/test_scheduler_router.py b/tests/isar/apis/scheduler/test_scheduler_router.py index 0b1c7cad..458845b4 100644 --- a/tests/isar/apis/scheduler/test_scheduler_router.py +++ b/tests/isar/apis/scheduler/test_scheduler_router.py @@ -207,8 +207,6 @@ def test_resume_mission_timeout(self, client: TestClient): class TestStopMission: schedule_stop_mission_path = "/schedule/stop-mission" valid_states = [ - States.Initiate, - States.Initialize, States.Idle, States.Monitor, States.Paused, diff --git a/tests/isar/services/utilities/test_scheduling_utilities.py b/tests/isar/services/utilities/test_scheduling_utilities.py index bbec2ed8..66ef2763 100644 --- a/tests/isar/services/utilities/test_scheduling_utilities.py +++ b/tests/isar/services/utilities/test_scheduling_utilities.py @@ -51,6 +51,6 @@ def test_state_machine_not_ready_to_receive_mission( ): with pytest.raises(HTTPException) as err: scheduling_utilities.verify_state_machine_ready_to_receive_mission( - States.Initialize + States.Monitor ) assert err.value.status_code == HTTPStatus.CONFLICT diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 75cbf04a..02b52d1c 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -97,12 +97,8 @@ def test_state_machine_transitions_when_running_mission_by_task( assert state_machine_thread.state_machine.transitions_list == deque( [ States.Idle, - States.Initialize, - States.Initiate, States.Monitor, - States.Initiate, States.Monitor, - States.Initiate, States.Idle, ] ) @@ -127,10 +123,7 @@ def test_state_machine_transitions_when_running_full_mission( assert state_machine_thread.state_machine.transitions_list == deque( [ States.Idle, - States.Initialize, - States.Initiate, States.Monitor, - States.Initiate, States.Idle, ] ) @@ -158,12 +151,8 @@ def test_state_machine_failed_dependency( assert state_machine_thread.state_machine.transitions_list == deque( [ States.Idle, - States.Initialize, - States.Initiate, States.Monitor, - States.Initiate, States.Monitor, - States.Initiate, States.Idle, ] ) @@ -187,10 +176,7 @@ def test_state_machine_with_successful_collection( assert state_machine_thread.state_machine.transitions_list == deque( [ States.Idle, - States.Initialize, - States.Initiate, States.Monitor, - States.Initiate, States.Idle, ] ) @@ -217,10 +203,7 @@ def test_state_machine_with_unsuccessful_collection( assert state_machine_thread.state_machine.transitions_list == deque( [ States.Idle, - States.Initialize, - States.Initiate, States.Monitor, - States.Initiate, States.Idle, ] ) @@ -232,14 +215,16 @@ def test_state_machine_with_successful_mission_stop( ) -> None: state_machine_thread.start() - mission: Mission = Mission(name="Dummy misson", tasks=[MockTask.take_image()]) + mission: Mission = Mission( + name="Dummy misson", tasks=[MockTask.take_image() for _ in range(0, 20)] + ) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission, initial_pose=None) 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.Monitor, States.Monitor, States.Stop, States.Idle] ) @@ -268,7 +253,7 @@ 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.Monitor, States.Stop, States.Idle] )