Add initialization of robot to state machine

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,

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,13 +70,15 @@ 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)
 
         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:

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

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

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"

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

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,

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(