WIP

README.md

@@ -211,8 +211,6 @@ In general the states
 
 ```
 States.Off,
-States.Initialize,
-States.Initiate,
 States.Stop,
 States.Monitor,
 States.Paused,

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}"

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,17 +334,101 @@ 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)
         self.current_task.update_task_status()
         self.iterate_current_task()
 
     def _full_mission_finished(self) -> None:
+        self._mission_finished()
         self.current_task = None
 
     def _mission_paused(self) -> None:

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()