Skip to content

Commit

Permalink
Move definition of mission to robot interface
Browse files Browse the repository at this point in the history
  • Loading branch information
Christdej authored and aeshub committed Mar 24, 2023
1 parent 40b4978 commit 6e58c59
Show file tree
Hide file tree
Showing 38 changed files with 237 additions and 183 deletions.
5 changes: 3 additions & 2 deletions src/isar/apis/models/start_mission_definition.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,16 +6,17 @@

from isar.apis.models.models import InputPose, InputPosition
from isar.mission_planner.mission_planner_interface import MissionPlannerError
from isar.models.mission.mission import Mission, Task
from robot_interface.models.mission.mission import Mission
from robot_interface.models.mission.step import (
STEPS,
DriveToPose,
RecordAudio,
STEPS,
TakeImage,
TakeThermalImage,
TakeThermalVideo,
TakeVideo,
)
from robot_interface.models.mission.task import Task


class InspectionTypes(str, Enum):
Expand Down
45 changes: 34 additions & 11 deletions src/isar/apis/schedule/scheduling_controller.py
Original file line number Diff line number Diff line change
@@ -1,23 +1,29 @@
import logging
from http import HTTPStatus
from typing import Optional
from typing import List, Optional

from alitra import Pose
from fastapi import Body, HTTPException, Path
from injector import inject

from isar.apis.models import InputPose, StartMissionResponse
from isar.apis.models.models import ControlMissionResponse, RobotInfoResponse
from isar.apis.models.models import (
ControlMissionResponse,
RobotInfoResponse,
TaskResponse,
)
from isar.apis.models.start_mission_definition import (
StartMissionDefinition,
to_isar_mission,
)
from isar.config.settings import robot_settings, settings
from isar.mission_planner.mission_planner_interface import MissionPlannerError
from isar.models.mission import Mission, Task
from isar.models.mission_metadata.mission_metadata import MissionMetadata
from isar.services.utilities.scheduling_utilities import SchedulingUtilities
from isar.state_machine.states_enum import States
from robot_interface.models.mission import DriveToPose
from robot_interface.models.mission.mission import Mission
from robot_interface.models.mission.step import DriveToPose
from robot_interface.models.mission.task import Task


class SchedulingController:
Expand Down Expand Up @@ -70,10 +76,11 @@ def start_mission_by_id(
)

self.logger.info(f"Starting mission with ISAR Mission ID: '{mission.id}'")
metadata: MissionMetadata = MissionMetadata(mission.id)
self.scheduling_utilities.start_mission(
mission=mission, initial_pose=initial_pose_alitra
mission=mission, initial_pose=initial_pose_alitra, mission_metadata=metadata
)
return mission.api_response()
return self._api_response(mission)

def start_mission(
self,
Expand Down Expand Up @@ -132,11 +139,12 @@ def start_mission(
initial_pose.to_alitra_pose() if initial_pose else None
)

metadata: MissionMetadata = MissionMetadata(mission.id)
self.logger.info(f"Starting mission: {mission.id}")
self.scheduling_utilities.start_mission(
mission=mission, initial_pose=initial_pose_alitra
mission=mission, mission_metadata=metadata, initial_pose=initial_pose_alitra
)
return mission.api_response()
return self._api_response(mission)

def pause_mission(self) -> ControlMissionResponse:
self.logger.info("Received request to pause current mission")
Expand Down Expand Up @@ -212,12 +220,14 @@ def drive_to(
pose: Pose = target_pose.to_alitra_pose()
step: DriveToPose = DriveToPose(pose=pose)
mission: Mission = Mission(tasks=[Task(steps=[step])])

metadata: MissionMetadata = MissionMetadata(mission.id)
self.logger.info(
f"Starting drive to mission with ISAR Mission ID: '{mission.id}'"
)
self.scheduling_utilities.start_mission(mission=mission, initial_pose=None)
return mission.api_response()
self.scheduling_utilities.start_mission(
mission=mission, initial_pose=None, mission_metadata=metadata
)
return self._api_response(mission)

def get_info(self):
return RobotInfoResponse(
Expand All @@ -228,3 +238,16 @@ def get_info(self):
robot_capabilities=robot_settings.CAPABILITIES,
plant_short_name=settings.STID_PLANT_NAME,
)

def _api_response(self, mission: Mission) -> StartMissionResponse:
return StartMissionResponse(
id=mission.id,
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)
12 changes: 9 additions & 3 deletions src/isar/mission_planner/echo_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,12 +13,18 @@
MissionPlannerError,
MissionPlannerInterface,
)
from isar.models.mission import Mission, Task
from isar.services.auth.azure_credentials import AzureCredentials
from isar.services.service_connections.request_handler import RequestHandler
from isar.services.service_connections.stid.stid_service import StidService
from robot_interface.models.mission import DriveToPose, TakeImage, TakeThermalImage
from robot_interface.models.mission.step import TakeThermalVideo, TakeVideo
from robot_interface.models.mission.mission import Mission
from robot_interface.models.mission.step import (
DriveToPose,
TakeImage,
TakeThermalImage,
TakeThermalVideo,
TakeVideo,
)
from robot_interface.models.mission.task import Task


class EchoPlanner(MissionPlannerInterface):
Expand Down
3 changes: 1 addition & 2 deletions src/isar/mission_planner/local_planner.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@
MissionPlannerError,
MissionPlannerInterface,
)
from isar.models.mission import Mission
from isar.services.readers.base_reader import BaseReader, BaseReaderError
from robot_interface.models.mission.mission import Mission

logger = logging.getLogger("api")

Expand All @@ -27,7 +27,6 @@ def get_mission(self, mission_id) -> Mission:
raise MissionPlannerError("There were no predefined missions")
try:
mission: Mission = missions[mission_id]["mission"]
mission.set_unique_id_and_metadata()
return mission
except KeyError as e:
raise MissionNotFoundError(
Expand Down
2 changes: 1 addition & 1 deletion src/isar/mission_planner/mission_planner_interface.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
from abc import ABCMeta, abstractmethod

from isar.models.mission import Mission
from robot_interface.models.mission.mission import Mission


class MissionPlannerInterface(metaclass=ABCMeta):
Expand Down
2 changes: 1 addition & 1 deletion src/isar/mission_planner/sequential_task_selector.py
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
TaskSelectorInterface,
TaskSelectorStop,
)
from isar.models.mission import Task
from robot_interface.models.mission.task import Task


class SequentialTaskSelector(TaskSelectorInterface):
Expand Down
2 changes: 1 addition & 1 deletion src/isar/mission_planner/task_selector_interface.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
from abc import ABCMeta, abstractmethod
from typing import List

from isar.models.mission import Task
from robot_interface.models.mission.task import Task


class TaskSelectorInterface(metaclass=ABCMeta):
Expand Down
4 changes: 3 additions & 1 deletion src/isar/models/communication/message.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,12 @@

from alitra import Pose

from isar.models.mission import Mission
from isar.models.mission_metadata.mission_metadata import MissionMetadata
from robot_interface.models.mission.mission import Mission


@dataclass
class StartMissionMessage:
mission: Mission
mission_metadata: MissionMetadata
initial_pose: Optional[Pose]
2 changes: 0 additions & 2 deletions src/isar/models/mission/__init__.py

This file was deleted.

21 changes: 0 additions & 21 deletions src/isar/models/mission/status.py

This file was deleted.

2 changes: 1 addition & 1 deletion src/isar/models/mission_metadata/mission_metadata.py
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
from dataclasses import dataclass
from datetime import date, datetime
from typing import Optional, Union
from typing import Optional

from isar.config.settings import settings

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -90,4 +90,4 @@ def run(self) -> None:
self.logger.warning(
"Failed to get robot status due to a communication exception"
)
time.sleep(settings.ROBOT_API_STATUS_POLL_INTERVAL)
time.sleep(settings.ROBOT_API_STATUS_POLL_INTERVAL)
16 changes: 12 additions & 4 deletions src/isar/services/utilities/scheduling_utilities.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,10 +17,11 @@
MissionPlannerInterface,
)
from isar.models.communication.message import StartMissionMessage
from isar.models.communication.queues import QueueIO, Queues, QueueTimeoutError
from isar.models.mission.mission import Mission
from isar.models.communication.queues import QueueIO, QueueTimeoutError, Queues
from isar.models.mission_metadata.mission_metadata import MissionMetadata
from isar.services.utilities.queue_utilities import QueueUtilities
from isar.state_machine.states_enum import States
from robot_interface.models.mission.mission import Mission


class SchedulingUtilities:
Expand Down Expand Up @@ -135,7 +136,12 @@ def verify_state_machine_ready_to_receive_mission(self, state: States) -> bool:

return is_state_machine_ready_to_receive_mission

def start_mission(self, mission: Mission, initial_pose: Optional[Pose]) -> None:
def start_mission(
self,
mission: Mission,
mission_metadata: MissionMetadata,
initial_pose: Optional[Pose],
) -> None:
"""Start mission
Raises
Expand All @@ -146,7 +152,9 @@ def start_mission(self, mission: Mission, initial_pose: Optional[Pose]) -> None:
try:
self._send_command(
StartMissionMessage(
mission=deepcopy(mission), initial_pose=initial_pose
mission=deepcopy(mission),
mission_metadata=deepcopy(mission_metadata),
initial_pose=initial_pose,
),
self.queues.start_mission,
)
Expand Down
13 changes: 9 additions & 4 deletions src/isar/state_machine/state_machine.py
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,7 @@
)
from isar.models.communication.message import StartMissionMessage
from isar.models.communication.queues.queues import Queues
from isar.models.mission import Mission, Task
from isar.models.mission.status import MissionStatus, TaskStatus
from isar.models.mission_metadata.mission_metadata import MissionMetadata
from isar.state_machine.states import (
Idle,
Initialize,
Expand All @@ -32,8 +31,10 @@
)
from isar.state_machine.states_enum import States
from robot_interface.models.initialize.initialize_params import InitializeParams
from robot_interface.models.mission import StepStatus
from robot_interface.models.mission.mission import Mission
from robot_interface.models.mission.status import MissionStatus, StepStatus, TaskStatus
from robot_interface.models.mission.step import Step
from robot_interface.models.mission.task import Task
from robot_interface.robot_interface import RobotInterface
from robot_interface.telemetry.mqtt_client import MqttClientInterface
from robot_interface.utilities.json_service import EnhancedJSONEncoder
Expand Down Expand Up @@ -194,6 +195,7 @@ def __init__(

self.stopped: bool = False
self.current_mission: Optional[Mission] = None
self.current_mission_metadata: Optional[MissionMetadata] = None
self.current_task: Optional[Task] = None
self.current_step: Optional[Step] = None
self.initial_pose: Optional[Pose] = None
Expand Down Expand Up @@ -394,9 +396,12 @@ def reset_state_machine(self) -> None:
self.current_mission = None
self.initial_pose = None

def start_mission(self, mission: Mission, initial_pose: Pose):
def start_mission(
self, mission: Mission, mission_metadata: MissionMetadata, initial_pose: Pose
):
"""Starts a scheduled mission."""
self.current_mission = mission
self.current_mission_metadata = mission_metadata
self.initial_pose = initial_pose

self.task_selector.initialize(tasks=self.current_mission.tasks)
Expand Down
1 change: 1 addition & 0 deletions src/isar/state_machine/states/idle.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ def _run(self) -> None:
if start_mission:
self.state_machine.start_mission(
mission=start_mission.mission,
mission_metadata=start_mission.mission_metadata,
initial_pose=start_mission.initial_pose,
)
transition = self.state_machine.mission_started # type: ignore
Expand Down
4 changes: 2 additions & 2 deletions src/isar/state_machine/states/monitor.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
)
from robot_interface.models.exceptions import RobotException
from robot_interface.models.inspection.inspection import Inspection
from robot_interface.models.mission import InspectionStep, Step, StepStatus
from robot_interface.models.mission.step import InspectionStep, Step, StepStatus

if TYPE_CHECKING:
from isar.state_machine.state_machine import StateMachine
Expand Down Expand Up @@ -99,7 +99,7 @@ def _queue_inspections_for_upload(self, current_step: InspectionStep) -> None:
# A deepcopy is made to freeze the metadata before passing it to another thread
# through the queue
mission_metadata: MissionMetadata = deepcopy(
self.state_machine.current_mission.metadata
self.state_machine.current_mission_metadata
)

for inspection in inspections:
Expand Down
4 changes: 2 additions & 2 deletions src/isar/storage/slimm_storage.py
Original file line number Diff line number Diff line change
Expand Up @@ -116,7 +116,7 @@ def _construct_multiform_request_image(
"ImageMetadata.Timestamp": inspection.metadata.start_time.isoformat(), # noqa: E501
"ImageMetadata.X": str(inspection.metadata.pose.position.x),
"ImageMetadata.Y": str(inspection.metadata.pose.position.y),
"ImageMetadata.Y": str(inspection.metadata.pose.position.z),
"ImageMetadata.Z": str(inspection.metadata.pose.position.z),
"ImageMetadata.CameraOrientation1": str(array_of_orientation[0]),
"ImageMetadata.CameraOrientation2": str(array_of_orientation[1]),
"ImageMetadata.CameraOrientation3": str(array_of_orientation[2]),
Expand Down Expand Up @@ -158,7 +158,7 @@ def _construct_multiform_request_video(
"VideoMetadata.Duration": str(inspection.metadata.duration), # type: ignore
"VideoMetadata.X": str(inspection.metadata.pose.position.x),
"VideoMetadata.Y": str(inspection.metadata.pose.position.y),
"VideoMetadata.Y": str(inspection.metadata.pose.position.z),
"VideoMetadata.Z": str(inspection.metadata.pose.position.z),
"VideoMetadata.CameraOrientation1": str(array_of_orientation[0]),
"VideoMetadata.CameraOrientation2": str(array_of_orientation[1]),
"VideoMetadata.CameraOrientation3": str(array_of_orientation[2]),
Expand Down
7 changes: 2 additions & 5 deletions src/isar/storage/utilities.py
Original file line number Diff line number Diff line change
@@ -1,12 +1,9 @@
import json
import logging
from pathlib import Path
from typing import Any, Tuple
from typing import Tuple

from isar.models.mission_metadata.mission_metadata import MissionMetadata
from isar.storage.storage_interface import StorageException
from robot_interface.models.inspection import ThermalVideo, Video
from robot_interface.models.inspection.inspection import Image, Inspection, ThermalImage
from robot_interface.models.inspection.inspection import Inspection
from robot_interface.utilities.json_service import EnhancedJSONEncoder


Expand Down
2 changes: 1 addition & 1 deletion src/robot_interface/models/inspection/inspection.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@

from alitra import Pose

from isar.services.utilities.uuid_string_factory import uuid4_string
from robot_interface.utilities.uuid_string_factory import uuid4_string


@dataclass
Expand Down
Loading

0 comments on commit 6e58c59

Please sign in to comment.