From f629a31109ded105b856f04de10b93a437a15005 Mon Sep 17 00:00:00 2001
From: haakonsf <hf@equinor.com>
Date: Tue, 2 Apr 2024 16:08:17 +0200
Subject: [PATCH] Add configurable return to home

---
 src/isar/apis/models/start_mission_definition.py | 12 +++++++++++-
 src/isar/apis/schedule/scheduling_controller.py  |  7 ++++++-
 src/robot_interface/models/mission/step.py       | 11 +++++++++++
 3 files changed, 28 insertions(+), 2 deletions(-)

diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py
index 3579cf1f..ae43f7e6 100644
--- a/src/isar/apis/models/start_mission_definition.py
+++ b/src/isar/apis/models/start_mission_definition.py
@@ -10,11 +10,12 @@
 from isar.mission_planner.mission_planner_interface import MissionPlannerError
 from robot_interface.models.mission.mission import Mission
 from robot_interface.models.mission.step import (
+    STEPS,
     DockingProcedure,
     DriveToPose,
     Localize,
     RecordAudio,
-    STEPS,
+    ReturnToHome,
     TakeImage,
     TakeThermalImage,
     TakeThermalVideo,
@@ -35,6 +36,7 @@ class TaskType(str, Enum):
     Inspection: str = "inspection"
     DriveTo: str = "drive_to"
     Localization: str = "localization"
+    ReturnToHome: str = "return_to_home"
     Dock: str = "dock"
 
 
@@ -112,6 +114,8 @@ def generate_steps(task) -> List[STEPS]:
                 steps.append(generate_steps_for_drive_to_task(task=task))
             case TaskType.Localization:
                 steps.append(generate_steps_for_localization_task(task=task))
+            case TaskType.ReturnToHome:
+                steps.append(generate_steps_for_return_to_home_task(task=task))
             case TaskType.Dock:
                 steps.append(generate_steps_for_dock_task())
     except ValueError as e:
@@ -146,6 +150,12 @@ def generate_steps_for_localization_task(task: StartMissionTaskDefinition) -> Lo
     return Localize(localization_pose=task.pose.to_alitra_pose())
 
 
+def generate_steps_for_return_to_home_task(
+    task: StartMissionTaskDefinition,
+) -> ReturnToHome:
+    return ReturnToHome(pose=task.pose.to_alitra_pose())
+
+
 def generate_steps_for_dock_task() -> DockingProcedure:
     return DockingProcedure(behavior="dock")
 
diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py
index cafd6378..8463bf9b 100644
--- a/src/isar/apis/schedule/scheduling_controller.py
+++ b/src/isar/apis/schedule/scheduling_controller.py
@@ -21,7 +21,12 @@
 from isar.services.utilities.scheduling_utilities import SchedulingUtilities
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import DriveToPose, Localize, MoveArm
+from robot_interface.models.mission.step import (
+    DriveToPose,
+    Localize,
+    MoveArm,
+    ReturnToHome,
+)
 from robot_interface.models.mission.task import Task
 
 
diff --git a/src/robot_interface/models/mission/step.py b/src/robot_interface/models/mission/step.py
index 55dc2257..54b1175a 100644
--- a/src/robot_interface/models/mission/step.py
+++ b/src/robot_interface/models/mission/step.py
@@ -111,6 +111,16 @@ class DockingProcedure(MotionStep):
     type: Literal["docking_procedure"] = "docking_procedure"
 
 
+@dataclass
+class ReturnToHome(MotionStep):
+    """
+    Step which cases the robot to return home
+    """
+
+    pose: Pose
+    type: Literal["return_to_home"] = "return_to_home"
+
+
 @dataclass
 class Localize(MotionStep):
     """
@@ -213,6 +223,7 @@ def get_inspection_type() -> Type[Inspection]:
 STEPS = Union[
     DriveToPose,
     DockingProcedure,
+    ReturnToHome,
     Localize,
     TakeImage,
     TakeThermalImage,