From e7bbe9bc230884771acf0794b7a69677d0ee7f66 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Wed, 25 Sep 2024 17:06:21 +0200 Subject: [PATCH 01/12] Remove isar steps --- src/isar/apis/models/models.py | 9 +- .../apis/models/start_mission_definition.py | 183 +++--- .../apis/schedule/scheduling_controller.py | 28 +- .../service_connections/mqtt/mqtt_client.py | 2 +- .../utilities/scheduling_utilities.py | 7 +- src/isar/state_machine/state_machine.py | 107 +--- src/isar/state_machine/states/initialize.py | 2 +- src/isar/state_machine/states/initiate.py | 14 +- src/isar/state_machine/states/monitor.py | 116 ++-- src/isar/state_machine/states/stop.py | 2 +- .../models/inspection/inspection.py | 2 +- src/robot_interface/models/mission/status.py | 8 - src/robot_interface/models/mission/step.py | 234 -------- src/robot_interface/models/mission/task.py | 266 +++++---- src/robot_interface/robot_interface.py | 11 +- .../turtlebot/test_successful_mission.py | 6 +- tests/isar/mission/test_mission.py | 2 +- .../models/test_start_mission_definition.py | 2 +- .../isar/services/readers/test_base_reader.py | 2 +- .../services/readers/test_mission_reader.py | 2 +- .../isar/state_machine/states/test_monitor.py | 105 ++-- .../isar/state_machine/test_state_machine.py | 531 +++++++++--------- tests/mocks/robot_interface.py | 169 +++--- tests/mocks/step.py | 2 +- 24 files changed, 755 insertions(+), 1057 deletions(-) delete mode 100644 src/robot_interface/models/mission/step.py diff --git a/src/isar/apis/models/models.py b/src/isar/apis/models/models.py index 93d211f7..dd6fd47f 100644 --- a/src/isar/apis/models/models.py +++ b/src/isar/apis/models/models.py @@ -4,15 +4,10 @@ from pydantic import BaseModel, Field -class StepResponse(BaseModel): - id: str - type: str - - class TaskResponse(BaseModel): id: str tag_id: Optional[str] = None - steps: List[StepResponse] + type: str class StartMissionResponse(BaseModel): @@ -25,8 +20,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..f09e1df9 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,10 @@ 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 from robot_interface.models.mission.mission import Mission -from robot_interface.models.mission.step import ( - STEPS, +from robot_interface.models.mission.task import ( DockingProcedure, - DriveToPose, Localize, RecordAudio, ReturnToHome, @@ -34,7 +33,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 +50,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 +64,35 @@ class StartMissionDefinition(BaseModel): undock: Optional[bool] = None -def to_isar_mission(mission_definition: StartMissionDefinition) -> Mission: +def to_isar_mission(start_mission_definition: StartMissionDefinition) -> Mission: isar_tasks: List[Task] = [] - all_steps_in_mission: List[STEPS] = [] - 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: Task = 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 +114,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[Task]): duplicate_ids = get_duplicate_ids(items=items) if len(duplicate_ids) > 0: raise MissionPlannerError( @@ -130,97 +123,97 @@ 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) -> Task: - 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 create_inspection_task( + start_mission_task_definition: StartMissionTaskDefinition, +) -> Task: -def generate_steps_for_inspection_task(task: StartMissionTaskDefinition) -> List[STEPS]: - drive_step: DriveToPose = DriveToPose(pose=task.pose.to_alitra_pose()) + inspection = Inspection( + id=start_mission_task_definition.inspection.id, + metadata=start_mission_task_definition.inspection.metadata, + ) - 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(), + inspection=inspection, + tag_id=start_mission_task_definition.tag, + robot_pose=start_mission_task_definition.pose.to_alitra_pose(), + ) + 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, + inspection=inspection, + tag_id=start_mission_task_definition.tag, + robot_pose=start_mission_task_definition.pose.to_alitra_pose(), ) - 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(), + inspection=inspection, + tag_id=start_mission_task_definition.tag, + robot_pose=start_mission_task_definition.pose.to_alitra_pose(), + ) + 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, + inspection=inspection, + tag_id=start_mission_task_definition.tag, + robot_pose=start_mission_task_definition.pose.to_alitra_pose(), + ) -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, + inspection=inspection, + tag_id=start_mission_task_definition.tag, + robot_pose=start_mission_task_definition.pose.to_alitra_pose(), + ) + 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[Task]) -> 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..abb1225e 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,7 @@ 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 ( Localize, MoveArm, ReturnToHome, @@ -67,8 +66,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 +134,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 +208,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 +218,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 +242,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 +289,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}'" @@ -323,8 +317,4 @@ def _api_response(self, mission: Mission) -> StartMissionResponse: ) 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) + return TaskResponse(id=task.id, tag_id=task.tag_id, type=task.type) 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..16568334 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -35,10 +35,8 @@ 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.robot_interface import RobotInterface from robot_interface.telemetry.mqtt_client import MqttClientInterface @@ -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", @@ -235,7 +233,6 @@ 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.initial_pose: Optional[Pose] = None self.current_state: State = States(self.state) # type: ignore @@ -256,13 +253,13 @@ def _initialization_failed(self) -> None: def _initiated(self) -> None: if self.stepwise_mission: - self.current_step.status = StepStatus.InProgress + 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,8 +288,8 @@ def _resume(self) -> None: ) self.queues.resume_mission.output.put(resume_mission_response) - self.current_task.reset_task() - self.iterate_current_step() + # self.current_task.reset_task() + # self.iterate_current_step() self.robot.resume() @@ -327,7 +324,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 +334,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) + def _task_finished(self) -> None: + self.publish_task_status(step=self.current_task) self.current_task.update_task_status() - self.publish_task_status(task=self.current_task) self.iterate_current_task() - self.iterate_current_step() def _full_mission_finished(self) -> None: self.current_task = None @@ -353,7 +347,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 +355,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 +362,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() + 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 +390,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 +424,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 +435,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 @@ -555,6 +523,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 +540,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 +572,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 +589,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..b2f1d732 100644 --- a/src/isar/state_machine/states/initiate.py +++ b/src/isar/state_machine/states/initiate.py @@ -63,9 +63,9 @@ def _run(self) -> None: if not self.initiate_thread: if self.state_machine.stepwise_mission: 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( @@ -82,12 +82,12 @@ def _run(self) -> None: time.sleep(self.state_machine.sleep_time) continue except RobotInfeasibleStepException 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.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..e148f8f2 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 @@ -65,54 +66,53 @@ def _run(self) -> None: 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( @@ -120,22 +120,21 @@ def _run(self) -> None: ) get_inspections_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.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._is_task_finished(self.state_machine.current_task): + self._report_task_status(self.state_machine.current_task) 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 + # Report finished task self.state_machine.publish_task_status( task=self.state_machine.current_task ) @@ -152,28 +151,23 @@ def _run(self) -> None: 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 + 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) + self.state_machine.robot.get_inspections(task=current_task) ) except (RobotRetrieveInspectionException, RobotException) as e: @@ -185,11 +179,11 @@ def _queue_inspections_for_upload( if not inspections: 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, @@ -198,37 +192,37 @@ def _queue_inspections_for_upload( 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: + def _is_task_finished(self, task: Task) -> bool: finished: bool = False - if step.status == StepStatus.Failed: + if task.status == TaskStatus.Failed: finished = True - elif step.status == StepStatus.Successful: + elif task.status == TaskStatus.Successful: finished = True return finished - def _report_step_status(self, step: Step) -> None: - if step.status == StepStatus.Failed: + 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"Step: {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 + task: Task = self.state_machine.current_task return ( - self._is_step_finished(step) - and step.status == StepStatus.Successful - and isinstance(step, InspectionStep) + self._is_task_finished(task) + and task.status == TaskStatus.Successful + and isinstance(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 +230,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 +241,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/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/inspection/inspection.py b/src/robot_interface/models/inspection/inspection.py index 5a45e345..f3e808be 100644 --- a/src/robot_interface/models/inspection/inspection.py +++ b/src/robot_interface/models/inspection/inspection.py @@ -45,7 +45,7 @@ class AudioMetadata(InspectionMetadata): @dataclass class Inspection: - id: str = field(default_factory=uuid4_string, init=False) + id: str metadata: InspectionMetadata data: Optional[bytes] = field(default=None, init=False) 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..60d72710 100644 --- a/src/robot_interface/models/mission/task.py +++ b/src/robot_interface/models/mission/task.py @@ -1,130 +1,172 @@ from dataclasses import dataclass, field -from typing import Iterator, List, Optional +from typing import Iterator, List, Literal, Optional, Type + +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 @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 + if ( + self.status == TaskStatus.Successful + or self.status == TaskStatus.PartiallySuccessful + or self.status == TaskStatus.Cancelled + ): + return True + return False 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) + 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) + tag_id: Optional[str] = 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["docking_procedure"] = "docking_procedure" + + +@dataclass +class ReturnToHome(Task): + """ + Task which cases the robot to return home + """ + + pose: Pose = field(default=None, init=True) + type: Literal["return_to_home"] = "return_to_home" + + +@dataclass +class Localize(Task): + """ + Task which causes the robot to localize + """ + + localization_pose: Pose = field(default=None, init=True) + type: Literal["localize"] = "localize" + + +@dataclass +class MoveArm(Task): + """ + Task which causes the robot to move its arm + """ + + arm_pose: str = field(default=None, init=True) + type: Literal["move_arm"] = "move_arm" + + +@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["take_image"] = "take_image" + + @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["take_thermal_image"] = "take_thermal_image" + + @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["take_video"] = "take_video" + + @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["take_thermal_video"] = "take_thermal_video" + + @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["record_audio"] = "record_audio" + + @staticmethod + def get_inspection_type() -> Type[Inspection]: + return Audio diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 2d00d641..bbb50c3e 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -6,8 +6,9 @@ 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 MissionStatus, RobotStatus, TaskStatus +from robot_interface.models.mission.task import InspectionTask +from robot_interface.models.mission.task import Task class RobotInterface(metaclass=ABCMeta): @@ -69,7 +70,7 @@ def mission_status(self) -> MissionStatus: """ @abstractmethod - def initiate_step(self, step: Step) -> None: + def initiate_task(self, task: Task) -> None: """Send a step to the robot and initiate the execution of the step This function should be used in combination with the step_status function @@ -101,7 +102,7 @@ def initiate_step(self, step: Step) -> None: raise NotImplementedError @abstractmethod - def step_status(self) -> StepStatus: + def task_status(self, task: Task) -> TaskStatus: """Gets the status of the currently active step on robot. This function should be used in combination with the initiate_step function @@ -181,7 +182,7 @@ def resume(self) -> None: raise NotImplementedError @abstractmethod - def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: + def get_inspections(self, task: InspectionTask) -> Sequence[Inspection]: """Return the inspections connected to the given step. Parameters diff --git a/tests/integration/turtlebot/test_successful_mission.py b/tests/integration/turtlebot/test_successful_mission.py index 9159db30..0d62e982 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,11 +110,11 @@ def test_successful_mission( mission_result_folder: Path = Path(f"tests/results/{mission_id}") - drive_to_steps: List[DriveToPose] = [ + drive_to_steps: List[ReturnToHome] = [ step for task in mission.tasks for step in task.steps - if isinstance(step, DriveToPose) + if isinstance(step, ReturnToHome) ] expected_positions: list = [step.pose.position for step in drive_to_steps] diff --git a/tests/isar/mission/test_mission.py b/tests/isar/mission/test_mission.py index d7e47e60..d43f0291 100644 --- a/tests/isar/mission/test_mission.py +++ b/tests/isar/mission/test_mission.py @@ -2,7 +2,7 @@ from isar.services.readers.base_reader import BaseReader from robot_interface.models.mission.mission import Mission, Task -from robot_interface.models.mission.step import ( +from robot_interface.models.mission.task_action import ( STEPS, DriveToPose, TakeImage, diff --git a/tests/isar/models/test_start_mission_definition.py b/tests/isar/models/test_start_mission_definition.py index c5a4b3a0..630c63a7 100644 --- a/tests/isar/models/test_start_mission_definition.py +++ b/tests/isar/models/test_start_mission_definition.py @@ -11,7 +11,7 @@ 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_action import STEPS, Step from robot_interface.models.mission.task import Task task_1: Task = Task([], tag_id=None, id="123") diff --git a/tests/isar/services/readers/test_base_reader.py b/tests/isar/services/readers/test_base_reader.py index 7dc562dc..a1394f45 100644 --- a/tests/isar/services/readers/test_base_reader.py +++ b/tests/isar/services/readers/test_base_reader.py @@ -6,7 +6,7 @@ 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 robot_interface.models.mission.task_action import Step from tests.mocks.mission_definition import MockMissionDefinition from tests.mocks.pose import MockPose from tests.mocks.step import MockStep diff --git a/tests/isar/services/readers/test_mission_reader.py b/tests/isar/services/readers/test_mission_reader.py index 088063b7..25ad2b9e 100644 --- a/tests/isar/services/readers/test_mission_reader.py +++ b/tests/isar/services/readers/test_mission_reader.py @@ -6,7 +6,7 @@ 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 ( +from robot_interface.models.mission.task_action import ( DriveToPose, Step, TakeImage, diff --git a/tests/isar/state_machine/states/test_monitor.py b/tests/isar/state_machine/states/test_monitor.py index 2b2c3067..bbbb4078 100644 --- a/tests/isar/state_machine/states/test_monitor.py +++ b/tests/isar/state_machine/states/test_monitor.py @@ -2,55 +2,56 @@ 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 - - -@pytest.mark.parametrize( - "mock_status, expected_output", - [ - (StepStatus.Successful, True), - (StepStatus.Successful, True), - (StepStatus.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 - - -@pytest.mark.parametrize( - "is_status_successful, should_queue_upload", - [ - (True, True), - (False, False), - ], -) -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]) - mission: Mission = Mission(tasks=[task]) - mission.status = ( - MissionStatus.Successful if is_status_successful else MissionStatus.Failed - ) - - 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) - - assert monitor.state_machine.queues.upload_queue.empty() == ( - not should_queue_upload - ) + +# from robot_interface.models.mission.status import MissionStatus, StepStatus +# from robot_interface.models.mission.task_type import Step, TakeImage +# from robot_interface.models.mission.task import Task +# from tests.mocks.step import MockStep + + +# @pytest.mark.parametrize( +# "mock_status, expected_output", +# [ +# (StepStatus.Successful, True), +# (StepStatus.Successful, True), +# (StepStatus.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 + + +# @pytest.mark.parametrize( +# "is_status_successful, should_queue_upload", +# [ +# (True, True), +# (False, False), +# ], +# ) +# 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]) +# mission: Mission = Mission(tasks=[task]) +# mission.status = ( +# MissionStatus.Successful if is_status_successful else MissionStatus.Failed +# ) + +# 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) + +# 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..3f4efc87 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -20,8 +20,8 @@ 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_action import DriveToPose, TakeImage from robot_interface.models.mission.task import Task from robot_interface.telemetry.mqtt_client import MqttClientInterface from tests.mocks.pose import MockPose @@ -84,271 +84,270 @@ 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 -empty_mission: Mission = Mission([], None) - - -@pytest.mark.parametrize( - "should_run_stepwise", - [ - (True), - (False), - ], -) -def test_state_machine_transitions( - injector, state_machine_thread, should_run_stepwise -) -> 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 - - state_machine_thread.state_machine.stepwise_mission = should_run_stepwise - 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: - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - else: - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -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.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 - - scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission, initial_pose=None) - - time.sleep(3) - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -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 - - mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.Failed) - - state_machine_thread.start() - - scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - scheduling_utilities.start_mission(mission=mission, initial_pose=None) - - time.sleep(3) - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -def test_state_machine_with_successful_collection( - injector, state_machine_thread, uploader_thread -) -> None: - state_machine_thread.start() - - storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] - - step: TakeImage = MockStep.take_image_in_coordinate_direction() - mission: Mission = Mission(tasks=[Task(steps=[step])]) - scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - - scheduling_utilities.start_mission(mission=mission, initial_pose=None) - time.sleep(3) - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - expected_stored_items = 1 - assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -def test_state_machine_with_unsuccessful_collection( - injector, mocker, state_machine_thread -) -> None: - storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] - - mocker.patch.object(MockRobot, "get_inspections", return_value=[]) - - state_machine_thread.start() - - step: TakeImage = MockStep.take_image_in_coordinate_direction() - mission: Mission = Mission(tasks=[Task(steps=[step])]) - scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - - scheduling_utilities.start_mission(mission=mission, initial_pose=None) - time.sleep(3) - expected_transitions_list = deque( - [ - States.Idle, - States.Initialize, - States.Initiate, - States.Monitor, - States.Initiate, - States.Idle, - ] - ) - expected_stored_items = 0 - assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore - print(state_machine_thread.state_machine.transitions_list) - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -def test_state_machine_with_successful_mission_stop( - injector: Injector, - state_machine_thread: StateMachineThread, - caplog: pytest.LogCaptureFixture, -) -> None: - state_machine_thread.start() - - step: TakeImage = MockStep.take_image_in_coordinate_direction() - mission: Mission = Mission(tasks=[Task(steps=[step])]) - - 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, - ] - ) - 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 - - -def test_state_machine_with_unsuccessful_mission_stop( - injector: Injector, - mocker: MockerFixture, - state_machine_thread: StateMachineThread, - caplog: pytest.LogCaptureFixture, -) -> None: - step: TakeImage = MockStep.take_image_in_coordinate_direction() - mission: Mission = Mission(tasks=[Task(steps=[step])]) - - scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.InProgress) - mocker.patch.object( - MockRobot, "stop", side_effect=_mock_robot_exception_with_message - ) - - state_machine_thread.start() - - 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, - ] - ) - actual = state_machine_thread.state_machine.transitions_list - expected_log = ( - "Be aware that the robot may still be " - "moving even though a stop has been attempted" - ) - assert expected_log in caplog.text - assert expected == actual - - -def test_state_machine_idle_to_offline_to_idle(state_machine_thread) -> None: - state_machine_thread.state_machine.robot = MockRobotIdleToOfflineToIdleTest() - - state_machine_thread.start() - # Robot status check happens every 5 seconds by default - time.sleep(13) # Ensure that robot_status have been called again in Idle - - expected_transitions_list = deque([States.Idle, States.Offline, States.Idle]) - assert ( - state_machine_thread.state_machine.transitions_list == expected_transitions_list - ) - - -def _mock_robot_exception_with_message() -> RobotException: - raise RobotException( - error_reason=ErrorReason.RobotUnknownErrorException, - error_description="This is an example error description", - ) +# empty_mission: Mission = Mission([], None) + + +# @pytest.mark.parametrize( +# "should_run_stepwise", +# [ +# (True), +# (False), +# ], +# ) +# def test_state_machine_transitions( +# injector, state_machine_thread, should_run_stepwise +# ) -> 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 + +# state_machine_thread.state_machine.stepwise_mission = should_run_stepwise +# 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: +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# else: +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# 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.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 + +# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) +# scheduling_utilities.start_mission(mission=mission, initial_pose=None) + +# time.sleep(3) +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# 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 + +# mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.Failed) + +# state_machine_thread.start() + +# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) +# scheduling_utilities.start_mission(mission=mission, initial_pose=None) + +# time.sleep(3) +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# def test_state_machine_with_successful_collection( +# injector, state_machine_thread, uploader_thread +# ) -> None: +# state_machine_thread.start() + +# storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] + +# step: TakeImage = MockStep.take_image_in_coordinate_direction() +# mission: Mission = Mission(tasks=[Task(steps=[step])]) +# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + +# scheduling_utilities.start_mission(mission=mission, initial_pose=None) +# time.sleep(3) +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# expected_stored_items = 1 +# assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# def test_state_machine_with_unsuccessful_collection( +# injector, mocker, state_machine_thread +# ) -> None: +# storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] + +# mocker.patch.object(MockRobot, "get_inspections", return_value=[]) + +# state_machine_thread.start() + +# step: TakeImage = MockStep.take_image_in_coordinate_direction() +# mission: Mission = Mission(tasks=[Task(steps=[step])]) +# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + +# scheduling_utilities.start_mission(mission=mission, initial_pose=None) +# time.sleep(3) +# expected_transitions_list = deque( +# [ +# States.Idle, +# States.Initialize, +# States.Initiate, +# States.Monitor, +# States.Initiate, +# States.Idle, +# ] +# ) +# expected_stored_items = 0 +# assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore +# print(state_machine_thread.state_machine.transitions_list) +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# def test_state_machine_with_successful_mission_stop( +# injector: Injector, +# state_machine_thread: StateMachineThread, +# caplog: pytest.LogCaptureFixture, +# ) -> None: +# state_machine_thread.start() + +# step: TakeImage = MockStep.take_image_in_coordinate_direction() +# mission: Mission = Mission(tasks=[Task(steps=[step])]) + +# 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, +# ] +# ) +# 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 + + +# def test_state_machine_with_unsuccessful_mission_stop( +# injector: Injector, +# mocker: MockerFixture, +# state_machine_thread: StateMachineThread, +# caplog: pytest.LogCaptureFixture, +# ) -> None: +# step: TakeImage = MockStep.take_image_in_coordinate_direction() +# mission: Mission = Mission(tasks=[Task(steps=[step])]) + +# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) +# mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.InProgress) +# mocker.patch.object( +# MockRobot, "stop", side_effect=_mock_robot_exception_with_message +# ) + +# state_machine_thread.start() + +# 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, +# ] +# ) +# actual = state_machine_thread.state_machine.transitions_list +# expected_log = ( +# "Be aware that the robot may still be " +# "moving even though a stop has been attempted" +# ) +# assert expected_log in caplog.text +# assert expected == actual + + +# def test_state_machine_idle_to_offline_to_idle(state_machine_thread) -> None: +# state_machine_thread.state_machine.robot = MockRobotIdleToOfflineToIdleTest() + +# state_machine_thread.start() +# # Robot status check happens every 5 seconds by default +# time.sleep(13) # Ensure that robot_status have been called again in Idle + +# expected_transitions_list = deque([States.Idle, States.Offline, States.Idle]) +# assert ( +# state_machine_thread.state_machine.transitions_list == expected_transitions_list +# ) + + +# def _mock_robot_exception_with_message() -> RobotException: +# raise RobotException( +# error_reason=ErrorReason.RobotUnknownErrorException, +# error_description="This is an example error description", +# ) diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index c99e5be9..0be40824 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -12,87 +12,88 @@ 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.robot_interface import RobotInterface - - -class MockRobot(RobotInterface): - def __init__( - self, - mission_status: MissionStatus = MissionStatus.Successful, - step_status: StepStatus = StepStatus.Successful, - stop: bool = True, - pose: Pose = Pose( - position=Position(x=0, y=0, z=0, frame=Frame("robot")), - orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame("robot")), - frame=Frame("robot"), - ), - robot_status: RobotStatus = RobotStatus.Available, - ): - self.mission_status_return_value: MissionStatus = mission_status - self.step_status_return_value: StepStatus = step_status - self.stop_return_value: bool = stop - self.robot_pose_return_value: Pose = pose - self.robot_status_return_value: RobotStatus = robot_status - - 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: - return - - def step_status(self) -> StepStatus: - return self.step_status_return_value - - def stop(self) -> None: - return - - def pause(self) -> None: - return - - def resume(self) -> None: - return - - def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: - image: Image = Image(mock_image_metadata()) - image.data = b"Some binary image data" - return [image] - - def initialize(self, params: InitializeParams) -> None: - return - - def get_telemetry_publishers( - self, queue: Queue, isar_id: str, robot_name: str - ) -> List[Thread]: - return [] - - def robot_status(self) -> RobotStatus: - return self.robot_status_return_value - - -def mock_image_metadata() -> ImageMetadata: - return ImageMetadata( - start_time=datetime.now(), - pose=Pose( - Position(0, 0, 0, Frame("robot")), - Orientation(0, 0, 0, 1, Frame("robot")), - Frame("robot"), - ), - file_type="jpg", - ) - - -class MockRobotIdleToOfflineToIdleTest(MockRobot): - def __init__(self): - self.first = True - - def robot_status(self) -> RobotStatus: - if self.first: - self.first = False - return RobotStatus.Offline - - return RobotStatus.Available + +# from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus +# from robot_interface.models.mission.task_type import InspectionStep, Step +# from robot_interface.robot_interface import RobotInterface + + +# class MockRobot(RobotInterface): +# def __init__( +# self, +# mission_status: MissionStatus = MissionStatus.Successful, +# step_status: StepStatus = StepStatus.Successful, +# stop: bool = True, +# pose: Pose = Pose( +# position=Position(x=0, y=0, z=0, frame=Frame("robot")), +# orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame("robot")), +# frame=Frame("robot"), +# ), +# robot_status: RobotStatus = RobotStatus.Available, +# ): +# self.mission_status_return_value: MissionStatus = mission_status +# self.step_status_return_value: StepStatus = step_status +# self.stop_return_value: bool = stop +# self.robot_pose_return_value: Pose = pose +# self.robot_status_return_value: RobotStatus = robot_status + +# 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: +# return + +# def step_status(self) -> StepStatus: +# return self.step_status_return_value + +# def stop(self) -> None: +# return + +# def pause(self) -> None: +# return + +# def resume(self) -> None: +# return + +# def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: +# image: Image = Image(mock_image_metadata()) +# image.data = b"Some binary image data" +# return [image] + +# def initialize(self, params: InitializeParams) -> None: +# return + +# def get_telemetry_publishers( +# self, queue: Queue, isar_id: str, robot_name: str +# ) -> List[Thread]: +# return [] + +# def robot_status(self) -> RobotStatus: +# return self.robot_status_return_value + + +# def mock_image_metadata() -> ImageMetadata: +# return ImageMetadata( +# start_time=datetime.now(), +# pose=Pose( +# Position(0, 0, 0, Frame("robot")), +# Orientation(0, 0, 0, 1, Frame("robot")), +# Frame("robot"), +# ), +# file_type="jpg", +# ) + + +# class MockRobotIdleToOfflineToIdleTest(MockRobot): +# def __init__(self): +# self.first = True + +# def robot_status(self) -> RobotStatus: +# if self.first: +# self.first = False +# return RobotStatus.Offline + +# return RobotStatus.Available diff --git a/tests/mocks/step.py b/tests/mocks/step.py index 3603fe05..78db6cb0 100644 --- a/tests/mocks/step.py +++ b/tests/mocks/step.py @@ -1,6 +1,6 @@ from alitra import Frame, Position -from robot_interface.models.mission.step import DriveToPose, TakeImage +from robot_interface.models.mission.task_action import DriveToPose, TakeImage from tests.mocks.pose import MockPose From 461c0d9763d96c1f1b639b43efd9b56915364273 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Tue, 8 Oct 2024 10:32:50 +0200 Subject: [PATCH 02/12] Create TaskType string enum --- src/isar/apis/models/models.py | 4 ++- src/robot_interface/models/mission/task.py | 33 +++++++++++++++------- src/robot_interface/robot_interface.py | 2 +- 3 files changed, 27 insertions(+), 12 deletions(-) diff --git a/src/isar/apis/models/models.py b/src/isar/apis/models/models.py index dd6fd47f..48289016 100644 --- a/src/isar/apis/models/models.py +++ b/src/isar/apis/models/models.py @@ -3,11 +3,13 @@ from alitra import Frame, Orientation, Pose, Position from pydantic import BaseModel, Field +from robot_interface.models.mission.task import TaskTypes + class TaskResponse(BaseModel): id: str tag_id: Optional[str] = None - type: str + type: TaskTypes class StartMissionResponse(BaseModel): diff --git a/src/robot_interface/models/mission/task.py b/src/robot_interface/models/mission/task.py index 60d72710..1f34f25d 100644 --- a/src/robot_interface/models/mission/task.py +++ b/src/robot_interface/models/mission/task.py @@ -1,5 +1,6 @@ from dataclasses import dataclass, field -from typing import Iterator, List, Literal, Optional, Type +from enum import StrEnum +from typing import Iterator, Literal, Optional, Type from alitra import Pose, Position @@ -16,6 +17,18 @@ from robot_interface.utilities.uuid_string_factory import uuid4_string +class TaskTypes(StrEnum): + 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: status: TaskStatus = field(default=TaskStatus.NotStarted, init=False) @@ -60,7 +73,7 @@ class DockingProcedure(Task): """ behavior: Literal["dock", "undock"] = field(default=None, init=True) - type: Literal["docking_procedure"] = "docking_procedure" + type: str = TaskTypes.DockingProcedure @dataclass @@ -70,7 +83,7 @@ class ReturnToHome(Task): """ pose: Pose = field(default=None, init=True) - type: Literal["return_to_home"] = "return_to_home" + type: str = TaskTypes.ReturnToHome @dataclass @@ -80,7 +93,7 @@ class Localize(Task): """ localization_pose: Pose = field(default=None, init=True) - type: Literal["localize"] = "localize" + type: str = TaskTypes.Localize @dataclass @@ -90,7 +103,7 @@ class MoveArm(Task): """ arm_pose: str = field(default=None, init=True) - type: Literal["move_arm"] = "move_arm" + type: str = TaskTypes.MoveArm @dataclass @@ -100,7 +113,7 @@ class TakeImage(InspectionTask): """ target: Position = field(default=None, init=True) - type: Literal["take_image"] = "take_image" + type: str = TaskTypes.TakeImage @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -114,7 +127,7 @@ class TakeThermalImage(InspectionTask): """ target: Position = field(default=None, init=True) - type: Literal["take_thermal_image"] = "take_thermal_image" + type: str = TaskTypes.TakeThermalImage @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -131,7 +144,7 @@ class TakeVideo(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: Literal["take_video"] = "take_video" + type: str = TaskTypes.TakeVideo @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -148,7 +161,7 @@ class TakeThermalVideo(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: Literal["take_thermal_video"] = "take_thermal_video" + type: str = TaskTypes.TakeThermalVideo @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -165,7 +178,7 @@ class RecordAudio(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: Literal["record_audio"] = "record_audio" + type: str = TaskTypes.RecordAudio @staticmethod def get_inspection_type() -> Type[Inspection]: diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index bbb50c3e..0efb34dc 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -102,7 +102,7 @@ def initiate_task(self, task: Task) -> None: raise NotImplementedError @abstractmethod - def task_status(self, task: Task) -> TaskStatus: + def task_status(self, task_id: str) -> TaskStatus: """Gets the status of the currently active step on robot. This function should be used in combination with the initiate_step function From a92c687bff88f6398c541f92ecfd832e7f5133a0 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Tue, 8 Oct 2024 10:35:49 +0200 Subject: [PATCH 03/12] Simplify get inspections function --- src/robot_interface/robot_interface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 0efb34dc..15ec0618 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -182,7 +182,7 @@ def resume(self) -> None: raise NotImplementedError @abstractmethod - def get_inspections(self, task: InspectionTask) -> Sequence[Inspection]: + def get_inspections(self, task: InspectionTask) -> Inspection: """Return the inspections connected to the given step. Parameters From d44f0f968d40e316b9965dabde71067303ba48d6 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Tue, 8 Oct 2024 11:49:39 +0200 Subject: [PATCH 04/12] Remove mission status from robot interface --- src/robot_interface/robot_interface.py | 27 +------------------------- tests/mocks/robot_interface.py | 3 --- 2 files changed, 1 insertion(+), 29 deletions(-) diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 15ec0618..6453110a 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -1,7 +1,7 @@ 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 @@ -44,31 +44,6 @@ def initiate_mission(self, mission: Mission) -> None: """ 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_task(self, task: Task) -> None: """Send a step to the robot and initiate the execution of the step diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index 0be40824..352b15d1 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -40,9 +40,6 @@ # 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: # return From 5cc5338f4a60e8957b36cb840942f1a235e83676 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Tue, 8 Oct 2024 11:57:01 +0200 Subject: [PATCH 05/12] Rename stepwise mission to by task mission --- src/isar/config/settings.env | 2 +- src/isar/config/settings.py | 4 ++-- src/isar/script.py | 2 +- src/isar/state_machine/state_machine.py | 8 ++++---- src/isar/state_machine/states/initiate.py | 2 +- src/isar/state_machine/states/monitor.py | 4 ++-- src/isar/state_machine/states/paused.py | 2 +- tests/isar/state_machine/test_state_machine.py | 4 ++-- 8 files changed, 14 insertions(+), 14 deletions(-) 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..29f5dee9 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") @@ -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/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/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 16568334..fe58470a 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -54,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, ): @@ -82,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) @@ -252,7 +252,7 @@ def _initialization_failed(self) -> None: self._finalize() def _initiated(self) -> None: - if self.stepwise_mission: + if self.run_mission_by_task: self.current_task.status = TaskStatus.InProgress self.current_mission.status = MissionStatus.InProgress self.publish_task_status(task=self.current_task) @@ -368,7 +368,7 @@ def _initiate_failed(self) -> None: self._finalize() def _initiate_infeasible(self) -> None: - if self.stepwise_mission: + if self.run_mission_by_task: self.current_task.status = TaskStatus.Failed self.publish_task_status(task=self.current_task) self.iterate_current_task() diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py index b2f1d732..2212612f 100644 --- a/src/isar/state_machine/states/initiate.py +++ b/src/isar/state_machine/states/initiate.py @@ -61,7 +61,7 @@ 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_task, function_argument=self.state_machine.current_task, diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index e148f8f2..4bc6b1c9 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -60,7 +60,7 @@ 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 @@ -124,7 +124,7 @@ def _run(self) -> None: name="State Machine Get Inspections", ) - if self.state_machine.stepwise_mission: + 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 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/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 3f4efc87..d0d16cde 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -105,7 +105,7 @@ def test_reset_state_machine(state_machine) -> None: # step_2: Step = TakeImage(target=MockPose.default_pose().position) # mission: Mission = Mission(tasks=[Task(steps=[step_1, step_2])]) # type: ignore -# state_machine_thread.state_machine.stepwise_mission = should_run_stepwise +# state_machine_thread.state_machine.run_mission_by_task = should_run_stepwise # state_machine_thread.start() # scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) @@ -144,7 +144,7 @@ def test_reset_state_machine(state_machine) -> None: # 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()) From 33cb25eeb637c099bad63fc959a351d37ceb552c Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Tue, 8 Oct 2024 15:23:45 +0200 Subject: [PATCH 06/12] Improve monitor.py --- src/isar/state_machine/state_machine.py | 2 +- src/isar/state_machine/states/monitor.py | 45 +++++++++--------------- 2 files changed, 17 insertions(+), 30 deletions(-) diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index fe58470a..d7e56163 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -336,7 +336,7 @@ def _mission_started(self) -> None: self.publish_task_status(task=self.current_task) def _task_finished(self) -> None: - self.publish_task_status(step=self.current_task) + self.publish_task_status(task=self.current_task) self.current_task.update_task_status() self.iterate_current_task() diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 4bc6b1c9..18646453 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -130,26 +130,22 @@ def _run(self) -> None: transition = self.state_machine.task_finished # type: ignore break else: - if self._is_task_finished(self.state_machine.current_task): + 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 + ) - if self.state_machine.current_task.is_finished(): - # Report finished 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 + 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 - ) + # 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) @@ -192,14 +188,6 @@ def _queue_inspections_for_upload( self.state_machine.queues.upload_queue.put(message) self.logger.info(f"Inspection: {str(inspection.id)[:8]} queued for upload") - def _is_task_finished(self, task: Task) -> bool: - finished: bool = False - if task.status == TaskStatus.Failed: - finished = True - elif task.status == TaskStatus.Successful: - finished = True - return finished - def _report_task_status(self, task: Task) -> None: if task.status == TaskStatus.Failed: self.logger.warning( @@ -211,11 +199,10 @@ def _report_task_status(self, task: Task) -> None: ) def _should_upload_inspections(self) -> bool: - task: Task = self.state_machine.current_task return ( - self._is_task_finished(task) - and task.status == TaskStatus.Successful - and isinstance(task, InspectionTask) + 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: From e384e61801ee6a4ab872281be591b858c3fe8021 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Wed, 9 Oct 2024 16:07:02 +0200 Subject: [PATCH 07/12] Fix tests --- .../config/predefined_missions/default.json | 158 +++--- .../default_turtlebot.json | 208 ++++--- src/isar/config/settings.py | 2 +- src/isar/mission_planner/local_planner.py | 49 ++ src/isar/state_machine/states/monitor.py | 23 +- .../models/inspection/inspection.py | 2 +- src/robot_interface/models/mission/task.py | 7 +- .../turtlebot/test_successful_mission.py | 9 +- .../apis/scheduler/test_scheduler_router.py | 56 +- tests/isar/mission/test_mission.py | 184 +++--- .../models/example_mission_definition.json | 73 ++- .../models/test_start_mission_definition.py | 51 +- .../isar/services/readers/test_base_reader.py | 10 +- .../services/readers/test_mission_reader.py | 92 ++- .../utilities/test_scheduling_utilities.py | 4 +- .../isar/state_machine/states/test_monitor.py | 98 ++-- .../isar/state_machine/test_state_machine.py | 530 +++++++++--------- tests/isar/storage/test_uploader.py | 4 +- tests/mocks/mission_definition.py | 108 +--- tests/mocks/robot_interface.py | 164 +++--- tests/mocks/step.py | 14 - tests/mocks/task.py | 16 + tests/test_data/test_mission_working.json | 178 +++--- .../test_data/test_thermal_image_mission.json | 34 +- 24 files changed, 961 insertions(+), 1113 deletions(-) delete mode 100644 tests/mocks/step.py 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.py b/src/isar/config/settings.py index 29f5dee9..bcc94c4f 100644 --- a/src/isar/config/settings.py +++ b/src/isar/config/settings.py @@ -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 diff --git a/src/isar/mission_planner/local_planner.py b/src/isar/mission_planner/local_planner.py index 2b1c80a0..2b8e7819 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 from alitra import Frame from injector import inject @@ -12,6 +13,18 @@ ) from isar.services.readers.base_reader import BaseReader, BaseReaderError from robot_interface.models.mission.mission import Mission +from robot_interface.models.mission.task import ( + DockingProcedure, + Localize, + MoveArm, + RecordAudio, + ReturnToHome, + TakeImage, + TakeThermalImage, + TakeThermalVideo, + TakeVideo, + Task, +) logger = logging.getLogger("api") @@ -38,6 +51,40 @@ 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_tasks: List[Task] = [] + task_dataclass: Task = None + + for task in mission_dict["tasks"]: + if task["type"] == "return_to_home": + task_dataclass = ReturnToHome + elif task["type"] == "localize": + task_dataclass = Localize + elif task["type"] == "move_arm": + task_dataclass = MoveArm + elif task["type"] == "take_image": + task_dataclass = TakeImage + elif task["type"] == "take_thermal_image": + task_dataclass = TakeThermalImage + elif task["type"] == "take_video": + task_dataclass = TakeVideo + elif task["type"] == "take_thermal_video": + task_dataclass = TakeThermalVideo + elif task["type"] == "record_audio": + task_dataclass = RecordAudio + elif task["type"] == "docking_procedure": + task_dataclass = DockingProcedure + + if task_dataclass: + task: Task = BaseReader.dict_to_dataclass( + dataclass_dict=task, + target_dataclass=task_dataclass, + cast_config=[Frame], + strict_config=True, + ) + mission_tasks.append(task) + + mission_dict["tasks"] = [] mission: Mission = BaseReader.dict_to_dataclass( dataclass_dict=mission_dict, target_dataclass=Mission, @@ -45,6 +92,8 @@ def read_mission_from_file(mission_path: Path) -> Mission: strict_config=True, ) + mission.tasks = mission_tasks + return mission def get_predefined_missions(self) -> dict: diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 18646453..1e569f70 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -162,8 +162,8 @@ def _queue_inspections_for_upload( self, mission: Mission, current_task: InspectionTask ) -> None: try: - inspections: Sequence[Inspection] = ( - self.state_machine.robot.get_inspections(task=current_task) + inspection: Inspection = self.state_machine.robot.get_inspections( + task=current_task ) except (RobotRetrieveInspectionException, RobotException) as e: @@ -173,25 +173,24 @@ def _queue_inspections_for_upload( ) return - if not inspections: + if not inspection: self.logger.warning( f"No inspection data retrieved for task {str(current_task.id)[:8]}" ) - for inspection in inspections: - inspection.metadata.tag_id = current_task.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") + 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(task.id)[:8]} was reported as failed by the robot" + f"Task: {str(task.id)[:8]} was reported as failed by the robot" ) elif task.status == TaskStatus.Successful: self.logger.info( diff --git a/src/robot_interface/models/inspection/inspection.py b/src/robot_interface/models/inspection/inspection.py index f3e808be..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 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/task.py b/src/robot_interface/models/mission/task.py index 1f34f25d..ccc9d8c0 100644 --- a/src/robot_interface/models/mission/task.py +++ b/src/robot_interface/models/mission/task.py @@ -1,5 +1,5 @@ from dataclasses import dataclass, field -from enum import StrEnum +from enum import Enum from typing import Iterator, Literal, Optional, Type from alitra import Pose, Position @@ -17,7 +17,7 @@ from robot_interface.utilities.uuid_string_factory import uuid4_string -class TaskTypes(StrEnum): +class TaskTypes(str, Enum): ReturnToHome = "return_to_home" Localize = "localize" MoveArm = "move_arm" @@ -35,13 +35,13 @@ class Task: 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 is_finished(self) -> bool: if ( self.status == TaskStatus.Successful or self.status == TaskStatus.PartiallySuccessful or self.status == TaskStatus.Cancelled + or self.status == TaskStatus.Failed ): return True return False @@ -57,7 +57,6 @@ class InspectionTask(Task): """ inspection: Inspection = field(default=None, init=True) - tag_id: Optional[str] = field(default=None, init=True) robot_pose: Pose = field(default=None, init=True) metadata: Optional[dict] = field(default_factory=dict, init=True) diff --git a/tests/integration/turtlebot/test_successful_mission.py b/tests/integration/turtlebot/test_successful_mission.py index 0d62e982..fadd51b2 100644 --- a/tests/integration/turtlebot/test_successful_mission.py +++ b/tests/integration/turtlebot/test_successful_mission.py @@ -110,14 +110,11 @@ def test_successful_mission( mission_result_folder: Path = Path(f"tests/results/{mission_id}") - drive_to_steps: List[ReturnToHome] = [ - step - for task in mission.tasks - for step in task.steps - if isinstance(step, ReturnToHome) + 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..0c036f27 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_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) @@ -220,21 +211,10 @@ def test_mission_with_duplicate_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_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): + def test_mission_with_duplicate_task_ids(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 - ), + json=jsonable_encoder(self.mock_start_mission_duplicate_task_ids_content), ) assert response.status_code == HTTPStatus.BAD_REQUEST diff --git a/tests/isar/mission/test_mission.py b/tests/isar/mission/test_mission.py index d43f0291..c7807516 100644 --- a/tests/isar/mission/test_mission.py +++ b/tests/isar/mission/test_mission.py @@ -2,40 +2,40 @@ from isar.services.readers.base_reader import BaseReader from robot_interface.models.mission.mission import Mission, Task -from robot_interface.models.mission.task_action import ( - STEPS, - DriveToPose, +from robot_interface.models.mission.task import ( 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 +45,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 +118,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 @@ -154,18 +131,3 @@ def test_mission_definition() -> None: 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 630c63a7..a8dd75c2 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.task_action import STEPS, Step from robot_interface.models.mission.task import 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[Task], 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 a1394f45..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.task_action 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 25ad2b9e..c7ec1436 100644 --- a/tests/isar/services/readers/test_mission_reader.py +++ b/tests/isar/services/readers/test_mission_reader.py @@ -6,9 +6,8 @@ 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.task_action import ( - DriveToPose, - Step, +from robot_interface.models.mission.task import ( + ReturnToHome, TakeImage, TakeThermalImage, ) @@ -28,43 +27,39 @@ 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: Task = 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: Task = TakeImage( + target=expected_inspection_target_1, robot_pose=expected_robot_pose_2 + ) + + 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_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_inspection_target_2 = Position(2, 2, 0, Frame("robot")) + task_3: Task = TakeImage( + target=expected_inspection_target_2, robot_pose=expected_robot_pose_3 ) - 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_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: Task = Task(steps=[expected_step_6]) + task_4: Task = ReturnToHome(pose=expected_robot_pose_4) expected_tasks = [task_1, task_2, task_3, task_4] mission: Mission = mission_reader.read_mission_from_file( @@ -72,11 +67,11 @@ def test_read_mission_from_file(mission_reader) -> None: ) 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): + assert expected_task.pose == task.pose + if isinstance(expected_task, TakeImage): + assert expected_task.target == task.target + assert expected_task.robot_pose == task.robot_pose @pytest.mark.parametrize( @@ -114,14 +109,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 bbbb4078..c3c9025a 100644 --- a/tests/isar/state_machine/states/test_monitor.py +++ b/tests/isar/state_machine/states/test_monitor.py @@ -3,55 +3,49 @@ 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.task_type import Step, TakeImage -# from robot_interface.models.mission.task import Task -# from tests.mocks.step import MockStep - - -# @pytest.mark.parametrize( -# "mock_status, expected_output", -# [ -# (StepStatus.Successful, True), -# (StepStatus.Successful, True), -# (StepStatus.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 - - -# @pytest.mark.parametrize( -# "is_status_successful, should_queue_upload", -# [ -# (True, True), -# (False, False), -# ], -# ) -# 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]) -# mission: Mission = Mission(tasks=[task]) -# mission.status = ( -# MissionStatus.Successful if is_status_successful else MissionStatus.Failed -# ) - -# 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) - -# assert monitor.state_machine.queues.upload_queue.empty() == ( -# not should_queue_upload -# ) +from robot_interface.models.mission.status import MissionStatus, TaskStatus +from robot_interface.models.mission.task import Task +from tests.mocks.task import MockTask + + +@pytest.mark.parametrize( + "mock_status, expected_output", + [ + (TaskStatus.Successful, True), + (TaskStatus.Successful, True), + (TaskStatus.Failed, True), + ], +) +def test_task_finished(monitor: Monitor, mock_status, expected_output): + task: Task = MockTask.return_home() + task.status = mock_status + task_completed: bool = task.is_finished() + assert task_completed == expected_output + + +@pytest.mark.parametrize( + "is_status_successful, should_queue_upload", + [ + (True, True), + (False, False), + ], +) +def test_should_only_upload_if_status_is_completed( + monitor: Monitor, is_status_successful, should_queue_upload +): + task: Task = 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 + ) + + monitor.state_machine.current_mission = mission + monitor.state_machine.current_task = task + + if monitor._should_upload_inspections(): + 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 d0d16cde..68bfb1a8 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -21,12 +21,12 @@ ) from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.status import TaskStatus -from robot_interface.models.mission.task_action import DriveToPose, TakeImage +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): @@ -88,266 +88,266 @@ def test_reset_state_machine(state_machine) -> None: assert state_machine.current_mission is None -# empty_mission: Mission = Mission([], None) - - -# @pytest.mark.parametrize( -# "should_run_stepwise", -# [ -# (True), -# (False), -# ], -# ) -# def test_state_machine_transitions( -# injector, state_machine_thread, should_run_stepwise -# ) -> 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 - -# state_machine_thread.state_machine.run_mission_by_task = should_run_stepwise -# 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: -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# else: -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# def test_state_machine_transitions_when_running_full_mission( -# injector, state_machine_thread -# ) -> None: -# 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 - -# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) -# scheduling_utilities.start_mission(mission=mission, initial_pose=None) - -# time.sleep(3) -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# 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 - -# mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.Failed) - -# state_machine_thread.start() - -# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) -# scheduling_utilities.start_mission(mission=mission, initial_pose=None) - -# time.sleep(3) -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# def test_state_machine_with_successful_collection( -# injector, state_machine_thread, uploader_thread -# ) -> None: -# state_machine_thread.start() - -# storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] - -# step: TakeImage = MockStep.take_image_in_coordinate_direction() -# mission: Mission = Mission(tasks=[Task(steps=[step])]) -# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - -# scheduling_utilities.start_mission(mission=mission, initial_pose=None) -# time.sleep(3) -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# expected_stored_items = 1 -# assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# def test_state_machine_with_unsuccessful_collection( -# injector, mocker, state_machine_thread -# ) -> None: -# storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] - -# mocker.patch.object(MockRobot, "get_inspections", return_value=[]) - -# state_machine_thread.start() - -# step: TakeImage = MockStep.take_image_in_coordinate_direction() -# mission: Mission = Mission(tasks=[Task(steps=[step])]) -# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) - -# scheduling_utilities.start_mission(mission=mission, initial_pose=None) -# time.sleep(3) -# expected_transitions_list = deque( -# [ -# States.Idle, -# States.Initialize, -# States.Initiate, -# States.Monitor, -# States.Initiate, -# States.Idle, -# ] -# ) -# expected_stored_items = 0 -# assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore -# print(state_machine_thread.state_machine.transitions_list) -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# def test_state_machine_with_successful_mission_stop( -# injector: Injector, -# state_machine_thread: StateMachineThread, -# caplog: pytest.LogCaptureFixture, -# ) -> None: -# state_machine_thread.start() - -# step: TakeImage = MockStep.take_image_in_coordinate_direction() -# mission: Mission = Mission(tasks=[Task(steps=[step])]) - -# 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, -# ] -# ) -# 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 - - -# def test_state_machine_with_unsuccessful_mission_stop( -# injector: Injector, -# mocker: MockerFixture, -# state_machine_thread: StateMachineThread, -# caplog: pytest.LogCaptureFixture, -# ) -> None: -# step: TakeImage = MockStep.take_image_in_coordinate_direction() -# mission: Mission = Mission(tasks=[Task(steps=[step])]) - -# scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) -# mocker.patch.object(MockRobot, "step_status", return_value=StepStatus.InProgress) -# mocker.patch.object( -# MockRobot, "stop", side_effect=_mock_robot_exception_with_message -# ) - -# state_machine_thread.start() - -# 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, -# ] -# ) -# actual = state_machine_thread.state_machine.transitions_list -# expected_log = ( -# "Be aware that the robot may still be " -# "moving even though a stop has been attempted" -# ) -# assert expected_log in caplog.text -# assert expected == actual - - -# def test_state_machine_idle_to_offline_to_idle(state_machine_thread) -> None: -# state_machine_thread.state_machine.robot = MockRobotIdleToOfflineToIdleTest() - -# state_machine_thread.start() -# # Robot status check happens every 5 seconds by default -# time.sleep(13) # Ensure that robot_status have been called again in Idle - -# expected_transitions_list = deque([States.Idle, States.Offline, States.Idle]) -# assert ( -# state_machine_thread.state_machine.transitions_list == expected_transitions_list -# ) - - -# def _mock_robot_exception_with_message() -> RobotException: -# raise RobotException( -# error_reason=ErrorReason.RobotUnknownErrorException, -# error_description="This is an example error description", -# ) +empty_mission: Mission = Mission([], None) + + +@pytest.mark.parametrize( + "should_run_by_task", + [ + (True), + (False), + ], +) +def test_state_machine_transitions( + injector, state_machine_thread, should_run_by_task +) -> None: + 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.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_by_task: + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + else: + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def test_state_machine_transitions_when_running_full_mission( + injector, state_machine_thread +) -> None: + state_machine_thread.state_machine.run_mission_by_task = False + state_machine_thread.start() + + 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) + + time.sleep(3) + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def test_state_machine_failed_dependency( + injector, state_machine_thread, mocker +) -> None: + 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, "task_status", return_value=TaskStatus.Failed) + + state_machine_thread.state_machine.run_mission_by_task = True + + state_machine_thread.start() + + scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + scheduling_utilities.start_mission(mission=mission, initial_pose=None) + + time.sleep(10) + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def test_state_machine_with_successful_collection( + injector, state_machine_thread, uploader_thread +) -> None: + state_machine_thread.start() + + storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] + + mission: Mission = Mission(tasks=[MockTask.take_image()]) + scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + + scheduling_utilities.start_mission(mission=mission, initial_pose=None) + time.sleep(10) + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + expected_stored_items = 1 + assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def test_state_machine_with_unsuccessful_collection( + injector, mocker, state_machine_thread +) -> None: + storage_mock: StorageInterface = injector.get(List[StorageInterface])[0] + + mocker.patch.object(MockRobot, "get_inspections", return_value=[]) + + state_machine_thread.start() + + mission: Mission = Mission(tasks=[MockTask.take_image()]) + scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + + scheduling_utilities.start_mission(mission=mission, initial_pose=None) + time.sleep(3) + expected_transitions_list = deque( + [ + States.Idle, + States.Initialize, + States.Initiate, + States.Monitor, + States.Initiate, + States.Idle, + ] + ) + expected_stored_items = 0 + assert len(storage_mock.stored_inspections) == expected_stored_items # type: ignore + print(state_machine_thread.state_machine.transitions_list) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def test_state_machine_with_successful_mission_stop( + injector: Injector, + state_machine_thread: StateMachineThread, + caplog: pytest.LogCaptureFixture, +) -> None: + state_machine_thread.start() + + 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() + 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 States.Idle == actual.pop() + assert States.Stop == actual.pop() + + +def test_state_machine_with_unsuccessful_mission_stop( + injector: Injector, + mocker: MockerFixture, + state_machine_thread: StateMachineThread, + caplog: pytest.LogCaptureFixture, +) -> None: + mission: Mission = Mission(tasks=[MockTask.take_image()]) + + scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + mocker.patch.object(MockRobot, "task_status", return_value=TaskStatus.InProgress) + mocker.patch.object( + MockRobot, "stop", side_effect=_mock_robot_exception_with_message + ) + + state_machine_thread.start() + + 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, + ] + ) + actual = state_machine_thread.state_machine.transitions_list + expected_log = ( + "Be aware that the robot may still be " + "moving even though a stop has been attempted" + ) + assert expected_log in caplog.text + assert expected == actual + + +def test_state_machine_idle_to_offline_to_idle(state_machine_thread) -> None: + state_machine_thread.state_machine.robot = MockRobotIdleToOfflineToIdleTest() + + state_machine_thread.start() + # Robot status check happens every 5 seconds by default + time.sleep(13) # Ensure that robot_status have been called again in Idle + + expected_transitions_list = deque([States.Idle, States.Offline, States.Idle]) + assert ( + state_machine_thread.state_machine.transitions_list == expected_transitions_list + ) + + +def _mock_robot_exception_with_message() -> RobotException: + raise RobotException( + error_reason=ErrorReason.RobotUnknownErrorException, + error_description="This is an example error description", + ) 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 352b15d1..e21bd142 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 @@ -13,84 +14,85 @@ ) from robot_interface.models.mission.mission import Mission -# from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus -# from robot_interface.models.mission.task_type import InspectionStep, Step -# from robot_interface.robot_interface import RobotInterface - - -# class MockRobot(RobotInterface): -# def __init__( -# self, -# mission_status: MissionStatus = MissionStatus.Successful, -# step_status: StepStatus = StepStatus.Successful, -# stop: bool = True, -# pose: Pose = Pose( -# position=Position(x=0, y=0, z=0, frame=Frame("robot")), -# orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame("robot")), -# frame=Frame("robot"), -# ), -# robot_status: RobotStatus = RobotStatus.Available, -# ): -# self.mission_status_return_value: MissionStatus = mission_status -# self.step_status_return_value: StepStatus = step_status -# self.stop_return_value: bool = stop -# self.robot_pose_return_value: Pose = pose -# self.robot_status_return_value: RobotStatus = robot_status - -# def initiate_mission(self, mission: Mission) -> None: -# return - -# def initiate_step(self, step: Step) -> None: -# return - -# def step_status(self) -> StepStatus: -# return self.step_status_return_value - -# def stop(self) -> None: -# return - -# def pause(self) -> None: -# return - -# def resume(self) -> None: -# return - -# def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]: -# image: Image = Image(mock_image_metadata()) -# image.data = b"Some binary image data" -# return [image] - -# def initialize(self, params: InitializeParams) -> None: -# return - -# def get_telemetry_publishers( -# self, queue: Queue, isar_id: str, robot_name: str -# ) -> List[Thread]: -# return [] - -# def robot_status(self) -> RobotStatus: -# return self.robot_status_return_value - - -# def mock_image_metadata() -> ImageMetadata: -# return ImageMetadata( -# start_time=datetime.now(), -# pose=Pose( -# Position(0, 0, 0, Frame("robot")), -# Orientation(0, 0, 0, 1, Frame("robot")), -# Frame("robot"), -# ), -# file_type="jpg", -# ) - - -# class MockRobotIdleToOfflineToIdleTest(MockRobot): -# def __init__(self): -# self.first = True - -# def robot_status(self) -> RobotStatus: -# if self.first: -# self.first = False -# return RobotStatus.Offline - -# return RobotStatus.Available +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, + task_status: TaskStatus = TaskStatus.Successful, + stop: bool = True, + pose: Pose = Pose( + position=Position(x=0, y=0, z=0, frame=Frame("robot")), + orientation=Orientation(x=0, y=0, z=0, w=1, frame=Frame("robot")), + frame=Frame("robot"), + ), + robot_status: RobotStatus = RobotStatus.Available, + ): + self.mission_status_return_value: MissionStatus = mission_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 + + def initiate_mission(self, mission: Mission) -> None: + return + + def initiate_task(self, task: Task) -> None: + return + + def task_status(self, task_id: str) -> TaskStatus: + return self.task_status_return_value + + def stop(self) -> None: + return + + def pause(self) -> None: + return + + def resume(self) -> None: + return + + def get_inspections(self, task: InspectionTask) -> Inspection: + image: Image = Image(mock_image_metadata()) + image.data = b"Some binary image data" + return image + + def initialize(self, params: InitializeParams) -> None: + return + + def get_telemetry_publishers( + self, queue: Queue, isar_id: str, robot_name: str + ) -> List[Thread]: + return [] + + def robot_status(self) -> RobotStatus: + return self.robot_status_return_value + + +def mock_image_metadata() -> ImageMetadata: + return ImageMetadata( + start_time=datetime.now(), + pose=Pose( + Position(0, 0, 0, Frame("robot")), + Orientation(0, 0, 0, 1, Frame("robot")), + Frame("robot"), + ), + file_type="jpg", + ) + + +class MockRobotIdleToOfflineToIdleTest(MockRobot): + def __init__(self): + self.first = True + + def robot_status(self) -> RobotStatus: + if self.first: + self.first = False + return RobotStatus.Offline + + return RobotStatus.Available diff --git a/tests/mocks/step.py b/tests/mocks/step.py deleted file mode 100644 index 78db6cb0..00000000 --- a/tests/mocks/step.py +++ /dev/null @@ -1,14 +0,0 @@ -from alitra import Frame, Position - -from robot_interface.models.mission.task_action 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" + } } ] } From 696eb739f19017080a3974fe32297b0a8bc7ac1e Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Thu, 10 Oct 2024 10:48:42 +0200 Subject: [PATCH 08/12] Rename get_inspections to singular --- src/isar/state_machine/states/monitor.py | 6 +++--- src/robot_interface/robot_interface.py | 2 +- tests/isar/state_machine/test_state_machine.py | 2 +- tests/mocks/robot_interface.py | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index 1e569f70..d182dbae 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -115,10 +115,10 @@ def _run(self) -> None: 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_task), name="State Machine Get Inspections", @@ -162,7 +162,7 @@ def _queue_inspections_for_upload( self, mission: Mission, current_task: InspectionTask ) -> None: try: - inspection: Inspection = self.state_machine.robot.get_inspections( + inspection: Inspection = self.state_machine.robot.get_inspection( task=current_task ) diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 6453110a..04e50a36 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -157,7 +157,7 @@ def resume(self) -> None: raise NotImplementedError @abstractmethod - def get_inspections(self, task: InspectionTask) -> Inspection: + def get_inspection(self, task: InspectionTask) -> Inspection: """Return the inspections connected to the given step. Parameters diff --git a/tests/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index 68bfb1a8..bed66668 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -244,7 +244,7 @@ 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() diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index e21bd142..2c8231f4 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -57,7 +57,7 @@ def pause(self) -> None: def resume(self) -> None: return - def get_inspections(self, task: InspectionTask) -> Inspection: + def get_inspection(self, task: InspectionTask) -> Inspection: image: Image = Image(mock_image_metadata()) image.data = b"Some binary image data" return image From 088fe0bf44e483b0d08fa3f461e34b2660340fe5 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Thu, 10 Oct 2024 11:13:36 +0200 Subject: [PATCH 09/12] Fix mypy --- .../apis/models/start_mission_definition.py | 17 ++++--- .../apis/schedule/scheduling_controller.py | 3 +- src/isar/mission_planner/local_planner.py | 50 +------------------ .../sequential_task_selector.py | 6 +-- .../task_selector_interface.py | 8 +-- src/isar/state_machine/state_machine.py | 6 +-- src/robot_interface/models/mission/mission.py | 4 +- src/robot_interface/models/mission/task.py | 35 +++++++++---- .../apis/scheduler/test_scheduler_router.py | 11 +--- tests/isar/mission/test_mission.py | 7 +-- .../models/test_start_mission_definition.py | 4 +- .../services/readers/test_mission_reader.py | 23 ++++++--- .../isar/state_machine/states/test_monitor.py | 6 +-- 13 files changed, 74 insertions(+), 106 deletions(-) diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py index f09e1df9..f5f23c48 100644 --- a/src/isar/apis/models/start_mission_definition.py +++ b/src/isar/apis/models/start_mission_definition.py @@ -8,9 +8,10 @@ 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 +from robot_interface.models.inspection.inspection import Inspection, InspectionMetadata from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.task import ( + TASKS, DockingProcedure, Localize, RecordAudio, @@ -43,7 +44,7 @@ class StartMissionInspectionDefinition(BaseModel): inspection_target: InputPosition analysis_type: Optional[str] = None duration: Optional[float] = None - metadata: Optional[dict] = None + metadata: Optional[InspectionMetadata] = None id: Optional[str] = None @@ -65,10 +66,10 @@ class StartMissionDefinition(BaseModel): def to_isar_mission(start_mission_definition: StartMissionDefinition) -> Mission: - isar_tasks: List[Task] = [] + isar_tasks: List[TASKS] = [] for start_mission_task_definition in start_mission_definition.tasks: - task: Task = create_isar_task(start_mission_task_definition) + 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) @@ -114,7 +115,7 @@ def to_isar_mission(start_mission_definition: StartMissionDefinition) -> Mission return isar_mission -def check_for_duplicate_ids(items: List[Task]): +def check_for_duplicate_ids(items: List[TASKS]): duplicate_ids = get_duplicate_ids(items=items) if len(duplicate_ids) > 0: raise MissionPlannerError( @@ -123,7 +124,7 @@ def check_for_duplicate_ids(items: List[Task]): ) -def create_isar_task(start_mission_task_definition) -> Task: +def create_isar_task(start_mission_task_definition) -> TASKS: if start_mission_task_definition.type == TaskType.Inspection: return create_inspection_task(start_mission_task_definition) @@ -141,7 +142,7 @@ def create_isar_task(start_mission_task_definition) -> Task: def create_inspection_task( start_mission_task_definition: StartMissionTaskDefinition, -) -> Task: +) -> TASKS: inspection = Inspection( id=start_mission_task_definition.inspection.id, @@ -213,7 +214,7 @@ def create_dock_task() -> DockingProcedure: return DockingProcedure(behavior="dock") -def get_duplicate_ids(items: List[Task]) -> 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 abb1225e..1562353b 100644 --- a/src/isar/apis/schedule/scheduling_controller.py +++ b/src/isar/apis/schedule/scheduling_controller.py @@ -22,6 +22,7 @@ from isar.state_machine.states_enum import States from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.task import ( + TASKS, Localize, MoveArm, ReturnToHome, @@ -316,5 +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: + 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/mission_planner/local_planner.py b/src/isar/mission_planner/local_planner.py index 2b8e7819..7af96df0 100644 --- a/src/isar/mission_planner/local_planner.py +++ b/src/isar/mission_planner/local_planner.py @@ -1,6 +1,6 @@ import logging from pathlib import Path -from typing import List +from typing import List, Optional from alitra import Frame from injector import inject @@ -13,18 +13,7 @@ ) from isar.services.readers.base_reader import BaseReader, BaseReaderError from robot_interface.models.mission.mission import Mission -from robot_interface.models.mission.task import ( - DockingProcedure, - Localize, - MoveArm, - RecordAudio, - ReturnToHome, - TakeImage, - TakeThermalImage, - TakeThermalVideo, - TakeVideo, - Task, -) + logger = logging.getLogger("api") @@ -52,39 +41,6 @@ def get_mission(self, mission_id) -> Mission: def read_mission_from_file(mission_path: Path) -> Mission: mission_dict: dict = BaseReader.read_json(location=mission_path) - mission_tasks: List[Task] = [] - task_dataclass: Task = None - - for task in mission_dict["tasks"]: - if task["type"] == "return_to_home": - task_dataclass = ReturnToHome - elif task["type"] == "localize": - task_dataclass = Localize - elif task["type"] == "move_arm": - task_dataclass = MoveArm - elif task["type"] == "take_image": - task_dataclass = TakeImage - elif task["type"] == "take_thermal_image": - task_dataclass = TakeThermalImage - elif task["type"] == "take_video": - task_dataclass = TakeVideo - elif task["type"] == "take_thermal_video": - task_dataclass = TakeThermalVideo - elif task["type"] == "record_audio": - task_dataclass = RecordAudio - elif task["type"] == "docking_procedure": - task_dataclass = DockingProcedure - - if task_dataclass: - task: Task = BaseReader.dict_to_dataclass( - dataclass_dict=task, - target_dataclass=task_dataclass, - cast_config=[Frame], - strict_config=True, - ) - mission_tasks.append(task) - - mission_dict["tasks"] = [] mission: Mission = BaseReader.dict_to_dataclass( dataclass_dict=mission_dict, target_dataclass=Mission, @@ -92,8 +48,6 @@ def read_mission_from_file(mission_path: Path) -> Mission: strict_config=True, ) - mission.tasks = mission_tasks - return mission def get_predefined_missions(self) -> dict: 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/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index d7e56163..68b62d29 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -37,7 +37,7 @@ RobotStatus, TaskStatus, ) -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 @@ -232,7 +232,7 @@ def __init__( self.stopped: bool = False self.current_mission: Optional[Mission] = None - self.current_task: Optional[Task] = None + self.current_task: Optional[TASKS] = None self.initial_pose: Optional[Pose] = None self.current_state: State = States(self.state) # type: ignore @@ -506,7 +506,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 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/task.py b/src/robot_interface/models/mission/task.py index ccc9d8c0..ed289ed3 100644 --- a/src/robot_interface/models/mission/task.py +++ b/src/robot_interface/models/mission/task.py @@ -1,6 +1,6 @@ from dataclasses import dataclass, field from enum import Enum -from typing import Iterator, Literal, Optional, Type +from typing import Iterator, Literal, Optional, Type, Union from alitra import Pose, Position @@ -46,7 +46,7 @@ def is_finished(self) -> bool: return True return False - def update_task_status(self) -> None: + def update_task_status(self) -> TaskStatus: return self.status @@ -72,7 +72,7 @@ class DockingProcedure(Task): """ behavior: Literal["dock", "undock"] = field(default=None, init=True) - type: str = TaskTypes.DockingProcedure + type: Literal[TaskTypes.DockingProcedure] = TaskTypes.DockingProcedure @dataclass @@ -82,7 +82,7 @@ class ReturnToHome(Task): """ pose: Pose = field(default=None, init=True) - type: str = TaskTypes.ReturnToHome + type: Literal[TaskTypes.ReturnToHome] = TaskTypes.ReturnToHome @dataclass @@ -92,7 +92,7 @@ class Localize(Task): """ localization_pose: Pose = field(default=None, init=True) - type: str = TaskTypes.Localize + type: Literal[TaskTypes.Localize] = TaskTypes.Localize @dataclass @@ -102,7 +102,7 @@ class MoveArm(Task): """ arm_pose: str = field(default=None, init=True) - type: str = TaskTypes.MoveArm + type: Literal[TaskTypes.MoveArm] = TaskTypes.MoveArm @dataclass @@ -112,7 +112,7 @@ class TakeImage(InspectionTask): """ target: Position = field(default=None, init=True) - type: str = TaskTypes.TakeImage + type: Literal[TaskTypes.TakeImage] = TaskTypes.TakeImage @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -126,7 +126,7 @@ class TakeThermalImage(InspectionTask): """ target: Position = field(default=None, init=True) - type: str = TaskTypes.TakeThermalImage + type: Literal[TaskTypes.TakeThermalImage] = TaskTypes.TakeThermalImage @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -143,7 +143,7 @@ class TakeVideo(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: str = TaskTypes.TakeVideo + type: Literal[TaskTypes.TakeVideo] = TaskTypes.TakeVideo @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -160,7 +160,7 @@ class TakeThermalVideo(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: str = TaskTypes.TakeThermalVideo + type: Literal[TaskTypes.TakeThermalVideo] = TaskTypes.TakeThermalVideo @staticmethod def get_inspection_type() -> Type[Inspection]: @@ -177,8 +177,21 @@ class RecordAudio(InspectionTask): target: Position = field(default=None, init=True) duration: float = field(default=None, init=True) - type: str = TaskTypes.RecordAudio + 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/tests/isar/apis/scheduler/test_scheduler_router.py b/tests/isar/apis/scheduler/test_scheduler_router.py index 0c036f27..46472c27 100644 --- a/tests/isar/apis/scheduler/test_scheduler_router.py +++ b/tests/isar/apis/scheduler/test_scheduler_router.py @@ -178,7 +178,7 @@ 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_task_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_task_ids_content[ "mission_definition" @@ -209,15 +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_task_ids(self, client: TestClient): - response = client.post( - url=self.schedule_start_mission_path, - json=jsonable_encoder(self.mock_start_mission_duplicate_task_ids_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 c7807516..9bf6bccd 100644 --- a/tests/isar/mission/test_mission.py +++ b/tests/isar/mission/test_mission.py @@ -1,8 +1,9 @@ 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.mission import Mission from robot_interface.models.mission.task import ( + TASKS, TakeImage, TakeThermalImage, ReturnToHome, @@ -126,8 +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 diff --git a/tests/isar/models/test_start_mission_definition.py b/tests/isar/models/test_start_mission_definition.py index a8dd75c2..dee0e983 100644 --- a/tests/isar/models/test_start_mission_definition.py +++ b/tests/isar/models/test_start_mission_definition.py @@ -11,7 +11,7 @@ to_isar_mission, ) from robot_interface.models.mission.mission import Mission -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") @@ -33,7 +33,7 @@ ), ], ) -def test_duplicate_id_check(item_list: List[Task], 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 diff --git a/tests/isar/services/readers/test_mission_reader.py b/tests/isar/services/readers/test_mission_reader.py index c7ec1436..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 @@ -7,6 +8,7 @@ from isar.mission_planner.mission_planner_interface import MissionNotFoundError from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.task import ( + TASKS, ReturnToHome, TakeImage, TakeThermalImage, @@ -32,7 +34,7 @@ def test_read_mission_from_file(mission_reader) -> None: orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), frame=Frame("asset"), ) - task_1: Task = ReturnToHome(pose=expected_robot_pose_1) + task_1: ReturnToHome = ReturnToHome(pose=expected_robot_pose_1) expected_robot_pose_2 = Pose( position=Position(-2, 2, 0, Frame("asset")), @@ -40,7 +42,7 @@ def test_read_mission_from_file(mission_reader) -> None: frame=Frame("asset"), ) expected_inspection_target_1 = Position(2, 2, 0, Frame("robot")) - task_2: Task = TakeImage( + task_2: TakeImage = TakeImage( target=expected_inspection_target_1, robot_pose=expected_robot_pose_2 ) @@ -50,7 +52,7 @@ def test_read_mission_from_file(mission_reader) -> None: frame=Frame("asset"), ) expected_inspection_target_2 = Position(2, 2, 0, Frame("robot")) - task_3: Task = TakeImage( + task_3: TakeImage = TakeImage( target=expected_inspection_target_2, robot_pose=expected_robot_pose_3 ) @@ -59,17 +61,22 @@ def test_read_mission_from_file(mission_reader) -> None: orientation=Orientation(0, 0, 0.4794255, 0.8775826, Frame("asset")), frame=Frame("asset"), ) - task_4: Task = ReturnToHome(pose=expected_robot_pose_4) - - expected_tasks = [task_1, task_2, task_3, task_4] + 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): - if isinstance(expected_task, ReturnToHome): + if isinstance(expected_task, ReturnToHome) and isinstance(task, ReturnToHome): assert expected_task.pose == task.pose - if isinstance(expected_task, TakeImage): + if isinstance(expected_task, TakeImage) and isinstance(task, TakeImage): assert expected_task.target == task.target assert expected_task.robot_pose == task.robot_pose diff --git a/tests/isar/state_machine/states/test_monitor.py b/tests/isar/state_machine/states/test_monitor.py index c3c9025a..1e1409ff 100644 --- a/tests/isar/state_machine/states/test_monitor.py +++ b/tests/isar/state_machine/states/test_monitor.py @@ -4,7 +4,7 @@ from robot_interface.models.mission.mission import Mission from robot_interface.models.mission.status import MissionStatus, TaskStatus -from robot_interface.models.mission.task import Task +from robot_interface.models.mission.task import ReturnToHome, TakeImage from tests.mocks.task import MockTask @@ -17,7 +17,7 @@ ], ) def test_task_finished(monitor: Monitor, mock_status, expected_output): - task: Task = MockTask.return_home() + task: ReturnToHome = MockTask.return_home() task.status = mock_status task_completed: bool = task.is_finished() assert task_completed == expected_output @@ -33,7 +33,7 @@ def test_task_finished(monitor: Monitor, mock_status, expected_output): def test_should_only_upload_if_status_is_completed( monitor: Monitor, is_status_successful, should_queue_upload ): - task: Task = MockTask.take_image() + task: TakeImage = MockTask.take_image() task.status = TaskStatus.Successful if is_status_successful else TaskStatus.Failed mission: Mission = Mission(tasks=[task]) mission.status = ( From 174a2b7c3600153fc35188b0460e9a7836438ff7 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Thu, 10 Oct 2024 12:41:57 +0200 Subject: [PATCH 10/12] Fix metadata received from flotilla --- .../apis/models/start_mission_definition.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/isar/apis/models/start_mission_definition.py b/src/isar/apis/models/start_mission_definition.py index f5f23c48..4eaf3816 100644 --- a/src/isar/apis/models/start_mission_definition.py +++ b/src/isar/apis/models/start_mission_definition.py @@ -44,7 +44,7 @@ class StartMissionInspectionDefinition(BaseModel): inspection_target: InputPosition analysis_type: Optional[str] = None duration: Optional[float] = None - metadata: Optional[InspectionMetadata] = None + metadata: Optional[dict] = None id: Optional[str] = None @@ -144,51 +144,46 @@ def create_inspection_task( start_mission_task_definition: StartMissionTaskDefinition, ) -> TASKS: - inspection = Inspection( - id=start_mission_task_definition.inspection.id, - metadata=start_mission_task_definition.inspection.metadata, - ) - if start_mission_task_definition.inspection.type == InspectionTypes.image: return TakeImage( target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), - inspection=inspection, 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, - inspection=inspection, 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_image: return TakeThermalImage( target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), - inspection=inspection, 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, - inspection=inspection, 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.audio: return RecordAudio( target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), duration=start_mission_task_definition.inspection.duration, - inspection=inspection, 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( From 912f492e16d44d9cdab599e59bea1c5ec2ec8fac Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Fri, 11 Oct 2024 12:37:18 +0200 Subject: [PATCH 11/12] Improve code quality --- src/isar/state_machine/state_machine.py | 3 -- src/isar/state_machine/states/initiate.py | 4 +- .../models/exceptions/robot_exceptions.py | 6 +-- src/robot_interface/robot_interface.py | 51 +++++++++---------- .../isar/state_machine/test_state_machine.py | 4 +- 5 files changed, 32 insertions(+), 36 deletions(-) diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 68b62d29..0f53b0bf 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -288,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: diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py index 2212612f..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: @@ -81,7 +81,7 @@ def _run(self) -> None: except ThreadedRequestNotFinishedError: time.sleep(self.state_machine.sleep_time) continue - except RobotInfeasibleStepException as e: + except RobotInfeasibleTaskException as e: self.state_machine.current_task.error_message = ErrorMessage( error_reason=e.error_reason, error_description=e.error_description ) 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/robot_interface.py b/src/robot_interface/robot_interface.py index 04e50a36..fb374390 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -6,9 +6,8 @@ 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, TaskStatus -from robot_interface.models.mission.task import InspectionTask -from robot_interface.models.mission.task import Task +from robot_interface.models.mission.status import RobotStatus, TaskStatus +from robot_interface.models.mission.task import InspectionTask, Task class RobotInterface(metaclass=ABCMeta): @@ -19,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 @@ -39,23 +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 @abstractmethod def initiate_task(self, task: Task) -> None: - """Send a step to the robot and initiate the execution of the step + """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 ------- @@ -63,12 +62,12 @@ def initiate_task(self, task: Task) -> 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. @@ -78,21 +77,21 @@ def initiate_task(self, task: Task) -> None: @abstractmethod def task_status(self, task_id: str) -> TaskStatus: - """Gets the status of the currently active step on robot. + """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. @@ -101,7 +100,7 @@ def task_status(self, task_id: str) -> TaskStatus: @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 ------- @@ -120,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 ------- @@ -139,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 ------- @@ -158,22 +157,22 @@ def resume(self) -> None: @abstractmethod def get_inspection(self, task: InspectionTask) -> Inspection: - """Return the inspections connected to the given step. + """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/isar/state_machine/test_state_machine.py b/tests/isar/state_machine/test_state_machine.py index bed66668..6cd5bacb 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -192,7 +192,7 @@ def test_state_machine_failed_dependency( scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission, initial_pose=None) - time.sleep(10) + time.sleep(3) expected_transitions_list = deque( [ States.Idle, @@ -221,7 +221,7 @@ def test_state_machine_with_successful_collection( scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission(mission=mission, initial_pose=None) - time.sleep(10) + time.sleep(3) expected_transitions_list = deque( [ States.Idle, From 5e389cc34926c6238bcfd34f7dd3fe99d8f19430 Mon Sep 17 00:00:00 2001 From: "Mariana R. Santos" Date: Fri, 11 Oct 2024 12:54:14 +0200 Subject: [PATCH 12/12] Depricate drive to endpoint --- src/isar/apis/api.py | 1 + 1 file changed, 1 insertion(+) 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",