Include start_pose in mission

equinor · Jun 4, 2024 · 6d1c941 · 6d1c941
1 parent ac2400f
commit 6d1c941
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 1 deletion.
diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py
@@ -2,7 +2,7 @@
 from enum import Enum
 from typing import Any, Dict, List, Optional, Union
 
-from alitra import Position
+from alitra import Position, Pose, Orientation, Frame
 from pydantic import BaseModel, Field
 
 from isar.apis.models.models import InputPose, InputPosition
@@ -61,6 +61,7 @@ class StartMissionDefinition(BaseModel):
     tasks: List[StartMissionTaskDefinition]
     id: Optional[str] = None
     name: Optional[str] = None
+    start_pose: Optional[InputPose] = None
 
 
 def to_isar_mission(mission_definition: StartMissionDefinition) -> Mission:
@@ -92,6 +93,26 @@ def to_isar_mission(mission_definition: StartMissionDefinition) -> Mission:
     if mission_definition.id:
         isar_mission.id = mission_definition.id
 
+    if mission_definition.start_pose:
+        input_pose: InputPose = mission_definition.start_pose
+        input_frame: Frame = Frame(name=input_pose.frame_name)
+        input_position: Position = Position(
+            input_pose.position.x,
+            input_pose.position.y,
+            input_pose.position.z,
+            input_frame,
+        )
+        input_orientation: Orientation = Orientation(
+            input_pose.orientation.x,
+            input_pose.orientation.y,
+            input_pose.orientation.z,
+            input_pose.orientation.w,
+            input_frame,
+        )
+        isar_mission.start_pose = Pose(
+            position=input_position, orientation=input_orientation, frame=input_frame
+        )
+
     return isar_mission
 
 

diff --git a/src/robot_interface/models/mission/mission.py b/src/robot_interface/models/mission/mission.py
@@ -1,6 +1,8 @@
 from dataclasses import dataclass, field
 from typing import List, Optional
 
+from alitra import Pose
+
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
 from robot_interface.models.mission.status import MissionStatus
 from robot_interface.models.mission.task import Task
@@ -12,6 +14,7 @@ class Mission:
     tasks: List[Task]
     id: str = field(default_factory=uuid4_string, init=True)
     name: str = ""
+    start_pose: Optional[Pose] = None
     status: MissionStatus = MissionStatus.NotStarted
     error_message: Optional[ErrorMessage] = field(default=None, init=False)