Add initialization of robot to state machine

src/isar/apis/models/__init__.py

@@ -1 +1 @@
-from .models import StartMissionResponse
+from .models import ApiPose, StartMissionResponse

src/isar/apis/models/models.py

@@ -1,3 +1,4 @@
+from email.policy import default
 from typing import List, Optional, Union
 from uuid import UUID
 
@@ -18,3 +19,12 @@ class TaskResponse(BaseModel):
 class StartMissionResponse(BaseModel):
     id: Union[UUID, int, str, None]
     tasks: List[TaskResponse]
+
+
+class ApiPose(BaseModel):
+    x: float
+    y: float
+    z: float
+    phi: float
+    theta: float
+    psi: float

src/isar/apis/schedule/scheduling_controller.py

@@ -1,14 +1,15 @@
 import logging
 from http import HTTPStatus
 from queue import Empty
-from typing import List
+from typing import List, Optional
 
+import numpy as np
 from alitra import Frame, Orientation, Pose, Position
-from fastapi import Query, Response
+from fastapi import Body, Query, Response
 from injector import inject
 from requests import HTTPError
 
-from isar.apis.models import StartMissionResponse
+from isar.apis.models import ApiPose, StartMissionResponse
 from isar.config.settings import robot_settings, settings
 from isar.mission_planner.mission_planner_interface import (
     MissionPlannerError,
@@ -44,6 +45,10 @@ def start_mission(
             title="Mission ID",
             description="ID-number for predefined mission",
         ),
+        initial_pose: Optional[ApiPose] = Body(
+            default=None,
+            description="The starting point of the mission. Used for initial localization of robot",
+        ),
     ):
         self.logger.info("Received request to start new mission")
         try:
@@ -56,6 +61,7 @@ def start_mission(
             return
 
         if state in [
+            States.Initialize,
             States.InitiateStep,
             States.StopStep,
             States.Monitor,
@@ -86,10 +92,32 @@ def start_mission(
             response.status_code = HTTPStatus.BAD_REQUEST.value
             return
 
+        initial_pose_alitra: Optional[Pose]
+        if initial_pose:
+            initial_pose_alitra = Pose(
+                position=Position(
+                    x=initial_pose.x,
+                    y=initial_pose.y,
+                    z=initial_pose.z,
+                    frame=Frame("asset"),
+                ),
+                orientation=Orientation.from_euler_array(
+                    euler=np.array(
+                        [initial_pose.phi, initial_pose.theta, initial_pose.psi]
+                    ),
+                    frame=Frame("asset"),
+                ),
+                frame=Frame("asset"),
+            )
+        else:
+            initial_pose_alitra = None
+
         self.logger.info(f"Starting mission: {mission.id}")
 
         try:
-            self.scheduling_utilities.start_mission(mission=mission)
+            self.scheduling_utilities.start_mission(
+                mission=mission, initial_pose=initial_pose_alitra
+            )
             self.logger.info("OK - Mission successfully started")
         except QueueTimeoutError:
             response.status_code = HTTPStatus.REQUEST_TIMEOUT.value
@@ -109,7 +137,7 @@ def pause_mission(self, response: Response):
             response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.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
@@ -132,7 +160,13 @@ def resume_mission(self, response: Response):
             response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.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
@@ -156,7 +190,7 @@ def stop_mission(self, response: Response):
             response.status_code = HTTPStatus.INTERNAL_SERVER_ERROR.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
@@ -202,6 +236,7 @@ def drive_to(
             return
 
         if state in [
+            States.Initialize,
             States.InitiateStep,
             States.StopStep,
             States.Monitor,
@@ -222,7 +257,7 @@ def drive_to(
         mission: Mission = Mission(tasks=[Task(steps=[step])])
 
         try:
-            self.scheduling_utilities.start_mission(mission=mission)
+            self.scheduling_utilities.start_mission(mission=mission, initial_pose=None)
             self.logger.info("OK - Drive to successfully started")
         except QueueTimeoutError:
             self.logger.error("Timout - Failed to start drive to")

src/isar/models/communication/message.py

@@ -1,6 +1,12 @@
 from dataclasses import dataclass
+from typing import Optional
+
+from alitra import Pose
+
+from isar.models.mission import Mission
 
 
 @dataclass
-class Message:
-    message: str
+class StartMissionMessage:
+    mission: Mission
+    initial_pose: Optional[Pose]

src/isar/services/utilities/scheduling_utilities.py

@@ -1,10 +1,12 @@
 import logging
 from copy import deepcopy
-from typing import Any
+from typing import Any, Optional
 
+from alitra import Pose
 from injector import inject
 
 from isar.config.settings import settings
+from isar.models.communication.message import StartMissionMessage
 from isar.models.communication.queues import QueueIO, Queues, QueueTimeoutError
 from isar.models.mission.mission import Mission
 from isar.services.utilities.queue_utilities import QueueUtilities
@@ -26,8 +28,11 @@ def __init__(self, queues: Queues, queue_timeout: int = settings.QUEUE_TIMEOUT):
     def get_state(self) -> States:
         return self.queues.state.check()
 
-    def start_mission(self, mission: Mission) -> None:
-        self._send_command(deepcopy(mission), self.queues.start_mission)
+    def start_mission(self, mission: Mission, initial_pose: Optional[Pose]) -> None:
+        self._send_command(
+            StartMissionMessage(mission=deepcopy(mission), initial_pose=initial_pose),
+            self.queues.start_mission,
+        )
 
     def pause_mission(self) -> None:
         self._send_command(True, self.queues.pause_mission)

src/isar/state_machine/state_machine.py

@@ -5,18 +5,29 @@
 from datetime import datetime
 from typing import Deque, List, Optional
 
+from alitra import Pose
 from injector import Injector, inject
 from transitions import Machine
 from transitions.core import State
 
 from isar.config.settings import settings
+from isar.models.communication.message import StartMissionMessage
 from isar.models.communication.queues.queues import Queues
 from isar.models.mission import Mission, Task
 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.initialize.initialize_params import InitializeParams
 from robot_interface.models.mission import StepStatus
 from robot_interface.models.mission.step import Step
 from robot_interface.robot_interface import RobotInterface
@@ -61,13 +72,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 +125,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,
@@ -161,6 +186,7 @@ def __init__(
         self.current_mission: Optional[Mission] = None
         self.current_task: Optional[Task] = None
         self.current_step: Optional[Step] = None
+        self.initial_pose: Optional[Pose] = None
 
         self.current_state: State = States(self.state)  # type: ignore
 
@@ -171,6 +197,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 +241,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()
@@ -302,13 +341,15 @@ def reset_state_machine(self) -> None:
         self.current_task = None
         self.current_mission = None
 
-    def start_mission(self, mission: Mission):
+    def start_mission(self, mission: Mission, initial_pose: Pose):
         """Starts a scheduled mission."""
         self.current_mission = mission
-        self.current_task = mission.next_task()
-        self.queues.start_mission.output.put(True)
+        self.initial_pose = initial_pose
+
+    def get_initialize_params(self):
+        return InitializeParams(initial_pose=self.initial_pose)
 
-    def should_start_mission(self) -> Optional[Mission]:
+    def should_start_mission(self) -> Optional[StartMissionMessage]:
         try:
             return self.queues.start_mission.input.get(block=False)
         except queue.Empty:

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/idle.py

@@ -1,10 +1,10 @@
 import logging
 import time
-from typing import TYPE_CHECKING
+from typing import TYPE_CHECKING, Optional
 
 from transitions import State
 
-from isar.models.mission import Mission
+from isar.models.communication.message import StartMissionMessage
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
@@ -25,9 +25,14 @@ def stop(self):
 
     def _run(self):
         while True:
-            mission: Mission = self.state_machine.should_start_mission()
-            if mission:
-                self.state_machine.start_mission(mission)
+            start_mission: Optional[
+                StartMissionMessage
+            ] = self.state_machine.should_start_mission()
+            if start_mission:
+                self.state_machine.start_mission(
+                    mission=start_mission.mission,
+                    initial_pose=start_mission.initial_pose,
+                )
                 transition = self.state_machine.mission_started
                 break
             time.sleep(self.state_machine.sleep_time)