diff --git a/src/isar/apis/api.py b/src/isar/apis/api.py
index 69f4d877..98cb4502 100644
--- a/src/isar/apis/api.py
+++ b/src/isar/apis/api.py
@@ -213,6 +213,7 @@ def _create_scheduler_router(self) -> APIRouter:
             methods=["POST"],
             dependencies=[authentication_dependency],
             summary="Drive to the provided pose",
+            deprecated=True,
             responses={
                 HTTPStatus.OK.value: {
                     "description": "Drive to succesfully started",
diff --git a/src/isar/apis/models/models.py b/src/isar/apis/models/models.py
index 93d211f7..48289016 100644
--- a/src/isar/apis/models/models.py
+++ b/src/isar/apis/models/models.py
@@ -3,16 +3,13 @@
 from alitra import Frame, Orientation, Pose, Position
 from pydantic import BaseModel, Field
 
-
-class StepResponse(BaseModel):
-    id: str
-    type: str
+from robot_interface.models.mission.task import TaskTypes
 
 
 class TaskResponse(BaseModel):
     id: str
     tag_id: Optional[str] = None
-    steps: List[StepResponse]
+    type: TaskTypes
 
 
 class StartMissionResponse(BaseModel):
@@ -25,8 +22,6 @@ class ControlMissionResponse(BaseModel):
     mission_status: str
     task_id: str
     task_status: str
-    step_id: str
-    step_status: str
 
 
 class RobotInfoResponse(BaseModel):
diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py
index 6a74ba9e..4eaf3816 100644
--- a/src/isar/apis/models/start_mission_definition.py
+++ b/src/isar/apis/models/start_mission_definition.py
@@ -1,6 +1,6 @@
 import time
 from enum import Enum
-from typing import Any, Dict, List, Optional, Union
+from typing import Any, Dict, List, Optional
 
 from alitra import Frame, Orientation, Pose, Position
 from pydantic import BaseModel, Field
@@ -8,11 +8,11 @@
 from isar.apis.models.models import InputPose, InputPosition
 from isar.config.settings import settings
 from isar.mission_planner.mission_planner_interface import MissionPlannerError
+from robot_interface.models.inspection.inspection import Inspection, InspectionMetadata
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import (
-    STEPS,
+from robot_interface.models.mission.task import (
+    TASKS,
     DockingProcedure,
-    DriveToPose,
     Localize,
     RecordAudio,
     ReturnToHome,
@@ -34,7 +34,6 @@ class InspectionTypes(str, Enum):
 
 class TaskType(str, Enum):
     Inspection: str = "inspection"
-    DriveTo: str = "drive_to"
     Localization: str = "localization"
     ReturnToHome: str = "return_to_home"
     Dock: str = "dock"
@@ -52,7 +51,7 @@ class StartMissionInspectionDefinition(BaseModel):
 class StartMissionTaskDefinition(BaseModel):
     type: TaskType = Field(default=TaskType.Inspection)
     pose: InputPose
-    inspections: List[StartMissionInspectionDefinition]
+    inspection: StartMissionInspectionDefinition
     tag: Optional[str] = None
     id: Optional[str] = None
 
@@ -66,40 +65,35 @@ class StartMissionDefinition(BaseModel):
     undock: Optional[bool] = None
 
 
-def to_isar_mission(mission_definition: StartMissionDefinition) -> Mission:
-    isar_tasks: List[Task] = []
-    all_steps_in_mission: List[STEPS] = []
+def to_isar_mission(start_mission_definition: StartMissionDefinition) -> Mission:
+    isar_tasks: List[TASKS] = []
 
-    for task in mission_definition.tasks:
-        steps: List[STEPS] = generate_steps(task)
-        all_steps_in_mission.extend(steps)
-
-        isar_task: Task = Task(steps=steps, tag_id=task.tag)
-        if task.id:
-            isar_task.id = task.id
-        isar_tasks.append(isar_task)
+    for start_mission_task_definition in start_mission_definition.tasks:
+        task: TASKS = create_isar_task(start_mission_task_definition)
+        if start_mission_task_definition.id:
+            task.id = start_mission_task_definition.id
+        isar_tasks.append(task)
 
     if not isar_tasks:
         raise MissionPlannerError("Mission does not contain any valid tasks")
 
     check_for_duplicate_ids(isar_tasks)
-    check_for_duplicate_ids(all_steps_in_mission)
 
     isar_mission: Mission = Mission(tasks=isar_tasks)
 
-    isar_mission.dock = mission_definition.dock
-    isar_mission.undock = mission_definition.undock
+    isar_mission.dock = start_mission_definition.dock
+    isar_mission.undock = start_mission_definition.undock
 
-    if mission_definition.name:
-        isar_mission.name = mission_definition.name
+    if start_mission_definition.name:
+        isar_mission.name = start_mission_definition.name
     else:
         isar_mission.name = _build_mission_name()
 
-    if mission_definition.id:
-        isar_mission.id = mission_definition.id
+    if start_mission_definition.id:
+        isar_mission.id = start_mission_definition.id
 
-    if mission_definition.start_pose:
-        input_pose: InputPose = mission_definition.start_pose
+    if start_mission_definition.start_pose:
+        input_pose: InputPose = start_mission_definition.start_pose
         input_frame: Frame = Frame(name=input_pose.frame_name)
         input_position: Position = Position(
             input_pose.position.x,
@@ -121,7 +115,7 @@ def to_isar_mission(mission_definition: StartMissionDefinition) -> Mission:
     return isar_mission
 
 
-def check_for_duplicate_ids(items: Union[List[Task], List[STEPS]]):
+def check_for_duplicate_ids(items: List[TASKS]):
     duplicate_ids = get_duplicate_ids(items=items)
     if len(duplicate_ids) > 0:
         raise MissionPlannerError(
@@ -130,97 +124,92 @@ def check_for_duplicate_ids(items: Union[List[Task], List[STEPS]]):
         )
 
 
-def generate_steps(task) -> List[STEPS]:
-    steps: List[STEPS] = []
+def create_isar_task(start_mission_task_definition) -> TASKS:
 
-    if task.type == TaskType.Inspection:
-        steps.extend(generate_steps_for_inspection_task(task=task))
-    elif task.type == TaskType.DriveTo:
-        steps.append(generate_steps_for_drive_to_task(task=task))
-    elif task.type == TaskType.Localization:
-        steps.append(generate_steps_for_localization_task(task=task))
-    elif task.type == TaskType.ReturnToHome:
-        steps.append(generate_steps_for_return_to_home_task(task=task))
-    elif task.type == TaskType.Dock:
-        steps.append(generate_steps_for_dock_task())
+    if start_mission_task_definition.type == TaskType.Inspection:
+        return create_inspection_task(start_mission_task_definition)
+    elif start_mission_task_definition.type == TaskType.Localization:
+        return create_localization_task(start_mission_task_definition)
+    elif start_mission_task_definition.type == TaskType.ReturnToHome:
+        return create_return_to_home_task(start_mission_task_definition)
+    elif start_mission_task_definition.type == TaskType.Dock:
+        return create_dock_task()
     else:
         raise MissionPlannerError(
-            f"Failed to create task: '{task.type}' is not a valid"
+            f"Failed to create task: '{start_mission_task_definition.type}' is not a valid"
         )
 
-    return steps
-
 
-def generate_steps_for_inspection_task(task: StartMissionTaskDefinition) -> List[STEPS]:
-    drive_step: DriveToPose = DriveToPose(pose=task.pose.to_alitra_pose())
+def create_inspection_task(
+    start_mission_task_definition: StartMissionTaskDefinition,
+) -> TASKS:
 
-    inspection_steps: List[STEPS] = [
-        create_inspection_step(
-            inspection_type=inspection.type,
-            duration=inspection.duration,
-            target=inspection.inspection_target.to_alitra_position(),
-            tag_id=task.tag,
-            metadata=inspection.metadata,
-            id=inspection.id,
+    if start_mission_task_definition.inspection.type == InspectionTypes.image:
+        return TakeImage(
+            target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(),
+            tag_id=start_mission_task_definition.tag,
+            robot_pose=start_mission_task_definition.pose.to_alitra_pose(),
+            metadata=start_mission_task_definition.inspection.metadata,
+        )
+    elif start_mission_task_definition.inspection.type == InspectionTypes.video:
+        return TakeVideo(
+            target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(),
+            duration=start_mission_task_definition.inspection.duration,
+            tag_id=start_mission_task_definition.tag,
+            robot_pose=start_mission_task_definition.pose.to_alitra_pose(),
+            metadata=start_mission_task_definition.inspection.metadata,
         )
-        for inspection in task.inspections
-    ]
 
-    return [drive_step, *inspection_steps]
+    elif start_mission_task_definition.inspection.type == InspectionTypes.thermal_image:
+        return TakeThermalImage(
+            target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(),
+            tag_id=start_mission_task_definition.tag,
+            robot_pose=start_mission_task_definition.pose.to_alitra_pose(),
+            metadata=start_mission_task_definition.inspection.metadata,
+        )
 
+    elif start_mission_task_definition.inspection.type == InspectionTypes.thermal_video:
+        return TakeThermalVideo(
+            target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(),
+            duration=start_mission_task_definition.inspection.duration,
+            tag_id=start_mission_task_definition.tag,
+            robot_pose=start_mission_task_definition.pose.to_alitra_pose(),
+            metadata=start_mission_task_definition.inspection.metadata,
+        )
 
-def generate_steps_for_drive_to_task(task: StartMissionTaskDefinition) -> DriveToPose:
-    return DriveToPose(pose=task.pose.to_alitra_pose())
+    elif start_mission_task_definition.inspection.type == InspectionTypes.audio:
+        return RecordAudio(
+            target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(),
+            duration=start_mission_task_definition.inspection.duration,
+            tag_id=start_mission_task_definition.tag,
+            robot_pose=start_mission_task_definition.pose.to_alitra_pose(),
+            metadata=start_mission_task_definition.inspection.metadata,
+        )
+    else:
+        raise ValueError(
+            f"Inspection type '{start_mission_task_definition.inspection.type}' not supported"
+        )
 
 
-def generate_steps_for_localization_task(task: StartMissionTaskDefinition) -> Localize:
-    return Localize(localization_pose=task.pose.to_alitra_pose())
+def create_localization_task(
+    start_mission_task_definition: StartMissionTaskDefinition,
+) -> Localize:
+    return Localize(
+        localization_pose=start_mission_task_definition.pose.to_alitra_pose()
+    )
 
 
-def generate_steps_for_return_to_home_task(
-    task: StartMissionTaskDefinition,
+def create_return_to_home_task(
+    start_mission_task_definition: StartMissionTaskDefinition,
 ) -> ReturnToHome:
-    return ReturnToHome(pose=task.pose.to_alitra_pose())
+    return ReturnToHome(pose=start_mission_task_definition.pose.to_alitra_pose())
 
 
-def generate_steps_for_dock_task() -> DockingProcedure:
+def create_dock_task() -> DockingProcedure:
     return DockingProcedure(behavior="dock")
 
 
-def create_inspection_step(
-    inspection_type: InspectionTypes,
-    duration: float,
-    target: Position,
-    tag_id: Optional[str],
-    metadata: Optional[dict],
-    id: Optional[str],
-) -> STEPS:
-    inspection_step_dict: Dict[str, Any] = {
-        InspectionTypes.image.value: TakeImage(target=target),
-        InspectionTypes.video.value: TakeVideo(target=target, duration=duration),
-        InspectionTypes.thermal_image.value: TakeThermalImage(target=target),
-        InspectionTypes.thermal_video.value: TakeThermalVideo(
-            target=target, duration=duration
-        ),
-        InspectionTypes.audio.value: RecordAudio(target=target, duration=duration),
-    }
-
-    if inspection_type not in inspection_step_dict:
-        raise ValueError(f"Inspection type '{inspection_type}' not supported")
-    else:
-        inspection_step = inspection_step_dict[inspection_type]
-
-    if tag_id:
-        inspection_step.tag_id = tag_id
-    if metadata:
-        inspection_step.metadata = metadata
-    if id:
-        inspection_step.id = id
-
-    return inspection_step
-
-
-def get_duplicate_ids(items: Union[List[Task], List[STEPS]]) -> List[str]:
+def get_duplicate_ids(items: List[TASKS]) -> List[str]:
     unique_ids: List[str] = []
     duplicate_ids: List[str] = []
     for item in items:
diff --git a/src/isar/apis/schedule/scheduling_controller.py b/src/isar/apis/schedule/scheduling_controller.py
index 8463bf9b..1562353b 100644
--- a/src/isar/apis/schedule/scheduling_controller.py
+++ b/src/isar/apis/schedule/scheduling_controller.py
@@ -1,6 +1,6 @@
 import logging
 from http import HTTPStatus
-from typing import List, Optional
+from typing import Optional
 
 from alitra import Pose
 from fastapi import Body, HTTPException, Path
@@ -21,8 +21,8 @@
 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,
+from robot_interface.models.mission.task import (
+    TASKS,
     Localize,
     MoveArm,
     ReturnToHome,
@@ -67,8 +67,7 @@ def start_mission_by_id(
         mission: Mission = self.scheduling_utilities.get_mission(mission_id)
         if return_pose:
             pose: Pose = return_pose.to_alitra_pose()
-            step: DriveToPose = DriveToPose(pose=pose)
-            mission.tasks.append(Task(steps=[step]))
+            mission.tasks.append(ReturnToHome(pose=pose))
 
         self.scheduling_utilities.verify_robot_capable_of_mission(
             mission=mission, robot_capabilities=robot_settings.CAPABILITIES
@@ -136,8 +135,7 @@ def start_mission(
         )
         if return_pose:
             pose: Pose = return_pose.to_alitra_pose()
-            step: DriveToPose = DriveToPose(pose=pose)
-            mission.tasks.append(Task(steps=[step]))
+            mission.tasks.append(ReturnToHome(pose=pose))
 
         initial_pose_alitra: Optional[Pose] = (
             initial_pose.to_alitra_pose() if initial_pose else None
@@ -211,7 +209,7 @@ def drive_to(
         target_pose: InputPose = Body(
             default=None,
             title="Target Pose",
-            description="The target pose for the drive_to step",
+            description="The target pose for the drive_to task",
         ),
     ) -> StartMissionResponse:
         self.logger.info("Received request to start new drive-to mission")
@@ -221,8 +219,7 @@ def drive_to(
         self.scheduling_utilities.verify_state_machine_ready_to_receive_mission(state)
 
         pose: Pose = target_pose.to_alitra_pose()
-        step: DriveToPose = DriveToPose(pose=pose)
-        mission: Mission = Mission(tasks=[Task(steps=[step])])
+        mission: Mission = Mission(tasks=[ReturnToHome(pose=pose)])
 
         self.logger.info(
             f"Starting drive to mission with ISAR Mission ID: '{mission.id}'"
@@ -246,8 +243,7 @@ def start_localization_mission(
         self.scheduling_utilities.verify_state_machine_ready_to_receive_mission(state)
 
         pose: Pose = localization_pose.to_alitra_pose()
-        step: Localize = Localize(localization_pose=pose)
-        mission: Mission = Mission(tasks=[Task(steps=[step])])
+        mission: Mission = Mission(tasks=[Localize(localization_pose=pose)])
 
         self.logger.info(
             f"Starting localization mission with ISAR Mission ID: '{mission.id}'"
@@ -294,8 +290,7 @@ def start_move_arm_mission(
 
         self.scheduling_utilities.verify_state_machine_ready_to_receive_mission(state)
 
-        step: MoveArm = MoveArm(arm_pose=arm_pose_literal)
-        mission: Mission = Mission(tasks=[Task(steps=[step])])
+        mission: Mission = Mission(tasks=[MoveArm(arm_pose=arm_pose_literal)])
 
         self.logger.info(
             f"Starting move arm mission with ISAR Mission ID: '{mission.id}'"
@@ -322,9 +317,5 @@ def _api_response(self, mission: Mission) -> StartMissionResponse:
             tasks=[self._task_api_response(task) for task in mission.tasks],
         )
 
-    def _task_api_response(self, task: Task) -> TaskResponse:
-        steps: List[dict] = []
-        for step in task.steps:
-            steps.append({"id": step.id, "type": step.__class__.__name__})
-
-        return TaskResponse(id=task.id, tag_id=task.tag_id, steps=steps)
+    def _task_api_response(self, task: TASKS) -> TaskResponse:
+        return TaskResponse(id=task.id, tag_id=task.tag_id, type=task.type)
diff --git a/src/isar/config/predefined_missions/default.json b/src/isar/config/predefined_missions/default.json
index 626db514..fd1dd033 100644
--- a/src/isar/config/predefined_missions/default.json
+++ b/src/isar/config/predefined_missions/default.json
@@ -1,92 +1,74 @@
 {
-    "id": "1",
-    "tasks": [
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": -2,
-                            "y": -2,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.4794255,
-                            "w": 0.8775826,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_image",
-                    "target": {
-                        "x": 2,
-                        "y": 2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+  "id": "1",
+  "tasks": [
+    {
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": -2,
+          "y": -2,
+          "z": 0,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": -2,
-                            "y": 2,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.4794255,
-                            "w": 0.8775826,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_thermal_image",
-                    "target": {
-                        "x": 2,
-                        "y": 2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 2,
-                            "y": 2,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.4794255,
-                            "w": 0.8775826,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                }
-            ]
-        }
-    ]
+        "frame": "asset"
+      },
+      "target": {
+        "x": 2,
+        "y": 2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "take_thermal_image",
+      "robot_pose": {
+        "position": {
+          "x": -2,
+          "y": 2,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 2,
+        "y": 2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "return_to_home",
+      "pose": {
+        "position": {
+          "x": 2,
+          "y": 2,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      }
+    }
+  ]
 }
diff --git a/src/isar/config/predefined_missions/default_turtlebot.json b/src/isar/config/predefined_missions/default_turtlebot.json
index e6bdd25b..080d8f98 100644
--- a/src/isar/config/predefined_missions/default_turtlebot.json
+++ b/src/isar/config/predefined_missions/default_turtlebot.json
@@ -1,110 +1,106 @@
 {
-    "id": "2",
-    "tasks": [
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": -3.6,
-                            "y": 4,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": -0.7286672256879113,
-                            "w": -0.6848660759820616,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_image",
-                    "target": {
-                        "x": -4.7,
-                        "y": 4.9,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+  "id": "2",
+  "tasks": [
+    {
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": -3.6,
+          "y": 4,
+          "z": 0,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 4.7,
-                            "y": 3,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.5769585,
-                            "w": 0.8167734,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_image",
-                    "target": {
-                        "x": 5.6,
-                        "y": 5.2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                },
-                {
-                    "type": "take_thermal_image",
-                    "target": {
-                        "x": 3.1,
-                        "y": 5.2,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": -0.7286672256879113,
+          "w": -0.6848660759820616,
+          "frame": "asset"
         },
-        {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 0.95,
-                            "y": 2.6,
-                            "z": 0,
-                            "frame": "asset"
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": -0.6992469,
-                            "w": 0.7148802,
-                            "frame": "asset"
-                        },
-                        "frame": "asset"
-                    }
-                },
-                {
-                    "type": "take_thermal_image",
-                    "target": {
-                        "x": 1.9,
-                        "y": 1.9,
-                        "z": 0,
-                        "frame": "robot"
-                    }
-                }
-            ]
-        }
-    ]
+        "frame": "asset"
+      },
+      "target": {
+        "x": -4.7,
+        "y": 4.9,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+
+    {
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": 4.7,
+          "y": 3,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.5769585,
+          "w": 0.8167734,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 5.6,
+        "y": 5.2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "take_thermal_image",
+      "robot_pose": {
+        "position": {
+          "x": 4.7,
+          "y": 3,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.5769585,
+          "w": 0.8167734,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 3.1,
+        "y": 5.2,
+        "z": 0,
+        "frame": "robot"
+      }
+    },
+    {
+      "type": "take_thermal_image",
+      "robot_pose": {
+        "position": {
+          "x": 0.95,
+          "y": 2.6,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": -0.6992469,
+          "w": 0.7148802,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 1.9,
+        "y": 1.9,
+        "z": 0,
+        "frame": "robot"
+      }
+    }
+  ]
 }
diff --git a/src/isar/config/settings.env b/src/isar/config/settings.env
index 0bcfcf5a..2dd837a3 100644
--- a/src/isar/config/settings.env
+++ b/src/isar/config/settings.env
@@ -1,6 +1,6 @@
 ISAR_ROBOT_PACKAGE = isar_robot
 
-ISAR_RUN_MISSION_STEPWISE = true
+ISAR_RUN_MISSION_BY_TASK = true
 
 ISAR_STORAGE_LOCAL_ENABLED = true
 ISAR_STORAGE_BLOB_ENABLED = false
diff --git a/src/isar/config/settings.py b/src/isar/config/settings.py
index 77009601..bcc94c4f 100644
--- a/src/isar/config/settings.py
+++ b/src/isar/config/settings.py
@@ -26,7 +26,7 @@ def __init__(self) -> None:
     ROBOT_PACKAGE: str = Field(default="isar_robot")
 
     # The run mode of the robot (stepwise or full mission)
-    RUN_MISSION_STEPWISE: bool = Field(default=True)
+    RUN_MISSION_BY_TASK: bool = Field(default=True)
 
     # Determines the local path in which results from missions are stored
     LOCAL_STORAGE_PATH: str = Field(default="./results")
@@ -327,7 +327,7 @@ def __init__(self) -> None:
 
     # ISAR steps the robot is capable of performing
     # This should be set in the robot package settings.env file
-    CAPABILITIES: List[str] = Field(default=["drive_to_pose", "take_image"])
+    CAPABILITIES: List[str] = Field(default=["return_to_home", "take_image"])
 
     # Model of the robot which ISAR is connected to
     # This should be set in the robot package settings.env file
@@ -345,6 +345,6 @@ def __init__(self) -> None:
 
 robot_settings = RobotSettings()
 
-if not settings.RUN_MISSION_STEPWISE:  # If mission-wise, do not run localize missions
+if not settings.RUN_MISSION_BY_TASK:  # If mission-wise, do not run localize missions
     if "localize" in robot_settings.CAPABILITIES:
         robot_settings.CAPABILITIES.remove("localize")
diff --git a/src/isar/mission_planner/local_planner.py b/src/isar/mission_planner/local_planner.py
index 2b1c80a0..7af96df0 100644
--- a/src/isar/mission_planner/local_planner.py
+++ b/src/isar/mission_planner/local_planner.py
@@ -1,5 +1,6 @@
 import logging
 from pathlib import Path
+from typing import List, Optional
 
 from alitra import Frame
 from injector import inject
@@ -13,6 +14,7 @@
 from isar.services.readers.base_reader import BaseReader, BaseReaderError
 from robot_interface.models.mission.mission import Mission
 
+
 logger = logging.getLogger("api")
 
 
@@ -38,6 +40,7 @@ def get_mission(self, mission_id) -> Mission:
     @staticmethod
     def read_mission_from_file(mission_path: Path) -> Mission:
         mission_dict: dict = BaseReader.read_json(location=mission_path)
+
         mission: Mission = BaseReader.dict_to_dataclass(
             dataclass_dict=mission_dict,
             target_dataclass=Mission,
diff --git a/src/isar/mission_planner/sequential_task_selector.py b/src/isar/mission_planner/sequential_task_selector.py
index c563eb64..b6041e8c 100644
--- a/src/isar/mission_planner/sequential_task_selector.py
+++ b/src/isar/mission_planner/sequential_task_selector.py
@@ -4,7 +4,7 @@
     TaskSelectorInterface,
     TaskSelectorStop,
 )
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 
 
 class SequentialTaskSelector(TaskSelectorInterface):
@@ -12,11 +12,11 @@ def __init__(self) -> None:
         super().__init__()
         self._iterator: Iterator = None
 
-    def initialize(self, tasks: List[Task]) -> None:
+    def initialize(self, tasks: List[TASKS]) -> None:
         super().initialize(tasks=tasks)
         self._iterator = iter(self.tasks)
 
-    def next_task(self) -> Task:
+    def next_task(self) -> TASKS:
         try:
             return next(self._iterator)
         except StopIteration:
diff --git a/src/isar/mission_planner/task_selector_interface.py b/src/isar/mission_planner/task_selector_interface.py
index d3d34f0e..15c075c2 100644
--- a/src/isar/mission_planner/task_selector_interface.py
+++ b/src/isar/mission_planner/task_selector_interface.py
@@ -1,18 +1,18 @@
 from abc import ABCMeta, abstractmethod
 from typing import List
 
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 
 
 class TaskSelectorInterface(metaclass=ABCMeta):
     def __init__(self) -> None:
-        self.tasks: List[Task] = None
+        self.tasks: List[TASKS] = None
 
-    def initialize(self, tasks: List[Task]) -> None:
+    def initialize(self, tasks: List[TASKS]) -> None:
         self.tasks = tasks
 
     @abstractmethod
-    def next_task(self) -> Task:
+    def next_task(self) -> TASKS:
         """
         Returns
         -------
diff --git a/src/isar/script.py b/src/isar/script.py
index 35495dab..e3988ee1 100644
--- a/src/isar/script.py
+++ b/src/isar/script.py
@@ -63,7 +63,7 @@ def print_setting(setting: str = "", value: Any = "", fillchar: str = " "):
     print_setting(fillchar="-")
     print_setting("Robot package", settings.ROBOT_PACKAGE)
     print_setting("Robot name", settings.ROBOT_NAME)
-    print_setting("Run mission stepwise", settings.RUN_MISSION_STEPWISE)
+    print_setting("Run mission stepwise", settings.RUN_MISSION_BY_TASK)
     print_setting("Running on port", settings.API_PORT)
     print_setting("Mission planner", settings.MISSION_PLANNER)
     print_setting("Using local storage", settings.STORAGE_LOCAL_ENABLED)
diff --git a/src/isar/services/service_connections/mqtt/mqtt_client.py b/src/isar/services/service_connections/mqtt/mqtt_client.py
index 39b53738..81aa808c 100644
--- a/src/isar/services/service_connections/mqtt/mqtt_client.py
+++ b/src/isar/services/service_connections/mqtt/mqtt_client.py
@@ -99,7 +99,7 @@ def on_disconnect(self, client, userdata, rc):
     )
     def connect(self, host: str, port: int) -> None:
         self.logger.info("Attempting to connect to MQTT Broker")
-        self.logger.debug(f"Host: {host}, Port: {port}")
+        self.logger.info(f"Host: {host}, Port: {port}")
         self.client.connect(host=host, port=port)
 
     def publish(self, topic: str, payload: str, qos: int = 0, retain: bool = False):
diff --git a/src/isar/services/utilities/scheduling_utilities.py b/src/isar/services/utilities/scheduling_utilities.py
index cab67ef7..53d9dce2 100644
--- a/src/isar/services/utilities/scheduling_utilities.py
+++ b/src/isar/services/utilities/scheduling_utilities.py
@@ -101,10 +101,9 @@ def verify_robot_capable_of_mission(
         is_capable: bool = True
         missing_capabilities: Set[str] = set()
         for task in mission.tasks:
-            for step in task.steps:
-                if not step.type in robot_capabilities:
-                    is_capable = False
-                    missing_capabilities.add(step.type)
+            if not task.type in robot_capabilities:
+                is_capable = False
+                missing_capabilities.add(task.type)
 
         if not is_capable:
             error_message = (
diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py
index 4b7bd0f4..0f53b0bf 100644
--- a/src/isar/state_machine/state_machine.py
+++ b/src/isar/state_machine/state_machine.py
@@ -35,11 +35,9 @@
 from robot_interface.models.mission.status import (
     MissionStatus,
     RobotStatus,
-    StepStatus,
     TaskStatus,
 )
-from robot_interface.models.mission.step import Step
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 from robot_interface.robot_interface import RobotInterface
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
@@ -56,7 +54,7 @@ def __init__(
         mqtt_publisher: MqttClientInterface,
         task_selector: TaskSelectorInterface,
         sleep_time: float = settings.FSM_SLEEP_TIME,
-        stepwise_mission: bool = settings.RUN_MISSION_STEPWISE,
+        run_mission_by_task: bool = settings.RUN_MISSION_BY_TASK,
         stop_robot_attempts_limit: int = settings.STOP_ROBOT_ATTEMPTS_LIMIT,
         transitions_log_length: int = settings.STATE_TRANSITIONS_LOG_LENGTH,
     ):
@@ -84,7 +82,7 @@ def __init__(
         self.robot: RobotInterface = robot
         self.mqtt_publisher: Optional[MqttClientInterface] = mqtt_publisher
         self.task_selector: TaskSelectorInterface = task_selector
-        self.stepwise_mission: bool = stepwise_mission
+        self.run_mission_by_task: bool = run_mission_by_task
 
         # List of states
         self.stop_state: State = Stop(self)
@@ -179,10 +177,10 @@ def __init__(
                     "before": self._resume,
                 },
                 {
-                    "trigger": "step_finished",
+                    "trigger": "task_finished",
                     "source": self.monitor_state,
                     "dest": self.initiate_state,
-                    "before": self._step_finished,
+                    "before": self._task_finished,
                 },
                 {
                     "trigger": "full_mission_finished",
@@ -234,8 +232,7 @@ def __init__(
 
         self.stopped: bool = False
         self.current_mission: Optional[Mission] = None
-        self.current_task: Optional[Task] = None
-        self.current_step: Optional[Step] = None
+        self.current_task: Optional[TASKS] = None
         self.initial_pose: Optional[Pose] = None
 
         self.current_state: State = States(self.state)  # type: ignore
@@ -255,14 +252,14 @@ def _initialization_failed(self) -> None:
         self._finalize()
 
     def _initiated(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.InProgress
+        if self.run_mission_by_task:
+            self.current_task.status = TaskStatus.InProgress
         self.current_mission.status = MissionStatus.InProgress
-        self.publish_step_status(step=self.current_step)
+        self.publish_task_status(task=self.current_task)
         self.logger.info(
             f"Successfully initiated "
-            f"{type(self.current_step).__name__} "
-            f"step: {str(self.current_step.id)[:8]}"
+            f"{type(self.current_task).__name__} "
+            f"task: {str(self.current_task.id)[:8]}"
         )
 
     def _pause(self) -> None:
@@ -291,9 +288,6 @@ def _resume(self) -> None:
         )
         self.queues.resume_mission.output.put(resume_mission_response)
 
-        self.current_task.reset_task()
-        self.iterate_current_step()
-
         self.robot.resume()
 
     def _mission_finished(self) -> None:
@@ -327,7 +321,7 @@ def _mission_started(self) -> None:
             f"Initialization successful. Starting new mission: "
             f"{self.current_mission.id}"
         )
-        self.log_step_overview(mission=self.current_mission)
+        self.log_mission_overview(mission=self.current_mission)
 
         self.current_mission.status = MissionStatus.InProgress
         self.publish_mission_status()
@@ -337,14 +331,11 @@ def _mission_started(self) -> None:
         else:
             self.current_task.status = TaskStatus.InProgress
             self.publish_task_status(task=self.current_task)
-            self.iterate_current_step()
 
-    def _step_finished(self) -> None:
-        self.publish_step_status(step=self.current_step)
-        self.current_task.update_task_status()
+    def _task_finished(self) -> None:
         self.publish_task_status(task=self.current_task)
+        self.current_task.update_task_status()
         self.iterate_current_task()
-        self.iterate_current_step()
 
     def _full_mission_finished(self) -> None:
         self.current_task = None
@@ -353,7 +344,6 @@ def _mission_paused(self) -> None:
         self.logger.info(f"Pausing mission: {self.current_mission.id}")
         self.current_mission.status = MissionStatus.Paused
         self.current_task.status = TaskStatus.Paused
-        self.current_step.status = StepStatus.NotStarted
 
         paused_mission_response: ControlMissionResponse = (
             self._make_control_mission_response()
@@ -362,7 +352,6 @@ def _mission_paused(self) -> None:
 
         self.publish_mission_status()
         self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
 
         self.robot.pause()
 
@@ -370,29 +359,21 @@ def _stop(self) -> None:
         self.stopped = True
 
     def _initiate_failed(self) -> None:
-        self.current_step.status = StepStatus.Failed
-        self.current_task.update_task_status()
+        self.current_task.status = TaskStatus.Failed
         self.current_mission.status = MissionStatus.Failed
-        self.publish_step_status(step=self.current_step)
         self.publish_task_status(task=self.current_task)
         self._finalize()
 
     def _initiate_infeasible(self) -> None:
-        if self.stepwise_mission:
-            self.current_step.status = StepStatus.Failed
-            self.publish_step_status(step=self.current_step)
-            self.current_task.update_task_status()
+        if self.run_mission_by_task:
+            self.current_task.status = TaskStatus.Failed
             self.publish_task_status(task=self.current_task)
             self.iterate_current_task()
-            self.iterate_current_step()
 
     def _mission_stopped(self) -> None:
         self.current_mission.status = MissionStatus.Cancelled
 
         for task in self.current_mission.tasks:
-            for step in task.steps:
-                if step.status in [StepStatus.NotStarted, StepStatus.InProgress]:
-                    step.status = StepStatus.Cancelled
             if task.status in [
                 TaskStatus.NotStarted,
                 TaskStatus.InProgress,
@@ -406,14 +387,13 @@ def _mission_stopped(self) -> None:
         self.queues.stop_mission.output.put(stopped_mission_response)
 
         self.publish_task_status(task=self.current_task)
-        self.publish_step_status(step=self.current_step)
         self._finalize()
 
     #################################################################################
 
     def _finalize(self) -> None:
         self.publish_mission_status()
-        self.log_step_overview(mission=self.current_mission)
+        self.log_mission_overview(mission=self.current_mission)
         state_transitions: str = ", ".join(
             [
                 f"\n  {transition}" if (i + 1) % 10 == 0 else f"{transition}"
@@ -441,20 +421,6 @@ def iterate_current_task(self):
                 # Indicates that all tasks are finished
                 self.current_task = None
 
-    def iterate_current_step(self):
-        if self.current_task != None:
-            self.current_step = self.current_task.next_step()
-
-    def update_remaining_steps(self):
-        if self.current_task:
-            for step in self.current_task.steps:
-                if (
-                    step.status == StepStatus.InProgress
-                    or step.status == StepStatus.NotStarted
-                ):
-                    step.status = self.current_task.status
-                    self.publish_step_status(step=step)
-
     def update_state(self):
         """Updates the current state of the state machine."""
         self.current_state = States(self.state)
@@ -466,7 +432,6 @@ def update_state(self):
     def reset_state_machine(self) -> None:
         self.logger.info("Resetting state machine")
         self.stopped = False
-        self.current_step = None
         self.current_task = None
         self.current_mission = None
         self.initial_pose = None
@@ -538,7 +503,7 @@ def publish_mission_status(self) -> None:
             retain=True,
         )
 
-    def publish_task_status(self, task: Task) -> None:
+    def publish_task_status(self, task: TASKS) -> None:
         """Publishes the task status to the MQTT Broker"""
         if not self.mqtt_publisher:
             return
@@ -555,6 +520,7 @@ def publish_task_status(self, task: Task) -> None:
                 "mission_id": self.current_mission.id if self.current_mission else None,
                 "task_id": task.id if task else None,
                 "status": task.status if task else None,
+                "task_type": task.type,
                 "error_reason": error_message.error_reason if error_message else None,
                 "error_description": (
                     error_message.error_description if error_message else None
@@ -571,41 +537,6 @@ def publish_task_status(self, task: Task) -> None:
             retain=True,
         )
 
-    def publish_step_status(self, step: Step) -> None:
-        """Publishes the step status to the MQTT Broker"""
-        if not self.mqtt_publisher:
-            return
-
-        error_message: Optional[ErrorMessage] = None
-        if step:
-            if step.error_message:
-                error_message = step.error_message
-
-        payload: str = json.dumps(
-            {
-                "isar_id": settings.ISAR_ID,
-                "robot_name": settings.ROBOT_NAME,
-                "mission_id": self.current_mission.id if self.current_mission else None,
-                "task_id": self.current_task.id if self.current_task else None,
-                "step_id": step.id if step else None,
-                "step_type": step.__class__.__name__ if step else None,
-                "status": step.status if step else None,
-                "error_reason": error_message.error_reason if error_message else None,
-                "error_description": (
-                    error_message.error_description if error_message else None
-                ),
-                "timestamp": datetime.now(timezone.utc),
-            },
-            cls=EnhancedJSONEncoder,
-        )
-
-        self.mqtt_publisher.publish(
-            topic=settings.TOPIC_ISAR_STEP,
-            payload=payload,
-            qos=1,
-            retain=True,
-        )
-
     def publish_status(self) -> None:
         if not self.mqtt_publisher:
             return
@@ -638,18 +569,13 @@ def _log_state_transition(self, next_state):
         """Logs all state transitions that are not self-transitions."""
         self.transitions_list.append(next_state)
 
-    def log_step_overview(self, mission: Mission):
-        """Log an overview of the steps in a mission"""
+    def log_mission_overview(self, mission: Mission):
+        """Log an overview of the tasks in a mission"""
         log_statements: List[str] = []
         for task in mission.tasks:
             log_statements.append(
                 f"{type(task).__name__:<20} {str(task.id)[:8]:<32} -- {task.status}"
             )
-            for j, step in enumerate(task.steps):
-                log_statements.append(
-                    f"{j:>3} {type(step).__name__:<20} {str(step.id)[:8]:<32} -- {step.status}"  # noqa: E501
-                )
-
         log_statement: str = "\n".join(log_statements)
 
         self.logger.info(f"Mission overview:\n{log_statement}")
@@ -660,8 +586,6 @@ def _make_control_mission_response(self) -> ControlMissionResponse:
             mission_status=self.current_mission.status,
             task_id=self.current_task.id,
             task_status=self.current_task.status,
-            step_id=self.current_step.id,
-            step_status=self.current_step.status,
         )
 
 
diff --git a/src/isar/state_machine/states/initialize.py b/src/isar/state_machine/states/initialize.py
index 0996be03..e719484a 100644
--- a/src/isar/state_machine/states/initialize.py
+++ b/src/isar/state_machine/states/initialize.py
@@ -57,7 +57,7 @@ def _run(self) -> None:
                 continue
 
             except (RobotInitializeException, RobotException) as e:
-                self.state_machine.current_step.error_message = ErrorMessage(
+                self.state_machine.current_task.error_message = ErrorMessage(
                     error_reason=e.error_reason, error_description=e.error_description
                 )
                 self.logger.error(
diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py
index f9546c6e..243e76e8 100644
--- a/src/isar/state_machine/states/initiate.py
+++ b/src/isar/state_machine/states/initiate.py
@@ -13,7 +13,7 @@
     ErrorMessage,
     RobotException,
     RobotInfeasibleMissionException,
-    RobotInfeasibleStepException,
+    RobotInfeasibleTaskException,
 )
 
 if TYPE_CHECKING:
@@ -61,11 +61,11 @@ def _run(self) -> None:
                 break
 
             if not self.initiate_thread:
-                if self.state_machine.stepwise_mission:
+                if self.state_machine.run_mission_by_task:
                     self._run_initiate_thread(
-                        initiate_function=self.state_machine.robot.initiate_step,
-                        function_argument=self.state_machine.current_step,
-                        thread_name="State Machine Initiate Step",
+                        initiate_function=self.state_machine.robot.initiate_task,
+                        function_argument=self.state_machine.current_task,
+                        thread_name="State Machine Initiate Task",
                     )
                 else:
                     self._run_initiate_thread(
@@ -81,13 +81,13 @@ def _run(self) -> None:
             except ThreadedRequestNotFinishedError:
                 time.sleep(self.state_machine.sleep_time)
                 continue
-            except RobotInfeasibleStepException as e:
-                self.state_machine.current_step.error_message = ErrorMessage(
+            except RobotInfeasibleTaskException as e:
+                self.state_machine.current_task.error_message = ErrorMessage(
                     error_reason=e.error_reason, error_description=e.error_description
                 )
                 self.logger.warning(
-                    f"Failed to initiate step "
-                    f"{str(self.state_machine.current_step.id)[:8]} after retrying "
+                    f"Failed to initiate task "
+                    f"{str(self.state_machine.current_task.id)[:8]} after retrying "
                     f"{self.initiate_failure_counter} times because: "
                     f"{e.error_description}"
                 )
@@ -115,7 +115,7 @@ def _run(self) -> None:
                 )
 
                 if self.initiate_failure_counter >= self.initiate_failure_counter_limit:
-                    self.state_machine.current_step.error_message = ErrorMessage(
+                    self.state_machine.current_task.error_message = ErrorMessage(
                         error_reason=e.error_reason,
                         error_description=e.error_description,
                     )
diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py
index cbaa7234..d182dbae 100644
--- a/src/isar/state_machine/states/monitor.py
+++ b/src/isar/state_machine/states/monitor.py
@@ -7,7 +7,6 @@
 from transitions import State
 
 from isar.config.settings import settings
-from isar.mission_planner.task_selector_interface import TaskSelectorStop
 from isar.services.utilities.threaded_request import (
     ThreadedRequest,
     ThreadedRequestNotFinishedError,
@@ -16,14 +15,16 @@
     ErrorMessage,
     RobotCommunicationTimeoutException,
     RobotException,
-    RobotMissionStatusException,
     RobotRetrieveInspectionException,
     RobotStepStatusException,
 )
 from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus, TaskStatus
-from robot_interface.models.mission.step import InspectionStep, Step, StepStatus
+from robot_interface.models.mission.status import TaskStatus
+from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import (
+    InspectionTask,
+)
 
 if TYPE_CHECKING:
     from isar.state_machine.state_machine import StateMachine
@@ -40,16 +41,16 @@ def __init__(self, state_machine: "StateMachine") -> None:
         )
 
         self.logger = logging.getLogger("state_machine")
-        self.step_status_thread: Optional[ThreadedRequest] = None
+        self.task_status_thread: Optional[ThreadedRequest] = None
 
     def start(self) -> None:
         self.state_machine.update_state()
         self._run()
 
     def stop(self) -> None:
-        if self.step_status_thread:
-            self.step_status_thread.wait_for_thread()
-        self.step_status_thread = None
+        if self.task_status_thread:
+            self.task_status_thread.wait_for_thread()
+        self.task_status_thread = None
 
     def _run(self) -> None:
         transition: Callable
@@ -59,121 +60,110 @@ def _run(self) -> None:
                 break
 
             if self.state_machine.should_pause_mission():
-                if self.state_machine.stepwise_mission:
+                if self.state_machine.run_mission_by_task:
                     transition = self.state_machine.pause  # type: ignore
                 else:
                     transition = self.state_machine.pause_full_mission  # type: ignore
                 break
 
-            if not self.step_status_thread:
+            if not self.task_status_thread:
                 self._run_get_status_thread(
-                    status_function=self.state_machine.robot.step_status,
+                    status_function=self.state_machine.robot.task_status,
+                    function_argument=self.state_machine.current_task.id,
                     thread_name="State Machine Monitor Get Step Status",
                 )
             try:
-                status: StepStatus = self.step_status_thread.get_output()
+                status: TaskStatus = self.task_status_thread.get_output()
             except ThreadedRequestNotFinishedError:
                 time.sleep(self.state_machine.sleep_time)
                 continue
 
             except RobotCommunicationTimeoutException as e:
-                step_failed: bool = self._handle_communication_timeout(e)
-                if step_failed:
-                    status = StepStatus.Failed
+                task_failed: bool = self._handle_communication_timeout(e)
+                if task_failed:
+                    status = TaskStatus.Failed
                 else:
                     continue
 
             except RobotStepStatusException as e:
-                self.state_machine.current_step.error_message = ErrorMessage(
+                self.state_machine.current_task.error_message = ErrorMessage(
                     error_reason=e.error_reason, error_description=e.error_description
                 )
                 self.logger.error(
-                    f"Monitoring step {self.state_machine.current_step.id[:8]} failed "
+                    f"Monitoring task {self.state_machine.current_task.id[:8]} failed "
                     f"because: {e.error_description}"
                 )
-                status = StepStatus.Failed
+                status = TaskStatus.Failed
 
             except RobotException as e:
                 self._set_error_message(e)
-                status = StepStatus.Failed
+                status = TaskStatus.Failed
 
                 self.logger.error(
                     f"Retrieving the status failed because: {e.error_description}"
                 )
 
-            if not isinstance(status, StepStatus):
+            if not isinstance(status, TaskStatus):
                 self.logger.error(
-                    f"Received an invalid status update when monitoring mission. Only StepStatus is expected."
+                    f"Received an invalid status update when monitoring mission. Only TaskStatus is expected."
                 )
                 break
 
             if self.state_machine.current_task == None:
                 self.state_machine.iterate_current_task()
-            if self.state_machine.current_step == None:
-                self.state_machine.iterate_current_step()
 
-            self.state_machine.current_step.status = status
+            self.state_machine.current_task.status = status
 
             if self._should_upload_inspections():
-                get_inspections_thread = ThreadedRequest(
+                get_inspection_thread = ThreadedRequest(
                     self._queue_inspections_for_upload
                 )
-                get_inspections_thread.start_thread(
+                get_inspection_thread.start_thread(
                     deepcopy(self.state_machine.current_mission),
-                    deepcopy(self.state_machine.current_step),
+                    deepcopy(self.state_machine.current_task),
                     name="State Machine Get Inspections",
                 )
 
-            if self.state_machine.stepwise_mission:
-                if self._is_step_finished(self.state_machine.current_step):
-                    self._report_step_status(self.state_machine.current_step)
-                    transition = self.state_machine.step_finished  # type: ignore
+            if self.state_machine.run_mission_by_task:
+                if self.state_machine.current_task.is_finished():
+                    self._report_task_status(self.state_machine.current_task)
+                    transition = self.state_machine.task_finished  # type: ignore
                     break
             else:
-                if self._is_step_finished(self.state_machine.current_step):
-                    self._report_step_status(self.state_machine.current_step)
-
-                    if self.state_machine.current_task.is_finished():
-                        # Report and update finished task
-                        self.state_machine.current_task.update_task_status()  # Uses the updated step status to set the task status
-                        self.state_machine.publish_task_status(
-                            task=self.state_machine.current_task
-                        )
-
-                        self.state_machine.iterate_current_task()
-
-                        if self.state_machine.current_task == None:
-                            transition = self.state_machine.full_mission_finished  # type: ignore
-                            break
-
-                        # Report and update next task
-                        self.state_machine.current_task.update_task_status()
-                        self.state_machine.publish_task_status(
-                            task=self.state_machine.current_task
-                        )
-
-                    self.state_machine.iterate_current_step()
-
-                else:  # If not all steps are done
-                    self.state_machine.current_task.status = TaskStatus.InProgress
-
-            self.step_status_thread = None
+                if self.state_machine.current_task.is_finished():
+                    self._report_task_status(self.state_machine.current_task)
+                    self.state_machine.publish_task_status(
+                        task=self.state_machine.current_task
+                    )
+
+                    self.state_machine.iterate_current_task()
+                    if self.state_machine.current_task == None:
+                        transition = self.state_machine.full_mission_finished  # type: ignore
+                        break
+
+                    # Report and update next task
+                    self.state_machine.current_task.update_task_status()
+                    self.state_machine.publish_task_status(
+                        task=self.state_machine.current_task
+                    )
+
+            self.task_status_thread = None
             time.sleep(self.state_machine.sleep_time)
 
         transition()
 
     def _run_get_status_thread(
-        self, status_function: Callable, thread_name: str
+        self, status_function: Callable, function_argument: str, thread_name: str
     ) -> None:
-        self.step_status_thread = ThreadedRequest(request_func=status_function)
-        self.step_status_thread.start_thread(name=thread_name)
+        self.task_status_thread = ThreadedRequest(request_func=status_function)
+        self.task_status_thread.start_thread(function_argument, name=thread_name)
 
     def _queue_inspections_for_upload(
-        self, mission: Mission, current_step: InspectionStep
+        self, mission: Mission, current_task: InspectionTask
     ) -> None:
         try:
-            inspections: Sequence[Inspection] = (
-                self.state_machine.robot.get_inspections(step=current_step)
+            inspection: Inspection = self.state_machine.robot.get_inspection(
+                task=current_task
             )
 
         except (RobotRetrieveInspectionException, RobotException) as e:
@@ -183,52 +173,42 @@ def _queue_inspections_for_upload(
             )
             return
 
-        if not inspections:
+        if not inspection:
             self.logger.warning(
-                f"No inspection data retrieved for step {str(current_step.id)[:8]}"
+                f"No inspection data retrieved for task {str(current_task.id)[:8]}"
             )
 
-        for inspection in inspections:
-            inspection.metadata.tag_id = current_step.tag_id
+        inspection.metadata.tag_id = current_task.tag_id
 
-            message: Tuple[Inspection, Mission] = (
-                inspection,
-                mission,
-            )
-            self.state_machine.queues.upload_queue.put(message)
-            self.logger.info(f"Inspection: {str(inspection.id)[:8]} queued for upload")
-
-    def _is_step_finished(self, step: Step) -> bool:
-        finished: bool = False
-        if step.status == StepStatus.Failed:
-            finished = True
-        elif step.status == StepStatus.Successful:
-            finished = True
-        return finished
-
-    def _report_step_status(self, step: Step) -> None:
-        if step.status == StepStatus.Failed:
+        message: Tuple[Inspection, Mission] = (
+            inspection,
+            mission,
+        )
+        self.state_machine.queues.upload_queue.put(message)
+        self.logger.info(f"Inspection: {str(inspection.id)[:8]} queued for upload")
+
+    def _report_task_status(self, task: Task) -> None:
+        if task.status == TaskStatus.Failed:
             self.logger.warning(
-                f"Step: {str(step.id)[:8]} was reported as failed by the robot"
+                f"Task: {str(task.id)[:8]} was reported as failed by the robot"
             )
-        elif step.status == StepStatus.Successful:
+        elif task.status == TaskStatus.Successful:
             self.logger.info(
-                f"{type(step).__name__} step: {str(step.id)[:8]} completed"
+                f"{type(task).__name__} task: {str(task.id)[:8]} completed"
             )
 
     def _should_upload_inspections(self) -> bool:
-        step: Step = self.state_machine.current_step
         return (
-            self._is_step_finished(step)
-            and step.status == StepStatus.Successful
-            and isinstance(step, InspectionStep)
+            self.state_machine.current_task.is_finished()
+            and self.state_machine.current_task.status == TaskStatus.Successful
+            and isinstance(self.state_machine.current_task, InspectionTask)
         )
 
     def _set_error_message(self, e: RobotException) -> None:
         error_message: ErrorMessage = ErrorMessage(
             error_reason=e.error_reason, error_description=e.error_description
         )
-        self.state_machine.current_step.error_message = error_message
+        self.state_machine.current_task.error_message = error_message
 
     def _handle_communication_timeout(
         self, e: RobotCommunicationTimeoutException
@@ -236,10 +216,10 @@ def _handle_communication_timeout(
         self.state_machine.current_mission.error_message = ErrorMessage(
             error_reason=e.error_reason, error_description=e.error_description
         )
-        self.step_status_thread = None
+        self.task_status_thread = None
         self.request_status_failure_counter += 1
         self.logger.warning(
-            f"Monitoring step {self.state_machine.current_step.id} failed #: "
+            f"Monitoring task {self.state_machine.current_task.id} failed #: "
             f"{self.request_status_failure_counter} failed because: {e.error_description}"
         )
 
@@ -247,12 +227,12 @@ def _handle_communication_timeout(
             self.request_status_failure_counter
             >= self.request_status_failure_counter_limit
         ):
-            self.state_machine.current_step.error_message = ErrorMessage(
+            self.state_machine.current_task.error_message = ErrorMessage(
                 error_reason=e.error_reason,
                 error_description=e.error_description,
             )
             self.logger.error(
-                f"Step will be cancelled after failing to get step status "
+                f"Step will be cancelled after failing to get task status "
                 f"{self.request_status_failure_counter} times because: "
                 f"{e.error_description}"
             )
diff --git a/src/isar/state_machine/states/paused.py b/src/isar/state_machine/states/paused.py
index f887e0b6..14b55e78 100644
--- a/src/isar/state_machine/states/paused.py
+++ b/src/isar/state_machine/states/paused.py
@@ -26,7 +26,7 @@ def _run(self) -> None:
                 break
 
             if self.state_machine.should_resume_mission():
-                if self.state_machine.stepwise_mission:
+                if self.state_machine.run_mission_by_task:
                     transition = self.state_machine.resume  # type: ignore
                 else:
                     transition = self.state_machine.resume_full_mission  # type: ignore
diff --git a/src/isar/state_machine/states/stop.py b/src/isar/state_machine/states/stop.py
index 3ac4515b..36c0ad3c 100644
--- a/src/isar/state_machine/states/stop.py
+++ b/src/isar/state_machine/states/stop.py
@@ -81,7 +81,7 @@ def _run(self) -> None:
     def handle_stop_fail(self, retry_limit: int, error_message: ErrorMessage) -> bool:
         self._count_number_retries += 1
         if self._count_number_retries > retry_limit:
-            self.state_machine.current_step.error_message = error_message
+            self.state_machine.current_task.error_message = error_message
 
             self.logger.error(
                 f"\nFailed to stop the robot after {retry_limit} attempts because: "
diff --git a/src/robot_interface/models/exceptions/robot_exceptions.py b/src/robot_interface/models/exceptions/robot_exceptions.py
index c07d8d0d..dea21cf5 100644
--- a/src/robot_interface/models/exceptions/robot_exceptions.py
+++ b/src/robot_interface/models/exceptions/robot_exceptions.py
@@ -6,7 +6,7 @@
 class ErrorReason(str, Enum):
     RobotCommunicationException: str = "robot_communication_exception"
     RobotCommunicationTimeoutException: str = "robot_communication_timeout_exception"
-    RobotInfeasibleStepException: str = "robot_infeasible_step_exception"
+    RobotInfeasibleTaskException: str = "robot_infeasible_step_exception"
     RobotInfeasibleMissionException: str = "robot_infeasible_mission_exception"
     RobotMissionStatusException: str = "robot_mission_status_exception"
     RobotStepStatusException: str = "robot_step_status_exception"
@@ -67,10 +67,10 @@ def __init__(self, error_description: str) -> None:
 
 # An exception which should be thrown by the robot package if it is unable to start the
 # current step.
-class RobotInfeasibleStepException(RobotException):
+class RobotInfeasibleTaskException(RobotException):
     def __init__(self, error_description: str) -> None:
         super().__init__(
-            error_reason=ErrorReason.RobotInfeasibleStepException,
+            error_reason=ErrorReason.RobotInfeasibleTaskException,
             error_description=error_description,
         )
 
diff --git a/src/robot_interface/models/inspection/inspection.py b/src/robot_interface/models/inspection/inspection.py
index 5a45e345..9ada0372 100644
--- a/src/robot_interface/models/inspection/inspection.py
+++ b/src/robot_interface/models/inspection/inspection.py
@@ -45,8 +45,8 @@ class AudioMetadata(InspectionMetadata):
 
 @dataclass
 class Inspection:
-    id: str = field(default_factory=uuid4_string, init=False)
     metadata: InspectionMetadata
+    id: str = field(default_factory=uuid4_string, init=True)
     data: Optional[bytes] = field(default=None, init=False)
 
     @staticmethod
diff --git a/src/robot_interface/models/mission/mission.py b/src/robot_interface/models/mission/mission.py
index f985d252..4fb03a49 100644
--- a/src/robot_interface/models/mission/mission.py
+++ b/src/robot_interface/models/mission/mission.py
@@ -5,13 +5,13 @@
 
 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
+from robot_interface.models.mission.task import TASKS
 from robot_interface.utilities.uuid_string_factory import uuid4_string
 
 
 @dataclass
 class Mission:
-    tasks: List[Task]
+    tasks: List[TASKS]
     id: str = field(default_factory=uuid4_string, init=True)
     name: str = ""
     start_pose: Optional[Pose] = None
diff --git a/src/robot_interface/models/mission/status.py b/src/robot_interface/models/mission/status.py
index edb15f7e..fcd772ae 100644
--- a/src/robot_interface/models/mission/status.py
+++ b/src/robot_interface/models/mission/status.py
@@ -11,14 +11,6 @@ class MissionStatus(str, Enum):
     PartiallySuccessful: str = "partially_successful"
 
 
-class StepStatus(str, Enum):
-    NotStarted: str = "not_started"
-    Successful: str = "successful"
-    InProgress: str = "in_progress"
-    Failed: str = "failed"
-    Cancelled: str = "cancelled"
-
-
 class TaskStatus(str, Enum):
     NotStarted: str = "not_started"
     InProgress: str = "in_progress"
diff --git a/src/robot_interface/models/mission/step.py b/src/robot_interface/models/mission/step.py
deleted file mode 100644
index 54b1175a..00000000
--- a/src/robot_interface/models/mission/step.py
+++ /dev/null
@@ -1,234 +0,0 @@
-from dataclasses import dataclass, field
-from typing import Any, List, Literal, Optional, Type, Union
-
-from alitra import Pose, Position
-
-from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
-from robot_interface.models.inspection import (
-    Audio,
-    Image,
-    Inspection,
-    ThermalImage,
-    ThermalVideo,
-    Video,
-)
-from robot_interface.models.mission.status import StepStatus
-from robot_interface.utilities.uuid_string_factory import uuid4_string
-
-
-@dataclass
-class Step:
-    """
-    Base class for all steps in a mission.
-    """
-
-    id: str = field(default_factory=uuid4_string, init=False)
-    status: StepStatus = field(default=StepStatus.NotStarted, init=False)
-    error_message: Optional[ErrorMessage] = field(default=None, init=False)
-
-    def __str__(self) -> str:
-        def add_indent(text: str) -> str:
-            return "".join("  " + line for line in text.splitlines(True))
-
-        def robot_class_to_pretty_string(obj: Step) -> str:
-            log_message: str = ""
-            for attr in dir(obj):
-                if callable(getattr(obj, attr)) or attr.startswith("__"):
-                    continue
-
-                value: Any = getattr(obj, attr)
-                try:
-                    package_name: Optional[str] = (
-                        str(value.__class__).split("'")[1].split(".")[0]
-                    )
-                except (AttributeError, IndexError):
-                    package_name = None
-
-                if package_name == "robot_interface":
-                    log_message += (
-                        "\n" + attr + ": " + robot_class_to_pretty_string(value)
-                    )
-                else:
-                    log_message += "\n" + str(attr) + ": " + str(value)
-
-            return add_indent(log_message)
-
-        class_name: str = type(self).__name__
-        return class_name + robot_class_to_pretty_string(self)
-
-
-@dataclass
-class InspectionStep(Step):
-    """
-    Base class for all inspection steps which produce results to be uploaded.
-    """
-
-    inspections: List[Inspection] = field(default_factory=list, init=False)
-    tag_id: Optional[str] = field(default=None, init=False)
-    type = "inspection_type"
-    metadata: Optional[dict] = field(default_factory=dict, init=False)
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return Inspection
-
-
-@dataclass
-class MotionStep(Step):
-    """
-    Base class for all steps which should move the robot, but not return a result.
-    """
-
-    pass
-
-
-@dataclass
-class ContinousInspectionStep(Step):
-    """
-    Base class for all continous inspection steps which produce a result to be uploaded.
-    """
-
-    pass
-
-
-@dataclass
-class DriveToPose(MotionStep):
-    """
-    Step which causes the robot to move to the given pose.
-    """
-
-    pose: Pose
-    type: Literal["drive_to_pose"] = "drive_to_pose"
-
-
-@dataclass
-class DockingProcedure(MotionStep):
-    """
-    Step which causes the robot to dock or undock
-    """
-
-    behavior: Literal["dock", "undock"]
-    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):
-    """
-    Step which causes the robot to localize
-    """
-
-    localization_pose: Pose
-    type: Literal["localize"] = "localize"
-
-
-@dataclass
-class MoveArm(MotionStep):
-    """
-    Step which causes the robot to move its arm
-    """
-
-    arm_pose: str
-    type: Literal["move_arm"] = "move_arm"
-
-
-@dataclass
-class TakeImage(InspectionStep):
-    """
-    Step which causes the robot to take an image towards the given coordinate.
-    """
-
-    target: Position
-    type: Literal["take_image"] = "take_image"
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return Image
-
-
-@dataclass
-class TakeThermalImage(InspectionStep):
-    """
-    Step which causes the robot to take a thermal image towards the given coordinate.
-    """
-
-    target: Position
-    type: Literal["take_thermal_image"] = "take_thermal_image"
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return ThermalImage
-
-
-@dataclass
-class TakeVideo(InspectionStep):
-    """
-    Step which causes the robot to take a video towards the given coordinate.
-
-    Duration of video is given in seconds.
-    """
-
-    target: Position
-    duration: float
-    type: Literal["take_video"] = "take_video"
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return Video
-
-
-@dataclass
-class TakeThermalVideo(InspectionStep):
-    """
-    Step which causes the robot to record thermal video towards the given coordinate
-
-    Duration of video is given in seconds.
-    """
-
-    target: Position
-    duration: float
-    type: Literal["take_thermal_video"] = "take_thermal_video"
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return ThermalVideo
-
-
-@dataclass
-class RecordAudio(InspectionStep):
-    """
-    Step which causes the robot to record a video at its position, facing the target.
-
-    Duration of audio is given in seconds.
-    """
-
-    target: Position
-    duration: float
-    type: Literal["record_audio"] = "record_audio"
-
-    @staticmethod
-    def get_inspection_type() -> Type[Inspection]:
-        return Audio
-
-
-STEPS = Union[
-    DriveToPose,
-    DockingProcedure,
-    ReturnToHome,
-    Localize,
-    TakeImage,
-    TakeThermalImage,
-    TakeVideo,
-    TakeThermalVideo,
-    RecordAudio,
-    MoveArm,
-]
diff --git a/src/robot_interface/models/mission/task.py b/src/robot_interface/models/mission/task.py
index a378710d..ed289ed3 100644
--- a/src/robot_interface/models/mission/task.py
+++ b/src/robot_interface/models/mission/task.py
@@ -1,130 +1,197 @@
 from dataclasses import dataclass, field
-from typing import Iterator, List, Optional
+from enum import Enum
+from typing import Iterator, Literal, Optional, Type, Union
+
+from alitra import Pose, Position
 
 from robot_interface.models.exceptions.robot_exceptions import ErrorMessage
-from robot_interface.models.mission.status import StepStatus, TaskStatus
-from robot_interface.models.mission.step import (
-    STEPS,
-    DriveToPose,
-    InspectionStep,
-    MotionStep,
-    Step,
+from robot_interface.models.inspection import (
+    Audio,
+    Image,
+    Inspection,
+    ThermalImage,
+    ThermalVideo,
+    Video,
 )
+from robot_interface.models.mission.status import TaskStatus
 from robot_interface.utilities.uuid_string_factory import uuid4_string
 
 
+class TaskTypes(str, Enum):
+    ReturnToHome = "return_to_home"
+    Localize = "localize"
+    MoveArm = "move_arm"
+    TakeImage = "take_image"
+    TakeThermalImage = "take_thermal_image"
+    TakeVideo = "take_video"
+    TakeThermalVideo = "take_thermal_video"
+    RecordAudio = "record_audio"
+    DockingProcedure = "docking_procedure"
+
+
 @dataclass
 class Task:
-    steps: List[STEPS]
     status: TaskStatus = field(default=TaskStatus.NotStarted, init=False)
     error_message: Optional[ErrorMessage] = field(default=None, init=False)
     tag_id: Optional[str] = field(default=None)
     id: str = field(default_factory=uuid4_string, init=True)
-    _iterator: Iterator = None
-
-    def next_step(self) -> Optional[Step]:
-        try:
-            step: Step = next(self._iterator)
-            while step.status != StepStatus.NotStarted:
-                step = next(self._iterator)
-            return step
-        except StopIteration:
-            return None
 
     def is_finished(self) -> bool:
-        for step in self.steps:
-            if step.status is StepStatus.Failed and isinstance(step, MotionStep):
-                # One motion step has failed meaning the task as a whole should be
-                # considered as failed
-                return True
-
-            elif (step.status is StepStatus.Failed) and isinstance(
-                step, InspectionStep
-            ):
-                # It should be possible to perform several inspections per task. If
-                # one out of many inspections fail the task is considered as
-                # partially successful.
-                continue
-
-            elif step.status is StepStatus.Successful:
-                # The task is complete once all steps are completed
-                continue
-            else:
-                # Not all steps have been completed yet
-                return False
-
-        return True
-
-    def update_task_status(self) -> None:
-        some_not_started: bool = False
-        for step in self.steps:
-            if step.status is StepStatus.Failed and isinstance(step, MotionStep):
-                self.error_message = ErrorMessage(
-                    error_reason=None,
-                    error_description=f"Inspection "
-                    f"{'of ' + self.tag_id if self.tag_id else ''}failed because the "
-                    f"robot could not navigate to the desired location",
-                )
-                self.status = TaskStatus.Failed
-                return
-
-            elif (step.status is StepStatus.Failed) and isinstance(
-                step, InspectionStep
-            ):
-                self.error_message = ErrorMessage(
-                    error_reason=None,
-                    error_description=f"Inspection "
-                    f"{'of ' + self.tag_id if self.tag_id else ''}was partially "
-                    f"successful because one or more inspection steps failed",
-                )
-                self.status = TaskStatus.PartiallySuccessful
-                continue
-
-            elif step.status is StepStatus.Successful:
-                continue
-
-            elif (step.status is StepStatus.NotStarted) and isinstance(
-                step, InspectionStep
-            ):
-                some_not_started = True
-
-        if self.status is not TaskStatus.PartiallySuccessful:
-            if some_not_started:
-                self.status = TaskStatus.InProgress  # TODO: handle all not_started
-            else:
-                self.status = TaskStatus.Successful
-
-        elif self._all_inspection_steps_failed():
-            self.error_message = ErrorMessage(
-                error_reason=None,
-                error_description=f"Inspection "
-                f"{'of ' + self.tag_id if self.tag_id else ''}failed as all inspection "
-                f"steps failed",
-            )
-            self.status = TaskStatus.Failed
-
-    def reset_task(self):
-        self.error_message = None
-        for step in self.steps:
-            step.error_message = None
-            if isinstance(step, DriveToPose):
-                step.status = StepStatus.NotStarted
-            elif (
-                isinstance(step, InspectionStep)
-                and step.status == StepStatus.InProgress
-            ):
-                step.status = StepStatus.NotStarted
-        self._iterator = iter(self.steps)
-
-    def _all_inspection_steps_failed(self) -> bool:
-        for step in self.steps:
-            if isinstance(step, MotionStep):
-                continue
-            elif step.status is not StepStatus.Failed:
-                return False
-
-        return True
-
-    def __post_init__(self) -> None:
-        if self._iterator is None:
-            self._iterator = iter(self.steps)
+        if (
+            self.status == TaskStatus.Successful
+            or self.status == TaskStatus.PartiallySuccessful
+            or self.status == TaskStatus.Cancelled
+            or self.status == TaskStatus.Failed
+        ):
+            return True
+        return False
+
+    def update_task_status(self) -> TaskStatus:
+        return self.status
+
+
+@dataclass
+class InspectionTask(Task):
+    """
+    Base class for all inspection tasks which produce results to be uploaded.
+    """
+
+    inspection: Inspection = field(default=None, init=True)
+    robot_pose: Pose = field(default=None, init=True)
+    metadata: Optional[dict] = field(default_factory=dict, init=True)
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return Inspection
+
+
+@dataclass
+class DockingProcedure(Task):
+    """
+    Task which causes the robot to dock or undock
+    """
+
+    behavior: Literal["dock", "undock"] = field(default=None, init=True)
+    type: Literal[TaskTypes.DockingProcedure] = TaskTypes.DockingProcedure
+
+
+@dataclass
+class ReturnToHome(Task):
+    """
+    Task which cases the robot to return home
+    """
+
+    pose: Pose = field(default=None, init=True)
+    type: Literal[TaskTypes.ReturnToHome] = TaskTypes.ReturnToHome
+
+
+@dataclass
+class Localize(Task):
+    """
+    Task which causes the robot to localize
+    """
+
+    localization_pose: Pose = field(default=None, init=True)
+    type: Literal[TaskTypes.Localize] = TaskTypes.Localize
+
+
+@dataclass
+class MoveArm(Task):
+    """
+    Task which causes the robot to move its arm
+    """
+
+    arm_pose: str = field(default=None, init=True)
+    type: Literal[TaskTypes.MoveArm] = TaskTypes.MoveArm
+
+
+@dataclass
+class TakeImage(InspectionTask):
+    """
+    Task which causes the robot to take an image towards the given coordinate.
+    """
+
+    target: Position = field(default=None, init=True)
+    type: Literal[TaskTypes.TakeImage] = TaskTypes.TakeImage
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return Image
+
+
+@dataclass
+class TakeThermalImage(InspectionTask):
+    """
+    Task which causes the robot to take a thermal image towards the given coordinate.
+    """
+
+    target: Position = field(default=None, init=True)
+    type: Literal[TaskTypes.TakeThermalImage] = TaskTypes.TakeThermalImage
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return ThermalImage
+
+
+@dataclass
+class TakeVideo(InspectionTask):
+    """
+    Task which causes the robot to take a video towards the given coordinate.
+
+    Duration of video is given in seconds.
+    """
+
+    target: Position = field(default=None, init=True)
+    duration: float = field(default=None, init=True)
+    type: Literal[TaskTypes.TakeVideo] = TaskTypes.TakeVideo
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return Video
+
+
+@dataclass
+class TakeThermalVideo(InspectionTask):
+    """
+    Task which causes the robot to record thermal video towards the given coordinate
+
+    Duration of video is given in seconds.
+    """
+
+    target: Position = field(default=None, init=True)
+    duration: float = field(default=None, init=True)
+    type: Literal[TaskTypes.TakeThermalVideo] = TaskTypes.TakeThermalVideo
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return ThermalVideo
+
+
+@dataclass
+class RecordAudio(InspectionTask):
+    """
+    Task which causes the robot to record a video at its position, facing the target.
+
+    Duration of audio is given in seconds.
+    """
+
+    target: Position = field(default=None, init=True)
+    duration: float = field(default=None, init=True)
+    type: Literal[TaskTypes.RecordAudio] = TaskTypes.RecordAudio
+
+    @staticmethod
+    def get_inspection_type() -> Type[Inspection]:
+        return Audio
+
+
+TASKS = Union[
+    ReturnToHome,
+    Localize,
+    MoveArm,
+    TakeImage,
+    TakeThermalImage,
+    TakeVideo,
+    TakeThermalVideo,
+    RecordAudio,
+    DockingProcedure,
+]
diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py
index 2d00d641..fb374390 100644
--- a/src/robot_interface/robot_interface.py
+++ b/src/robot_interface/robot_interface.py
@@ -1,13 +1,13 @@
 from abc import ABCMeta, abstractmethod
 from queue import Queue
 from threading import Thread
-from typing import List, Sequence
+from typing import List
 
 from robot_interface.models.initialize import InitializeParams
 from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus
-from robot_interface.models.mission.step import InspectionStep, Step
+from robot_interface.models.mission.status import RobotStatus, TaskStatus
+from robot_interface.models.mission.task import InspectionTask, Task
 
 
 class RobotInterface(metaclass=ABCMeta):
@@ -18,7 +18,7 @@ def initiate_mission(self, mission: Mission) -> None:
         """Send a mission to the robot and initiate execution of the mission
 
         This function should be used in combination with the mission_status function
-        if the robot is designed to run full missions and not in a stepwise
+        if the robot is designed to run full missions and not in a taskwise
         configuration.
 
         Parameters
@@ -38,48 +38,23 @@ def initiate_mission(self, mission: Mission) -> None:
             Will catch all RobotExceptions not previously listed and retry scheduling of
             the mission until the number of allowed retries is exceeded
         NotImplementedError
-            If the robot is designed for stepwise mission execution
+            If the robot is designed for taskwise mission execution
 
         """
         raise NotImplementedError
 
-    def mission_status(self) -> MissionStatus:
-        """Gets the status of the currently active mission on the robot
-
-        This function should be used in combination with the initiate_mission function
-        if the robot is designed to run full missions and not in a stepwise
-        configuration.
-
-        Returns
-        -------
-        MissionStatus
-            Status of the executing mission on the robot.
-
-        Raises
-        ------
-        RobotMissionStatusException
-            If the mission status could not be collected this will lead to the mission
-            being marked as failed
-        RobotException
-            An uncaught RobotException in the robot package while retrieving the status
-            will cause the mission to be marked as failed
-        NotImplementedError
-            If the robot is designed for stepwise mission execution
-
-        """
-
     @abstractmethod
-    def initiate_step(self, step: Step) -> None:
-        """Send a step to the robot and initiate the execution of the step
+    def initiate_task(self, task: Task) -> None:
+        """Send a task to the robot and initiate the execution of the task
 
-        This function should be used in combination with the step_status function
-        if the robot is designed to run stepwise missions and not in a full mission
+        This function should be used in combination with the task_status function
+        if the robot is designed to run taskwise missions and not in a full mission
         configuration.
 
         Parameters
         ----------
-        step : Step
-            The step that should be initiated on the robot.
+        task : Task
+            The task that should be initiated on the robot.
 
         Returns
         -------
@@ -87,12 +62,12 @@ def initiate_step(self, step: Step) -> None:
 
         Raises
         ------
-        RobotInfeasibleStepException
-            If the step input is infeasible and the step fails to be scheduled in
+        RobotInfeasibleTaskException
+            If the task input is infeasible and the task fails to be scheduled in
             a way that means attempting to schedule again is not necessary
         RobotException
             Will catch all RobotExceptions not previously listed and retry scheduling
-            of the step until the number of allowed retries is exceeded before the step
+            of the task until the number of allowed retries is exceeded before the task
             will be marked as failed and the mission cancelled
         NotImplementedError
             If the robot is designed for full mission execution.
@@ -101,22 +76,22 @@ def initiate_step(self, step: Step) -> None:
         raise NotImplementedError
 
     @abstractmethod
-    def step_status(self) -> StepStatus:
-        """Gets the status of the currently active step on robot.
+    def task_status(self, task_id: str) -> TaskStatus:
+        """Gets the status of the currently active task on robot.
 
-        This function should be used in combination with the initiate_step function
-        if the robot is designed to run stepwise missions and not in a full mission
+        This function should be used in combination with the initiate_task function
+        if the robot is designed to run taskwise missions and not in a full mission
         configuration.
 
         Returns
         -------
-        StepStatus
-            Status of the execution of current step.
+        TaskStatus
+            Status of the execution of current task.
 
         Raises
         ------
         RobotException
-            If the step status could not be retrieved.
+            If the task status could not be retrieved.
         NotImplementedError
             If the robot is designed for full mission execution.
 
@@ -125,7 +100,7 @@ def step_status(self) -> StepStatus:
 
     @abstractmethod
     def stop(self) -> None:
-        """Stops the execution of the current step and stops the movement of the robot.
+        """Stops the execution of the current task and stops the movement of the robot.
 
         Returns
         -------
@@ -144,7 +119,7 @@ def stop(self) -> None:
 
     @abstractmethod
     def pause(self) -> None:
-        """Pauses the execution of the current step and stops the movement of the robot.
+        """Pauses the execution of the current task and stops the movement of the robot.
 
         Returns
         -------
@@ -163,7 +138,7 @@ def pause(self) -> None:
 
     @abstractmethod
     def resume(self) -> None:
-        """Resumes the execution of the current step and continues the rest of the mission.
+        """Resumes the execution of the current task and continues the rest of the mission.
 
         Returns
         -------
@@ -181,23 +156,23 @@ def resume(self) -> None:
         raise NotImplementedError
 
     @abstractmethod
-    def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
-        """Return the inspections connected to the given step.
+    def get_inspection(self, task: InspectionTask) -> Inspection:
+        """Return the inspection connected to the given task.
 
         Parameters
         ----------
-        step : Step
+        task : InspectionTask
 
         Returns
         -------
         Sequence[InspectionResult]
-            List containing all the inspection results connected to the given step
+            List containing all the inspection results connected to the given task
 
         Raises
         ------
         RobotRetrieveInspectionException
             If the robot package is unable to retrieve the inspections for the relevant
-            mission or step an error message is logged and the state machine continues
+            mission or task an error message is logged and the state machine continues
         RobotException
             Catches other RobotExceptions that lead to the same result as a
             RobotRetrieveInspectionException
diff --git a/tests/integration/turtlebot/test_successful_mission.py b/tests/integration/turtlebot/test_successful_mission.py
index 9159db30..fadd51b2 100644
--- a/tests/integration/turtlebot/test_successful_mission.py
+++ b/tests/integration/turtlebot/test_successful_mission.py
@@ -28,7 +28,7 @@
 from isar.services.readers.base_reader import BaseReader
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import DriveToPose
+from robot_interface.models.mission.task import ReturnToHome
 from tests.isar.state_machine.test_state_machine import (
     StateMachineThread,
     UploaderThread,
@@ -110,14 +110,11 @@ def test_successful_mission(
 
     mission_result_folder: Path = Path(f"tests/results/{mission_id}")
 
-    drive_to_steps: List[DriveToPose] = [
-        step
-        for task in mission.tasks
-        for step in task.steps
-        if isinstance(step, DriveToPose)
+    drive_to_tasks: List[ReturnToHome] = [
+        task for task in mission.tasks if isinstance(task, ReturnToHome)
     ]
 
-    expected_positions: list = [step.pose.position for step in drive_to_steps]
+    expected_positions: list = [task.pose for task in drive_to_tasks]
 
     paths = mission_result_folder.rglob("*.json")
     for path in paths:
diff --git a/tests/isar/apis/scheduler/test_scheduler_router.py b/tests/isar/apis/scheduler/test_scheduler_router.py
index 343e40bc..46472c27 100644
--- a/tests/isar/apis/scheduler/test_scheduler_router.py
+++ b/tests/isar/apis/scheduler/test_scheduler_router.py
@@ -15,6 +15,7 @@
 from isar.models.communication.queues.queue_timeout_error import QueueTimeoutError
 from isar.services.utilities.scheduling_utilities import SchedulingUtilities
 from isar.state_machine.states_enum import States
+from robot_interface.models.mission.task import TaskTypes
 from tests.mocks.mission_definition import MockMissionDefinition
 
 mock_mission = MockMissionDefinition.default_mission
@@ -26,15 +27,12 @@
 mock_void = mock.Mock()
 
 mock_task = mock_mission.tasks[0]
-mock_step = mock_task.steps[0]
 mock_start_mission_response = MockMissionDefinition.mock_start_mission_response
 mock_control_mission_response = ControlMissionResponse(
     mission_id=mock_mission.id,
     mission_status=mock_mission.status,
     task_id=mock_task.id,
     task_status=mock_task.status,
-    step_id=mock_step.id,
-    step_status=mock_step.status,
 )
 mock_return_control_mission_response = mock.Mock(
     return_value=mock_control_mission_response
@@ -90,7 +88,7 @@ def test_robot_not_capable(self, client: TestClient):
         assert re.match(
             "Bad Request - Robot is not capable of performing mission.", response_detail
         )
-        assert re.search("drive_to_pose", response_detail)
+        assert re.search("return_to_home", response_detail)
         assert re.search("take_image", response_detail)
 
     @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
@@ -109,18 +107,9 @@ class TestStartMission:
     mock_start_mission_with_task_ids_content = {
         "mission_definition": MockMissionDefinition.mock_start_mission_definition_task_ids
     }
-    mock_start_mission_with_step_ids_content = {
-        "mission_definition": MockMissionDefinition.mock_start_mission_definition_step_ids
-    }
     mock_start_mission_duplicate_task_ids_content = {
         "mission_definition": MockMissionDefinition.mock_start_mission_definition_with_duplicate_task_ids
     }
-    mock_start_mission_duplicate_step_ids_content = {
-        "mission_definition": MockMissionDefinition.mock_start_mission_definition_with_duplicate_step_ids
-    }
-    mock_start_mission_duplicate_step_ids_cross_task_content = {
-        "mission_definition": MockMissionDefinition.mock_start_mission_definition_with_duplicate_step_ids_cross_tasks
-    }
 
     @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
     @mock.patch.object(SchedulingUtilities, "start_mission", mock_void)
@@ -165,7 +154,7 @@ def test_robot_not_capable(self, client: TestClient):
         assert re.match(
             "Bad Request - Robot is not capable of performing mission.", response_detail
         )
-        assert re.search("drive_to_pose", response_detail)
+        assert re.search("return_to_home", response_detail)
         assert re.search("take_image", response_detail)
 
     @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
@@ -175,7 +164,8 @@ def test_mission_with_input_task_ids(self, client: TestClient):
         for task in self.mock_start_mission_with_task_ids_content[
             "mission_definition"
         ].tasks:
-            expected_ids.append(task.id)
+            if task.id:
+                expected_ids.append(task.id)
 
         response = client.post(
             url=self.schedule_start_mission_path,
@@ -188,26 +178,27 @@ def test_mission_with_input_task_ids(self, client: TestClient):
 
     @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
     @mock.patch.object(SchedulingUtilities, "start_mission", mock_void)
-    def test_mission_with_input_step_ids(self, client: TestClient):
+    def test_mission_with_input_inspection_task_ids(self, client: TestClient):
         expected_inspection_ids: List[str] = []
-        for task in self.mock_start_mission_with_step_ids_content[
+        for task in self.mock_start_mission_with_task_ids_content[
             "mission_definition"
         ].tasks:
-            for inspection in task.inspections:
-                expected_inspection_ids.append(inspection.id)
+            expected_inspection_ids.append(task.inspection.id)
 
         response = client.post(
             url=self.schedule_start_mission_path,
-            json=jsonable_encoder(self.mock_start_mission_with_step_ids_content),
+            json=jsonable_encoder(self.mock_start_mission_with_task_ids_content),
         )
         assert response.status_code == HTTPStatus.OK
         start_mission_response: dict = response.json()
         for task in start_mission_response["tasks"]:
-            for step in task["steps"]:
-                type: str = step["type"]
-                is_inspection_step: bool = type.lower().find("drive") == -1
-                if is_inspection_step:
-                    assert step["id"] in expected_inspection_ids
+            if (
+                task["type"] == TaskTypes.ReturnToHome == False
+                and task["type"] == TaskTypes.Localize == False
+                and task["type"] == TaskTypes.DockingProcedure == False
+                and task["type"] == TaskTypes.MoveArm == False
+            ):
+                assert task["id"] in expected_inspection_ids
 
     @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
     @mock.patch.object(SchedulingUtilities, "start_mission", mock_void)
@@ -218,26 +209,6 @@ def test_mission_with_duplicate_task_ids(self, client: TestClient):
         )
         assert response.status_code == HTTPStatus.BAD_REQUEST
 
-    @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
-    @mock.patch.object(SchedulingUtilities, "start_mission", mock_void)
-    def test_mission_with_duplicate_step_ids(self, client: TestClient):
-        response = client.post(
-            url=self.schedule_start_mission_path,
-            json=jsonable_encoder(self.mock_start_mission_duplicate_step_ids_content),
-        )
-        assert response.status_code == HTTPStatus.BAD_REQUEST
-
-    @mock.patch.object(SchedulingUtilities, "get_state", mock_return_idle)
-    @mock.patch.object(SchedulingUtilities, "start_mission", mock_void)
-    def test_mission_with_duplicate_step_ids_cross_tasks(self, client: TestClient):
-        response = client.post(
-            url=self.schedule_start_mission_path,
-            json=jsonable_encoder(
-                self.mock_start_mission_duplicate_step_ids_cross_task_content
-            ),
-        )
-        assert response.status_code == HTTPStatus.BAD_REQUEST
-
 
 class TestPauseMission:
     schedule_pause_mission_path = "/schedule/pause-mission"
diff --git a/tests/isar/mission/test_mission.py b/tests/isar/mission/test_mission.py
index d7e47e60..9bf6bccd 100644
--- a/tests/isar/mission/test_mission.py
+++ b/tests/isar/mission/test_mission.py
@@ -1,41 +1,42 @@
 from alitra import Frame, Orientation, Pose, Position
 
 from isar.services.readers.base_reader import BaseReader
-from robot_interface.models.mission.mission import Mission, Task
-from robot_interface.models.mission.step import (
-    STEPS,
-    DriveToPose,
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.task import (
+    TASKS,
     TakeImage,
     TakeThermalImage,
+    ReturnToHome,
 )
 
-drive_to_pose_1 = DriveToPose(
-    pose=Pose(
-        position=Position(x=2, y=2, z=0, frame=Frame(name="asset")),
-        orientation=Orientation(
-            x=0, y=0, z=0.7071068, w=0.7071068, frame=Frame(name="asset")
-        ),
-        frame=Frame(name="asset"),
+robot_pose_1 = Pose(
+    position=Position(x=2, y=2, z=0, frame=Frame(name="asset")),
+    orientation=Orientation(
+        x=0, y=0, z=0.7071068, w=0.7071068, frame=Frame(name="asset")
     ),
+    frame=Frame(name="asset"),
 )
 
-take_image_1 = TakeImage(target=Position(x=2, y=3, z=1, frame=Frame(name="asset")))
-
-drive_to_pose_2 = DriveToPose(
-    pose=Pose(
-        position=Position(x=4, y=4, z=0, frame=Frame(name="asset")),
-        orientation=Orientation(
-            x=0, y=0, z=-0.7071068, w=0.7071068, frame=Frame(name="asset")
-        ),
-        frame=Frame(name="asset"),
+robot_pose_2 = Pose(
+    position=Position(x=4, y=4, z=0, frame=Frame(name="asset")),
+    orientation=Orientation(
+        x=0, y=0, z=-0.7071068, w=0.7071068, frame=Frame(name="asset")
     ),
+    frame=Frame(name="asset"),
+)
+
+
+task_take_image = TakeImage(
+    target=Position(x=2, y=3, z=1, frame=Frame(name="asset")),
+    robot_pose=robot_pose_1,
 )
 
-take_image_2 = TakeThermalImage(
-    target=Position(x=4, y=3, z=1, frame=Frame(name="asset"))
+task_take_thermal_image = TakeThermalImage(
+    target=Position(x=4, y=3, z=1, frame=Frame(name="asset")),
+    robot_pose=robot_pose_2,
 )
 
-drive_to_pose_3 = DriveToPose(
+task_return_to_home = ReturnToHome(
     pose=Pose(
         position=Position(x=0, y=0, z=0, frame=Frame(name="asset")),
         orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame(name="asset")),
@@ -45,93 +46,70 @@
 
 expected_mission = Mission(
     id="1",
-    tasks=[
-        Task(id="11", steps=[drive_to_pose_1, take_image_1]),
-        Task(id="12", steps=[drive_to_pose_2, take_image_2]),
-        Task(id="13", steps=[drive_to_pose_3]),
-    ],
+    tasks=[task_take_image, task_take_thermal_image, task_return_to_home],
 )
 
-
 example_mission_dict = {
     "id": "1",
     "tasks": [
         {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 2,
-                            "y": 2,
-                            "z": 0,
-                            "frame": {"name": "asset"},
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0.7071068,
-                            "w": 0.7071068,
-                            "frame": {"name": "asset"},
-                        },
-                        "frame": {"name": "asset"},
-                    },
+            "type": "take_image",
+            "target": {"x": 2, "y": 3, "z": 1, "frame": {"name": "asset"}},
+            "robot_pose": {
+                "position": {
+                    "x": 2,
+                    "y": 2,
+                    "z": 0,
+                    "frame": {"name": "asset"},
                 },
-                {
-                    "type": "take_image",
-                    "target": {"x": 2, "y": 3, "z": 1, "frame": {"name": "asset"}},
+                "orientation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0.7071068,
+                    "w": 0.7071068,
+                    "frame": {"name": "asset"},
                 },
-            ],
+                "frame": {"name": "asset"},
+            },
         },
         {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 4,
-                            "y": 4,
-                            "z": 0,
-                            "frame": {"name": "asset"},
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": -0.7071068,
-                            "w": 0.7071068,
-                            "frame": {"name": "asset"},
-                        },
-                        "frame": {"name": "asset"},
-                    },
+            "type": "take_thermal_image",
+            "target": {"x": 4, "y": 3, "z": 1, "frame": {"name": "asset"}},
+            "pose": {
+                "position": {
+                    "x": 4,
+                    "y": 4,
+                    "z": 0,
+                    "frame": {"name": "asset"},
                 },
-                {
-                    "type": "take_thermal_image",
-                    "target": {"x": 4, "y": 3, "z": 1, "frame": {"name": "asset"}},
+                "orientation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": -0.7071068,
+                    "w": 0.7071068,
+                    "frame": {"name": "asset"},
                 },
-            ],
+                "frame": {"name": "asset"},
+            },
         },
         {
-            "steps": [
-                {
-                    "type": "drive_to_pose",
-                    "pose": {
-                        "position": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0,
-                            "frame": {"name": "asset"},
-                        },
-                        "orientation": {
-                            "x": 0,
-                            "y": 0,
-                            "z": 0,
-                            "w": 1,
-                            "frame": {"name": "asset"},
-                        },
-                        "frame": {"name": "asset"},
-                    },
-                }
-            ],
+            "type": "return_to_home",
+            "pose": {
+                "position": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0,
+                    "frame": {"name": "asset"},
+                },
+                "orientation": {
+                    "x": 0,
+                    "y": 0,
+                    "z": 0,
+                    "w": 1,
+                    "frame": {"name": "asset"},
+                },
+                "frame": {"name": "asset"},
+            },
         },
     ],
 }
@@ -141,7 +119,7 @@ def test_mission_definition() -> None:
     loaded_mission: Mission = BaseReader.dict_to_dataclass(
         dataclass_dict=example_mission_dict,
         target_dataclass=Mission,
-        strict_config=True,
+        strict_config=False,
     )
 
     assert loaded_mission.id == expected_mission.id
@@ -149,23 +127,8 @@ def test_mission_definition() -> None:
 
     assert len(loaded_mission.tasks) == len(expected_mission.tasks)
     for i_task in range(len(loaded_mission.tasks)):
-        loaded_task: Task = loaded_mission.tasks[i_task]
-        expected_task: Task = expected_mission.tasks[i_task]
+        loaded_task: TASKS = loaded_mission.tasks[i_task]
+        expected_task: TASKS = expected_mission.tasks[i_task]
 
         assert loaded_task.status == expected_task.status
         assert loaded_task.tag_id == expected_task.tag_id
-
-        for i_step in range(len(loaded_task.steps)):
-            loaded_step: STEPS = loaded_task.steps[i_step]
-            expected_step: STEPS = expected_task.steps[i_step]
-
-            ignore_attributes = set(
-                ("id", "status", "tag_id", "error_message", "analysis")
-            )
-            loaded_attributes = set(loaded_step.__dict__.keys()) - ignore_attributes
-            expected_attributes = set(expected_step.__dict__.keys()) - ignore_attributes
-
-            assert loaded_attributes == expected_attributes
-
-            for attr in loaded_attributes:
-                assert loaded_step.__dict__[attr] == expected_step.__dict__[attr]
diff --git a/tests/isar/models/example_mission_definition.json b/tests/isar/models/example_mission_definition.json
index fbe1bccd..e19b1455 100644
--- a/tests/isar/models/example_mission_definition.json
+++ b/tests/isar/models/example_mission_definition.json
@@ -1,40 +1,37 @@
 {
-    "tasks": [
-        {
-            "pose": {
-                "position": {
-                    "x": 0,
-                    "y": 0,
-                    "z": 0,
-                    "frame_name": "robot"
-                },
-                "orientation": {
-                    "x": 0,
-                    "y": 0,
-                    "z": 0,
-                    "w": 0,
-                    "frame_name": "robot"
-                },
-                "frame_name": "robot"
-            },
-            "inspections": [
-                {
-                    "type": "Image",
-                    "inspection_target": {
-                        "x": 0,
-                        "y": 0,
-                        "z": 0,
-                        "frame_name": "robot"
-                    },
-                    "duration": 0,
-                    "metadata": {},
-                    "id": "generated_inspection_id"
-                }
-            ],
-            "tag": "MY-TAG-123",
-            "id": "generated_task_id"
-        }
-    ],
-    "id": "generated_mission_id",
-    "name": "my-mission"
+  "id": "generated_mission_id",
+  "name": "my-mission",
+  "tasks": [
+    {
+      "type": "inspection",
+      "pose": {
+        "position": {
+          "x": 0,
+          "y": 0,
+          "z": 0,
+          "frame_name": "robot"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0,
+          "w": 0,
+          "frame_name": "robot"
+        },
+        "frame_name": "robot"
+      },
+      "inspection": {
+        "type": "Image",
+        "inspection_target": {
+          "x": 0,
+          "y": 0,
+          "z": 0,
+          "frame_name": "robot"
+        },
+        "id": "generated_inspection_id"
+      },
+      "tag": "MY-TAG-123",
+      "id": "generated_task_id"
+    }
+  ]
 }
diff --git a/tests/isar/models/test_start_mission_definition.py b/tests/isar/models/test_start_mission_definition.py
index c5a4b3a0..dee0e983 100644
--- a/tests/isar/models/test_start_mission_definition.py
+++ b/tests/isar/models/test_start_mission_definition.py
@@ -1,6 +1,6 @@
 import json
 import os
-from typing import List, Union
+from typing import List
 
 import pytest
 from alitra import Frame, Orientation, Pose, Position
@@ -11,21 +11,13 @@
     to_isar_mission,
 )
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import STEPS, Step
-from robot_interface.models.mission.task import Task
+from robot_interface.models.mission.task import TASKS, Task
 
-task_1: Task = Task([], tag_id=None, id="123")
-task_2: Task = Task([], tag_id=None, id="123")
-task_3: Task = Task([], tag_id=None, id="123456")
-task_4: Task = Task([])
-task_5: Task = Task([])
-
-step_1: Step = Step()
-step_1.id = "123"
-step_2: Step = Step()
-step_2.id = "123"
-step_3: Step = Step()
-step_3.id = "123456"
+task_1: Task = Task(tag_id=None, id="123")
+task_2: Task = Task(tag_id=None, id="123")
+task_3: Task = Task(tag_id=None, id="123456")
+task_4: Task = Task()
+task_5: Task = Task()
 
 
 @pytest.mark.parametrize(
@@ -39,23 +31,9 @@
             [task_1, task_3, task_4, task_5],
             False,
         ),
-        (
-            [step_1, step_2, step_3],
-            True,
-        ),
-        (
-            [step_1, step_3],
-            False,
-        ),
-        (
-            [step_1, step_3],
-            False,
-        ),
     ],
 )
-def test_duplicate_id_check(
-    item_list: Union[List[Task], List[STEPS]], expected_boolean: bool
-):
+def test_duplicate_id_check(item_list: List[TASKS], expected_boolean: bool):
     duplicates: List[str] = get_duplicate_ids(item_list)
     has_duplicates: bool = len(duplicates) > 0
     assert has_duplicates == expected_boolean
@@ -76,19 +54,10 @@ def test_mission_definition_to_isar_mission():
 
     task = generated_mission.tasks[0]
     assert task.id == "generated_task_id"
-    assert task.tag_id == "MY-TAG-123"
-
-    first_step = task.steps[0]
-    # assert first_step.id == "generated_inspection_id"
-    assert first_step.type == "drive_to_pose"
-    assert first_step.pose == Pose(
+    assert task.robot_pose == Pose(
         position=Position(0.0, 0.0, 0.0, frame=Frame("robot")),
         orientation=Orientation(0.0, 0.0, 0.0, 0.0, frame=Frame("robot")),
         frame=Frame("robot"),
     )
-
-    second_step = task.steps[1]
-    # assert second_step.id == "generated_inspection_id"
-    assert second_step.type == "take_image"
-    assert second_step.tag_id == "MY-TAG-123"
-    assert second_step.target == Position(0.0, 0.0, 0.0, frame=Frame("robot"))
+    assert task.type == "take_image"
+    assert task.target == Position(0.0, 0.0, 0.0, frame=Frame("robot"))
diff --git a/tests/isar/services/readers/test_base_reader.py b/tests/isar/services/readers/test_base_reader.py
index 7dc562dc..b694a4b0 100644
--- a/tests/isar/services/readers/test_base_reader.py
+++ b/tests/isar/services/readers/test_base_reader.py
@@ -5,20 +5,16 @@
 from alitra import Pose
 
 from isar.services.readers.base_reader import BaseReader
-from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import Step
-from tests.mocks.mission_definition import MockMissionDefinition
+from robot_interface.models.mission.task import ReturnToHome, TakeImage
 from tests.mocks.pose import MockPose
-from tests.mocks.step import MockStep
+from tests.mocks.task import MockTask
 
 
 class TestBaseReader:
     @pytest.mark.parametrize(
         "dataclass_dict, expected_dataclass",
         [
-            (asdict(MockMissionDefinition.default_mission), Mission),
-            (asdict(MockStep.drive_to()), Step),
-            (asdict(MockStep.take_image_in_coordinate_direction()), Step),
+            (asdict(MockTask.return_home()), ReturnToHome),
             (asdict(MockPose.default_pose()), Pose),
         ],
     )
diff --git a/tests/isar/services/readers/test_mission_reader.py b/tests/isar/services/readers/test_mission_reader.py
index 088063b7..80c88376 100644
--- a/tests/isar/services/readers/test_mission_reader.py
+++ b/tests/isar/services/readers/test_mission_reader.py
@@ -1,4 +1,5 @@
 from pathlib import Path
+from typing import List, Union
 
 import pytest
 from alitra import Frame, Orientation, Pose, Position
@@ -6,9 +7,9 @@
 from isar.config.settings import settings
 from isar.mission_planner.mission_planner_interface import MissionNotFoundError
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.step import (
-    DriveToPose,
-    Step,
+from robot_interface.models.mission.task import (
+    TASKS,
+    ReturnToHome,
     TakeImage,
     TakeThermalImage,
 )
@@ -28,55 +29,56 @@ def test_get_mission(mission_reader, mission_path) -> None:
 
 
 def test_read_mission_from_file(mission_reader) -> None:
-    expected_step_1 = DriveToPose(
-        pose=Pose(
-            position=Position(-2, -2, 0, Frame("asset")),
-            orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
-            frame=Frame("asset"),
-        )
+    expected_robot_pose_1 = Pose(
+        position=Position(-2, -2, 0, Frame("asset")),
+        orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
+        frame=Frame("asset"),
     )
-    task_1: Task = Task(steps=[expected_step_1])
-
-    expected_step_2 = DriveToPose(
-        pose=Pose(
-            position=Position(-2, 2, 0, Frame("asset")),
-            orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
-            frame=Frame("asset"),
-        )
+    task_1: ReturnToHome = ReturnToHome(pose=expected_robot_pose_1)
+
+    expected_robot_pose_2 = Pose(
+        position=Position(-2, 2, 0, Frame("asset")),
+        orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
+        frame=Frame("asset"),
+    )
+    expected_inspection_target_1 = Position(2, 2, 0, Frame("robot"))
+    task_2: TakeImage = TakeImage(
+        target=expected_inspection_target_1, robot_pose=expected_robot_pose_2
     )
-    expected_step_3 = TakeImage(target=Position(2, 2, 0, Frame("robot")))
-    task_2: Task = Task(steps=[expected_step_2, expected_step_3])
-
-    expected_step_4 = DriveToPose(
-        pose=Pose(
-            position=Position(2, 2, 0, Frame("asset")),
-            orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
-            frame=Frame("asset"),
-        )
+
+    expected_robot_pose_3 = Pose(
+        position=Position(2, 2, 0, Frame("asset")),
+        orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
+        frame=Frame("asset"),
     )
-    expected_step_5 = TakeImage(target=Position(2, 2, 0, Frame("robot")))
-    task_3: Task = Task(steps=[expected_step_4, expected_step_5])
-
-    expected_step_6 = DriveToPose(
-        pose=Pose(
-            position=Position(0, 0, 0, Frame("asset")),
-            orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
-            frame=Frame("asset"),
-        )
+    expected_inspection_target_2 = Position(2, 2, 0, Frame("robot"))
+    task_3: TakeImage = TakeImage(
+        target=expected_inspection_target_2, robot_pose=expected_robot_pose_3
     )
-    task_4: Task = Task(steps=[expected_step_6])
 
-    expected_tasks = [task_1, task_2, task_3, task_4]
+    expected_robot_pose_4 = Pose(
+        position=Position(0, 0, 0, Frame("asset")),
+        orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")),
+        frame=Frame("asset"),
+    )
+    task_4: ReturnToHome = ReturnToHome(pose=expected_robot_pose_4)
+
+    expected_tasks: List[TASKS] = [
+        task_1,
+        task_2,
+        task_3,
+        task_4,
+    ]
     mission: Mission = mission_reader.read_mission_from_file(
         Path("./tests/test_data/test_mission_working.json")
     )
 
     for expected_task, task in zip(expected_tasks, mission.tasks):
-        for expected_step, step in zip(expected_task.steps, task.steps):
-            if isinstance(expected_step, DriveToPose) and isinstance(step, DriveToPose):
-                assert expected_step.pose == step.pose
-            if isinstance(expected_step, TakeImage) and isinstance(step, TakeImage):
-                assert expected_step.target == step.target
+        if isinstance(expected_task, ReturnToHome) and isinstance(task, ReturnToHome):
+            assert expected_task.pose == task.pose
+        if isinstance(expected_task, TakeImage) and isinstance(task, TakeImage):
+            assert expected_task.target == task.target
+            assert expected_task.robot_pose == task.robot_pose
 
 
 @pytest.mark.parametrize(
@@ -114,14 +116,13 @@ def test_valid_predefined_missions_files(mission_reader) -> None:
         assert mission is not None
 
 
-def test_thermal_image_step(mission_reader) -> None:
+def test_thermal_image_task(mission_reader) -> None:
     mission_path: Path = Path("./tests/test_data/test_thermal_image_mission.json")
     output: Mission = mission_reader.read_mission_from_file(mission_path)
 
-    step: Step = output.tasks[0].steps[0]
-
-    assert isinstance(step, TakeThermalImage)
-    assert hasattr(step, "target")
-    assert step.type == "take_thermal_image"
-    assert hasattr(step, "id")
-    assert hasattr(step, "tag_id")
+    task = output.tasks[0]
+    assert isinstance(task, TakeThermalImage)
+    assert hasattr(task, "target")
+    assert task.type == "take_thermal_image"
+    assert hasattr(task, "id")
+    assert hasattr(task, "tag_id")
diff --git a/tests/isar/services/utilities/test_scheduling_utilities.py b/tests/isar/services/utilities/test_scheduling_utilities.py
index 74e40259..f60a67c7 100644
--- a/tests/isar/services/utilities/test_scheduling_utilities.py
+++ b/tests/isar/services/utilities/test_scheduling_utilities.py
@@ -24,7 +24,7 @@ def test_timeout_send_command(
 def test_robot_capable_of_mission(scheduling_utilities: SchedulingUtilities):
     assert scheduling_utilities.verify_robot_capable_of_mission(
         mission=MockMissionDefinition.default_mission,
-        robot_capabilities=["drive_to_pose", "take_image"],
+        robot_capabilities=["return_to_home", "take_image"],
     )
 
 
@@ -32,7 +32,7 @@ def test_robot_not_capable_of_mission(scheduling_utilities: SchedulingUtilities)
     with pytest.raises(HTTPException) as err:
         scheduling_utilities.verify_robot_capable_of_mission(
             mission=MockMissionDefinition.default_mission,
-            robot_capabilities=["drive_to_pose"],
+            robot_capabilities=["return_to_home"],
         )
     assert err.value.status_code == HTTPStatus.BAD_REQUEST
 
diff --git a/tests/isar/state_machine/states/test_monitor.py b/tests/isar/state_machine/states/test_monitor.py
index 2b2c3067..1e1409ff 100644
--- a/tests/isar/state_machine/states/test_monitor.py
+++ b/tests/isar/state_machine/states/test_monitor.py
@@ -2,28 +2,25 @@
 
 from isar.state_machine.states.monitor import Monitor
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus, StepStatus
-from robot_interface.models.mission.step import Step, TakeImage
-from robot_interface.models.mission.task import Task
-from tests.mocks.step import MockStep
+
+from robot_interface.models.mission.status import MissionStatus, TaskStatus
+from robot_interface.models.mission.task import ReturnToHome, TakeImage
+from tests.mocks.task import MockTask
 
 
 @pytest.mark.parametrize(
     "mock_status, expected_output",
     [
-        (StepStatus.Successful, True),
-        (StepStatus.Successful, True),
-        (StepStatus.Failed, True),
+        (TaskStatus.Successful, True),
+        (TaskStatus.Successful, True),
+        (TaskStatus.Failed, True),
     ],
 )
-def test_step_finished(monitor: Monitor, mock_status, expected_output):
-    step: Step = MockStep.drive_to()
-    step.status = mock_status
-    step_completed: bool = monitor._is_step_finished(
-        step=step,
-    )
-
-    assert step_completed == expected_output
+def test_task_finished(monitor: Monitor, mock_status, expected_output):
+    task: ReturnToHome = MockTask.return_home()
+    task.status = mock_status
+    task_completed: bool = task.is_finished()
+    assert task_completed == expected_output
 
 
 @pytest.mark.parametrize(
@@ -36,9 +33,8 @@ def test_step_finished(monitor: Monitor, mock_status, expected_output):
 def test_should_only_upload_if_status_is_completed(
     monitor: Monitor, is_status_successful, should_queue_upload
 ):
-    step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    step.status = StepStatus.Successful if is_status_successful else StepStatus.Failed
-    task: Task = Task(steps=[step])
+    task: TakeImage = MockTask.take_image()
+    task.status = TaskStatus.Successful if is_status_successful else TaskStatus.Failed
     mission: Mission = Mission(tasks=[task])
     mission.status = (
         MissionStatus.Successful if is_status_successful else MissionStatus.Failed
@@ -46,10 +42,9 @@ def test_should_only_upload_if_status_is_completed(
 
     monitor.state_machine.current_mission = mission
     monitor.state_machine.current_task = task
-    monitor.state_machine.current_step = step
 
     if monitor._should_upload_inspections():
-        monitor._queue_inspections_for_upload(mission, step)
+        monitor._queue_inspections_for_upload(mission, task)
 
     assert monitor.state_machine.queues.upload_queue.empty() == (
         not should_queue_upload
diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py
index e198c576..6cd5bacb 100644
--- a/tests/isar/state_machine/test_state_machine.py
+++ b/tests/isar/state_machine/test_state_machine.py
@@ -20,13 +20,13 @@
     RobotException,
 )
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import StepStatus
-from robot_interface.models.mission.step import DriveToPose, Step, TakeImage
+from robot_interface.models.mission.status import TaskStatus
+from robot_interface.models.mission.task import ReturnToHome, TakeImage
 from robot_interface.models.mission.task import Task
 from robot_interface.telemetry.mqtt_client import MqttClientInterface
 from tests.mocks.pose import MockPose
 from tests.mocks.robot_interface import MockRobot, MockRobotIdleToOfflineToIdleTest
-from tests.mocks.step import MockStep
+from tests.mocks.task import MockTask
 
 
 class StateMachineThread(object):
@@ -84,7 +84,6 @@ def test_send_status(state_machine) -> None:
 def test_reset_state_machine(state_machine) -> None:
     state_machine.reset_state_machine()
 
-    assert state_machine.current_step is None
     assert state_machine.current_task is None
     assert state_machine.current_mission is None
 
@@ -93,27 +92,29 @@ def test_reset_state_machine(state_machine) -> None:
 
 
 @pytest.mark.parametrize(
-    "should_run_stepwise",
+    "should_run_by_task",
     [
         (True),
         (False),
     ],
 )
 def test_state_machine_transitions(
-    injector, state_machine_thread, should_run_stepwise
+    injector, state_machine_thread, should_run_by_task
 ) -> None:
-    step_1: Step = DriveToPose(pose=MockPose.default_pose())
-    step_2: Step = TakeImage(target=MockPose.default_pose().position)
-    mission: Mission = Mission(tasks=[Task(steps=[step_1, step_2])])  # type: ignore
+    task_1: Task = TakeImage(
+        target=MockPose.default_pose().position, robot_pose=MockPose.default_pose()
+    )
+    task_2: Task = ReturnToHome(pose=MockPose.default_pose())
+    mission: Mission = Mission(tasks=[task_1, task_2])  # type: ignore
 
-    state_machine_thread.state_machine.stepwise_mission = should_run_stepwise
+    state_machine_thread.state_machine.run_mission_by_task = should_run_by_task
     state_machine_thread.start()
 
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
 
     time.sleep(3)
-    if should_run_stepwise:
+    if should_run_by_task:
         expected_transitions_list = deque(
             [
                 States.Idle,
@@ -145,12 +146,14 @@ def test_state_machine_transitions(
 def test_state_machine_transitions_when_running_full_mission(
     injector, state_machine_thread
 ) -> None:
-    state_machine_thread.state_machine.stepwise_mission = False
+    state_machine_thread.state_machine.run_mission_by_task = False
     state_machine_thread.start()
 
-    step_1: Step = DriveToPose(pose=MockPose.default_pose())
-    step_2: Step = TakeImage(target=MockPose.default_pose().position)
-    mission: Mission = Mission(tasks=[Task(steps=[step_1, step_2])])  # type: ignore
+    task_1: Task = TakeImage(
+        target=MockPose.default_pose().position, robot_pose=MockPose.default_pose()
+    )
+    task_2: Task = ReturnToHome(pose=MockPose.default_pose())
+    mission: Mission = Mission(tasks=[task_1, task_2])  # type: ignore
 
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
@@ -174,11 +177,15 @@ def test_state_machine_transitions_when_running_full_mission(
 def test_state_machine_failed_dependency(
     injector, state_machine_thread, mocker
 ) -> None:
-    drive_to_step: Step = DriveToPose(pose=MockPose.default_pose())
-    inspection_step: Step = MockStep.take_image_in_coordinate_direction()
-    mission: Mission = Mission(tasks=[Task(steps=[drive_to_step, inspection_step])])  # type: ignore
+    task_1: Task = TakeImage(
+        target=MockPose.default_pose().position, robot_pose=MockPose.default_pose()
+    )
+    task_2: Task = ReturnToHome(pose=MockPose.default_pose())
+    mission: Mission = Mission(tasks=[task_1, task_2])  # type: ignore
 
-    mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.Failed)
+    mocker.patch.object(MockRobot, "task_status", return_value=TaskStatus.Failed)
+
+    state_machine_thread.state_machine.run_mission_by_task = True
 
     state_machine_thread.start()
 
@@ -193,6 +200,8 @@ def test_state_machine_failed_dependency(
             States.Initiate,
             States.Monitor,
             States.Initiate,
+            States.Monitor,
+            States.Initiate,
             States.Idle,
         ]
     )
@@ -208,8 +217,7 @@ def test_state_machine_with_successful_collection(
 
     storage_mock: StorageInterface = injector.get(List[StorageInterface])[0]
 
-    step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    mission: Mission = Mission(tasks=[Task(steps=[step])])
+    mission: Mission = Mission(tasks=[MockTask.take_image()])
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
 
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
@@ -236,12 +244,11 @@ def test_state_machine_with_unsuccessful_collection(
 ) -> None:
     storage_mock: StorageInterface = injector.get(List[StorageInterface])[0]
 
-    mocker.patch.object(MockRobot, "get_inspections", return_value=[])
+    mocker.patch.object(MockRobot, "get_inspection", return_value=[])
 
     state_machine_thread.start()
 
-    step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    mission: Mission = Mission(tasks=[Task(steps=[step])])
+    mission: Mission = Mission(tasks=[MockTask.take_image()])
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
 
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
@@ -271,28 +278,21 @@ def test_state_machine_with_successful_mission_stop(
 ) -> None:
     state_machine_thread.start()
 
-    step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    mission: Mission = Mission(tasks=[Task(steps=[step])])
+    mission: Mission = Mission(tasks=[MockTask.take_image()])
 
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
     scheduling_utilities.start_mission(mission=mission, initial_pose=None)
     scheduling_utilities.stop_mission()
-    expected = deque(
-        [
-            States.Idle,
-            States.Initialize,
-            States.Initiate,
-            States.Stop,
-            States.Idle,
-        ]
-    )
+    time.sleep(3)
+
     actual = state_machine_thread.state_machine.transitions_list
     unexpected_log = (
         "Could not communicate request: Reached limit for stop attempts. "
         "Cancelled mission and transitioned to idle."
     )
     assert unexpected_log not in caplog.text
-    assert expected == actual
+    assert States.Idle == actual.pop()
+    assert States.Stop == actual.pop()
 
 
 def test_state_machine_with_unsuccessful_mission_stop(
@@ -301,11 +301,10 @@ def test_state_machine_with_unsuccessful_mission_stop(
     state_machine_thread: StateMachineThread,
     caplog: pytest.LogCaptureFixture,
 ) -> None:
-    step: TakeImage = MockStep.take_image_in_coordinate_direction()
-    mission: Mission = Mission(tasks=[Task(steps=[step])])
+    mission: Mission = Mission(tasks=[MockTask.take_image()])
 
     scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities)
-    mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.InProgress)
+    mocker.patch.object(MockRobot, "task_status", return_value=TaskStatus.InProgress)
     mocker.patch.object(
         MockRobot, "stop", side_effect=_mock_robot_exception_with_message
     )
diff --git a/tests/isar/storage/test_uploader.py b/tests/isar/storage/test_uploader.py
index fb1eca5a..528d5d4b 100644
--- a/tests/isar/storage/test_uploader.py
+++ b/tests/isar/storage/test_uploader.py
@@ -47,7 +47,7 @@ def uploader_thread(injector) -> UploaderThread:
 
 def test_should_upload_from_queue(uploader_thread) -> None:
     mission: Mission = Mission([])
-    inspection: Inspection = Inspection(ARBITRARY_IMAGE_METADATA)
+    inspection: Inspection = Inspection(metadata=ARBITRARY_IMAGE_METADATA)
 
     message: Tuple[Inspection, Mission] = (
         inspection,
@@ -61,7 +61,7 @@ def test_should_upload_from_queue(uploader_thread) -> None:
 
 def test_should_retry_failed_upload_from_queue(uploader_thread, mocker) -> None:
     mission: Mission = Mission([])
-    inspection: Inspection = Inspection(ARBITRARY_IMAGE_METADATA)
+    inspection: Inspection = Inspection(metadata=ARBITRARY_IMAGE_METADATA)
 
     message: Tuple[Inspection, Mission] = (
         inspection,
diff --git a/tests/mocks/mission_definition.py b/tests/mocks/mission_definition.py
index 337f32d4..546b48ff 100644
--- a/tests/mocks/mission_definition.py
+++ b/tests/mocks/mission_definition.py
@@ -3,7 +3,6 @@
     InputPose,
     InputPosition,
     StartMissionResponse,
-    StepResponse,
     TaskResponse,
 )
 from isar.apis.models.start_mission_definition import (
@@ -13,8 +12,7 @@
     StartMissionTaskDefinition,
 )
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.task import Task
-from tests.mocks.step import MockStep
+from tests.mocks.task import MockTask
 
 
 class MockMissionDefinition:
@@ -26,21 +24,13 @@ class MockMissionDefinition:
         frame_name="robot",
     )
     mock_input_target_position = InputPosition(x=5, y=5, z=5, frame_name="robot")
+    mock_task_take_image = MockTask.take_image()
+    mock_task_return_home = MockTask.return_home()
     default_mission = Mission(
         id="default_mission",
         tasks=[
-            Task(
-                steps=[
-                    MockStep.take_image_in_coordinate_direction(),
-                    MockStep.drive_to(),
-                ]
-            ),
-            Task(
-                steps=[
-                    MockStep.take_image_in_coordinate_direction(),
-                    MockStep.take_image_in_coordinate_direction(),
-                ]
-            ),
+            mock_task_take_image,
+            mock_task_return_home,
         ],
     )
     mock_start_mission_inspection_definition = StartMissionInspectionDefinition(
@@ -59,26 +49,27 @@ class MockMissionDefinition:
             id="123456",
         )
     )
+    mock_task_response_take_image = TaskResponse(
+        id=mock_task_take_image.id,
+        tag_id=mock_task_take_image.tag_id,
+        type=mock_task_take_image.type,
+    )
+
+    mock_task_response_return_home = TaskResponse(
+        id=mock_task_return_home.id,
+        tag_id=mock_task_return_home.tag_id,
+        type=mock_task_return_home.type,
+    )
     mock_start_mission_response = StartMissionResponse(
         id=default_mission.id,
-        tasks=[
-            TaskResponse(
-                id=task.id,
-                tag_id=task.tag_id,
-                steps=[
-                    StepResponse(id=step.id, type=step.__class__.__name__)
-                    for step in task.steps
-                ],
-            )
-            for task in default_mission.tasks
-        ],
+        tasks=[mock_task_response_take_image, mock_task_response_return_home],
     )
     mock_start_mission_definition = StartMissionDefinition(
         tasks=[
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
             ),
         ]
     )
@@ -87,93 +78,42 @@ class MockMissionDefinition:
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[
-                    mock_start_mission_inspection_definition_id_123,
-                    mock_start_mission_inspection_definition_id_123456,
-                ],
+                inspection=mock_start_mission_inspection_definition_id_123,
                 id="123",
             ),
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
                 id="123456",
             ),
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
                 id="123456789",
             ),
         ]
     )
-    mock_start_mission_definition_step_ids = StartMissionDefinition(
-        tasks=[
-            StartMissionTaskDefinition(
-                pose=mock_input_pose,
-                tag="dummy_tag",
-                inspections=[
-                    mock_start_mission_inspection_definition_id_123,
-                    mock_start_mission_inspection_definition_id_123456,
-                ],
-                id="123",
-            )
-        ]
-    )
     mock_start_mission_definition_with_duplicate_task_ids = StartMissionDefinition(
         tasks=[
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
                 id="123",
             ),
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
                 id="123456",
             ),
             StartMissionTaskDefinition(
                 pose=mock_input_pose,
                 tag="dummy_tag",
-                inspections=[mock_start_mission_inspection_definition],
+                inspection=mock_start_mission_inspection_definition,
                 id="123",
             ),
         ]
     )
-    mock_start_mission_definition_with_duplicate_step_ids_cross_tasks = (
-        StartMissionDefinition(
-            tasks=[
-                StartMissionTaskDefinition(
-                    pose=mock_input_pose,
-                    tag="dummy_tag",
-                    inspections=[
-                        mock_start_mission_inspection_definition_id_123,
-                        mock_start_mission_inspection_definition,
-                        mock_start_mission_inspection_definition,
-                    ],
-                ),
-                StartMissionTaskDefinition(
-                    pose=mock_input_pose,
-                    tag="dummy_tag",
-                    inspections=[
-                        mock_start_mission_inspection_definition,
-                        mock_start_mission_inspection_definition_id_123,
-                    ],
-                ),
-            ]
-        )
-    )
-    mock_start_mission_definition_with_duplicate_step_ids = StartMissionDefinition(
-        tasks=[
-            StartMissionTaskDefinition(
-                pose=mock_input_pose,
-                tag="dummy_tag",
-                inspections=[
-                    mock_start_mission_inspection_definition_id_123,
-                    mock_start_mission_inspection_definition_id_123,
-                ],
-            ),
-        ]
-    )
diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py
index c99e5be9..2c8231f4 100644
--- a/tests/mocks/robot_interface.py
+++ b/tests/mocks/robot_interface.py
@@ -1,3 +1,4 @@
+from dataclasses import field
 from datetime import datetime
 from queue import Queue
 from threading import Thread
@@ -12,16 +13,18 @@
     Inspection,
 )
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus
-from robot_interface.models.mission.step import InspectionStep, Step
+
+from robot_interface.models.mission.status import MissionStatus, RobotStatus, TaskStatus
+from robot_interface.models.mission.task import InspectionTask, Task
 from robot_interface.robot_interface import RobotInterface
 
 
 class MockRobot(RobotInterface):
+
     def __init__(
         self,
         mission_status: MissionStatus = MissionStatus.Successful,
-        step_status: StepStatus = StepStatus.Successful,
+        task_status: TaskStatus = TaskStatus.Successful,
         stop: bool = True,
         pose: Pose = Pose(
             position=Position(x=0, y=0, z=0, frame=Frame("robot")),
@@ -31,7 +34,7 @@ def __init__(
         robot_status: RobotStatus = RobotStatus.Available,
     ):
         self.mission_status_return_value: MissionStatus = mission_status
-        self.step_status_return_value: StepStatus = step_status
+        self.task_status_return_value: TaskStatus = task_status
         self.stop_return_value: bool = stop
         self.robot_pose_return_value: Pose = pose
         self.robot_status_return_value: RobotStatus = robot_status
@@ -39,14 +42,11 @@ def __init__(
     def initiate_mission(self, mission: Mission) -> None:
         return
 
-    def mission_status(self) -> MissionStatus:
-        return self.mission_status_return_value
-
-    def initiate_step(self, step: Step) -> None:
+    def initiate_task(self, task: Task) -> None:
         return
 
-    def step_status(self) -> StepStatus:
-        return self.step_status_return_value
+    def task_status(self, task_id: str) -> TaskStatus:
+        return self.task_status_return_value
 
     def stop(self) -> None:
         return
@@ -57,10 +57,10 @@ def pause(self) -> None:
     def resume(self) -> None:
         return
 
-    def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
+    def get_inspection(self, task: InspectionTask) -> Inspection:
         image: Image = Image(mock_image_metadata())
         image.data = b"Some binary image data"
-        return [image]
+        return image
 
     def initialize(self, params: InitializeParams) -> None:
         return
diff --git a/tests/mocks/step.py b/tests/mocks/step.py
deleted file mode 100644
index 3603fe05..00000000
--- a/tests/mocks/step.py
+++ /dev/null
@@ -1,14 +0,0 @@
-from alitra import Frame, Position
-
-from robot_interface.models.mission.step import DriveToPose, TakeImage
-from tests.mocks.pose import MockPose
-
-
-class MockStep:
-    @staticmethod
-    def drive_to() -> DriveToPose:
-        return DriveToPose(pose=MockPose.default_pose())
-
-    @staticmethod
-    def take_image_in_coordinate_direction() -> TakeImage:
-        return TakeImage(target=Position(x=1, y=1, z=1, frame=Frame("robot")))
diff --git a/tests/mocks/task.py b/tests/mocks/task.py
index e69de29b..5eca2f15 100644
--- a/tests/mocks/task.py
+++ b/tests/mocks/task.py
@@ -0,0 +1,16 @@
+from alitra import Frame, Position
+
+from robot_interface.models.mission.task import ReturnToHome, TakeImage
+from tests.mocks.pose import MockPose
+
+
+class MockTask:
+    @staticmethod
+    def return_home() -> ReturnToHome:
+        return ReturnToHome(pose=MockPose.default_pose())
+
+    @staticmethod
+    def take_image() -> TakeImage:
+        target_pose = Position(x=1, y=1, z=1, frame=Frame("robot"))
+        robot_pose = Position(x=0, y=0, z=1, frame=Frame("robot"))
+        return TakeImage(target=target_pose, robot_pose=robot_pose)
diff --git a/tests/test_data/test_mission_working.json b/tests/test_data/test_mission_working.json
index 70fc46b6..9c72258e 100644
--- a/tests/test_data/test_mission_working.json
+++ b/tests/test_data/test_mission_working.json
@@ -2,114 +2,92 @@
   "id": "1",
   "tasks": [
     {
-      "steps": [
-        {
-          "type": "drive_to_pose",
-          "pose": {
-            "position": {
-              "x": -2,
-              "y": -2,
-              "z": 0,
-              "frame": "asset"
-            },
-            "orientation": {
-              "x": 0,
-              "y": 0,
-              "z": 0.4794255,
-              "w": 0.8775826,
-              "frame": "asset"
-            },
-            "frame": "asset"
-          }
-        }
-      ]
+      "type": "return_to_home",
+      "pose": {
+        "position": {
+          "x": -2,
+          "y": -2,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      }
     },
     {
-      "steps": [
-        {
-          "type": "drive_to_pose",
-          "pose": {
-            "position": {
-              "x": -2,
-              "y": 2,
-              "z": 0,
-              "frame": "asset"
-            },
-            "orientation": {
-              "x": 0,
-              "y": 0,
-              "z": 0.4794255,
-              "w": 0.8775826,
-              "frame": "asset"
-            },
-            "frame": "asset"
-          }
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": -2,
+          "y": 2,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
         },
-        {
-          "type": "take_image",
-          "target": {
-            "x": 2,
-            "y": 2,
-            "z": 0,
-            "frame": "robot"
-          }
-        }
-      ]
+        "frame": "asset"
+      },
+      "target": {
+        "x": 2,
+        "y": 2,
+        "z": 0,
+        "frame": "robot"
+      }
     },
     {
-      "steps": [
-        {
-          "type": "drive_to_pose",
-          "pose": {
-            "position": {
-              "x": 2,
-              "y": 2,
-              "z": 0,
-              "frame": "asset"
-            },
-            "orientation": {
-              "x": 0,
-              "y": 0,
-              "z": 0.4794255,
-              "w": 0.8775826,
-              "frame": "asset"
-            },
-            "frame": "asset"
-          }
+      "type": "take_image",
+      "robot_pose": {
+        "position": {
+          "x": 2,
+          "y": 2,
+          "z": 0,
+          "frame": "asset"
         },
-        {
-          "type": "take_image",
-          "target": {
-            "x": 2,
-            "y": 2,
-            "z": 0,
-            "frame": "robot"
-          }
-        }
-      ]
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      },
+      "target": {
+        "x": 2,
+        "y": 2,
+        "z": 0,
+        "frame": "robot"
+      }
     },
     {
-      "steps": [
-        {
-          "type": "drive_to_pose",
-          "pose": {
-            "position": {
-              "x": 0,
-              "y": 0,
-              "z": 0,
-              "frame": "asset"
-            },
-            "orientation": {
-              "x": 0,
-              "y": 0,
-              "z": 0.4794255,
-              "w": 0.8775826,
-              "frame": "asset"
-            },
-            "frame": "asset"
-          }
-        }
-      ]
+      "type": "return_to_home",
+      "pose": {
+        "position": {
+          "x": 0,
+          "y": 0,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      }
     }
   ]
 }
diff --git a/tests/test_data/test_thermal_image_mission.json b/tests/test_data/test_thermal_image_mission.json
index b4c06921..56107918 100644
--- a/tests/test_data/test_thermal_image_mission.json
+++ b/tests/test_data/test_thermal_image_mission.json
@@ -2,17 +2,29 @@
   "id": "1",
   "tasks": [
     {
-      "steps": [
-        {
-          "type": "take_thermal_image",
-          "target": {
-            "x": 2,
-            "y": 2,
-            "z": 0,
-            "frame": "robot"
-          }
-        }
-      ]
+      "type": "take_thermal_image",
+      "target": {
+        "x": 2,
+        "y": 2,
+        "z": 0,
+        "frame": "robot"
+      },
+      "robot_pose": {
+        "position": {
+          "x": -2,
+          "y": 2,
+          "z": 0,
+          "frame": "asset"
+        },
+        "orientation": {
+          "x": 0,
+          "y": 0,
+          "z": 0.4794255,
+          "w": 0.8775826,
+          "frame": "asset"
+        },
+        "frame": "asset"
+      }
     }
   ]
 }