diff --git a/src/isar/config/settings.env b/src/isar/config/settings.env index 042e89ac..38fbc49f 100644 --- a/src/isar/config/settings.env +++ b/src/isar/config/settings.env @@ -1,5 +1,7 @@ ISAR_ROBOT_PACKAGE = isar_robot +ISAR_RUN_MISSION_STEPWISE = true + ISAR_STORAGE_LOCAL_ENABLED = true ISAR_STORAGE_BLOB_ENABLED = false ISAR_STORAGE_SLIMM_ENABLED = false diff --git a/src/isar/state_machine/state_machine.py b/src/isar/state_machine/state_machine.py index 6a393c6d..af94da21 100644 --- a/src/isar/state_machine/state_machine.py +++ b/src/isar/state_machine/state_machine.py @@ -165,6 +165,12 @@ def __init__( "dest": self.initiate_state, "before": self._step_finished, }, + { + "trigger": "full_mission_finished", + "source": self.monitor_state, + "dest": self.initiate_state, + "before": self._full_mission_finished, + }, { "trigger": "mission_paused", "source": self.stop_state, @@ -228,7 +234,7 @@ def _initialization_successful(self) -> None: self.publish_mission_status() self.current_task = self.task_selector.next_task() self.current_task.status = TaskStatus.InProgress - self.publish_task_status() + self.publish_task_status(task=self.current_task) self.update_current_step() def _initialization_failed(self) -> None: @@ -236,13 +242,19 @@ def _initialization_failed(self) -> None: self._finalize() def _initiated(self) -> None: - self.current_step.status = StepStatus.InProgress - self.publish_step_status() - self.logger.info( - f"Successfully initiated " - f"{type(self.current_step).__name__} " - f"step: {str(self.current_step.id)[:8]}" - ) + if self.stepwise_mission: + self.current_step.status = StepStatus.InProgress + self.publish_step_status(step=self.current_step) + self.logger.info( + f"Successfully initiated " + f"{type(self.current_step).__name__} " + f"step: {str(self.current_step.id)[:8]}" + ) + else: + self.logger.info( + f"Successfully initiated full mission with ID: " + f"{str(self.current_mission.id)[:8]}" + ) def _pause(self) -> None: return @@ -255,7 +267,7 @@ def _resume(self) -> None: self.current_mission.status = MissionStatus.InProgress self.current_task.status = TaskStatus.InProgress self.publish_mission_status() - self.publish_task_status() + self.publish_task_status(task=self.current_task) resume_mission_response: ControlMissionResponse = ( self._make_control_mission_response() @@ -287,10 +299,35 @@ def _mission_started(self) -> None: return def _step_finished(self) -> None: - self.publish_step_status() + self.publish_step_status(step=self.current_step) self.update_current_task() self.update_current_step() + def _full_mission_finished(self) -> None: + self.current_task = None + step_status: StepStatus = StepStatus.Failed + task_status: TaskStatus = TaskStatus.Failed + + if self.current_mission.status == MissionStatus.Failed: + step_status = StepStatus.Failed + task_status = TaskStatus.Failed + elif self.current_mission.status == MissionStatus.Cancelled: + step_status = StepStatus.Cancelled + task_status = TaskStatus.Cancelled + elif ( + self.current_mission.status == MissionStatus.Successful + or self.current_mission.status == MissionStatus.PartiallySuccessful + ): + step_status = StepStatus.Successful + task_status = TaskStatus.Successful + + for task in self.current_mission.tasks: + task.status = task_status + for step in task.steps: + step.status = step_status + self.publish_step_status(step=step) + self.publish_task_status(task=task) + def _mission_paused(self) -> None: self.logger.info(f"Pausing mission: {self.current_mission.id}") self.current_mission.status = MissionStatus.Paused @@ -303,8 +340,8 @@ def _mission_paused(self) -> None: self.queues.pause_mission.output.put(paused_mission_response) self.publish_mission_status() - self.publish_task_status() - self.publish_step_status() + self.publish_task_status(task=self.current_task) + self.publish_step_status(step=self.current_step) def _stop(self) -> None: self.stopped = True @@ -313,15 +350,16 @@ def _initiate_failed(self) -> None: self.current_step.status = StepStatus.Failed self.current_task.update_task_status() self.current_mission.status = MissionStatus.Failed - self.publish_step_status() - self.publish_task_status() + self.publish_step_status(step=self.current_step) + self.publish_task_status(task=self.current_task) self._finalize() def _initiate_infeasible(self) -> None: - self.current_step.status = StepStatus.Failed - self.publish_step_status() - self.update_current_task() - self.update_current_step() + if self.stepwise_mission: + self.current_step.status = StepStatus.Failed + self.publish_step_status(step=self.current_step) + self.update_current_task() + self.update_current_step() def _mission_stopped(self) -> None: self.current_mission.status = MissionStatus.Cancelled @@ -341,8 +379,8 @@ def _mission_stopped(self) -> None: ) self.queues.stop_mission.output.put(stopped_mission_response) - self.publish_task_status() - self.publish_step_status() + self.publish_task_status(task=self.current_task) + self.publish_step_status(step=self.current_step) self._finalize() ################################################################################# @@ -370,11 +408,11 @@ def begin(self): def update_current_task(self): if self.current_task.is_finished(): self.current_task.update_task_status() - self.publish_task_status() + self.publish_task_status(task=self.current_task) try: self.current_task = self.task_selector.next_task() self.current_task.status = TaskStatus.InProgress - self.publish_task_status() + self.publish_task_status(task=self.current_task) except TaskSelectorStop: # Indicates that all tasks are finished self.current_task = None @@ -458,8 +496,8 @@ def publish_mission_status(self) -> None: retain=False, ) - def publish_task_status(self) -> None: - """Publishes the current task status to the MQTT Broker""" + def publish_task_status(self, task: Task) -> None: + """Publishes the task status to the MQTT Broker""" if not self.mqtt_publisher: return payload: str = json.dumps( @@ -467,8 +505,8 @@ def publish_task_status(self) -> None: "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, - "status": self.current_task.status if self.current_task else None, + "task_id": task.id if task else None, + "status": task.status if task else None, "timestamp": datetime.utcnow(), }, cls=EnhancedJSONEncoder, @@ -480,8 +518,8 @@ def publish_task_status(self) -> None: retain=False, ) - def publish_step_status(self) -> None: - """Publishes the current step status to the MQTT Broker""" + def publish_step_status(self, step: Step) -> None: + """Publishes the step status to the MQTT Broker""" if not self.mqtt_publisher: return payload: str = json.dumps( @@ -490,11 +528,9 @@ def publish_step_status(self) -> None: "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": self.current_step.id if self.current_step else None, - "step_type": self.current_step.__class__.__name__ - if self.current_step - else None, - "status": self.current_step.status if self.current_step 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, "timestamp": datetime.utcnow(), }, cls=EnhancedJSONEncoder, diff --git a/src/isar/state_machine/states/initiate.py b/src/isar/state_machine/states/initiate.py index 6fa1e473..36cbbfd3 100644 --- a/src/isar/state_machine/states/initiate.py +++ b/src/isar/state_machine/states/initiate.py @@ -11,13 +11,11 @@ ) from robot_interface.models.exceptions import ( RobotException, + RobotInfeasibleMissionException, RobotInfeasibleStepException, RobotLowBatteryException, RobotLowPressureException, ) -from robot_interface.models.exceptions.robot_exceptions import ( - RobotInfeasibleMissionException, -) if TYPE_CHECKING: from isar.state_machine.state_machine import StateMachine diff --git a/src/isar/state_machine/states/monitor.py b/src/isar/state_machine/states/monitor.py index b16783e7..014a57e6 100644 --- a/src/isar/state_machine/states/monitor.py +++ b/src/isar/state_machine/states/monitor.py @@ -1,7 +1,7 @@ import logging import time from copy import deepcopy -from typing import Callable, Optional, Sequence, TYPE_CHECKING, Tuple +from typing import Callable, Optional, Sequence, TYPE_CHECKING, Tuple, Union from injector import inject from transitions import State @@ -13,6 +13,8 @@ ) from robot_interface.models.exceptions import RobotException from robot_interface.models.inspection.inspection import Inspection +from robot_interface.models.mission.mission import Mission +from robot_interface.models.mission.status import MissionStatus from robot_interface.models.mission.step import InspectionStep, Step, StepStatus if TYPE_CHECKING: @@ -49,46 +51,77 @@ def _run(self) -> None: break if not self.step_status_thread: - self.step_status_thread = ThreadedRequest( - self.state_machine.robot.step_status - ) - self.step_status_thread.start_thread( - name="State Machine Monitor Current Status" - ) + if self.state_machine.stepwise_mission: + self._run_get_status_thread( + status_function=self.state_machine.robot.step_status, + thread_name="State Machine Monitor Get Step Status", + ) + else: + self._run_get_status_thread( + status_function=self.state_machine.robot.mission_status, + thread_name="State Machine Monitor Get Mission Status", + ) try: - step_status: StepStatus = self.step_status_thread.get_output() + status: Union[ + StepStatus, MissionStatus + ] = self.step_status_thread.get_output() except ThreadedRequestNotFinishedError: time.sleep(self.state_machine.sleep_time) continue except RobotException: - step_status = StepStatus.Failed + status = StepStatus.Failed - self.state_machine.current_step.status = step_status + if self.state_machine.stepwise_mission and isinstance(status, StepStatus): + self.state_machine.current_step.status = status + elif isinstance(status, MissionStatus): + self.state_machine.current_mission.status = status - if self._step_finished(step=self.state_machine.current_step): - get_inspections_thread = ThreadedRequest(self._process_finished_step) + if self._should_upload_inspections(): + get_inspections_thread = ThreadedRequest( + self._queue_inspections_for_upload + ) get_inspections_thread.start_thread( self.state_machine.current_step, name="State Machine Get Inspections", ) - transition = self.state_machine.step_finished # type: ignore - break + + if self.state_machine.stepwise_mission: + if self._step_finished(self.state_machine.current_step): + transition = self.state_machine.step_finished # type: ignore + break + else: + if self._mission_finished(self.state_machine.current_mission): + transition = self.state_machine.full_mission_finished # type: ignore + break self.step_status_thread = None time.sleep(self.state_machine.sleep_time) transition() + def _run_get_status_thread( + self, status_function: Callable, thread_name: str + ) -> None: + self.step_status_thread = ThreadedRequest(request_func=status_function) + self.step_status_thread.start_thread(name=thread_name) + def _queue_inspections_for_upload(self, current_step: InspectionStep) -> None: try: inspections: Sequence[ Inspection ] = self.state_machine.robot.get_inspections(step=current_step) except Exception as e: - self.logger.error( - f"Error getting inspections for step {str(current_step.id)[:8]}: {e}" - ) + if self.state_machine.stepwise_mission: + self.logger.error( + f"Error getting inspections for step " + f"{str(current_step.id)[:8]}: {e}" + ) + else: + self.logger.error( + f"Error getting inspections for mission " + f"{str(self.state_machine.current_mission.id)[:8]}: {e}" + ) return if not inspections: @@ -124,6 +157,29 @@ def _step_finished(self, step: Step) -> bool: finished = True return finished - def _process_finished_step(self, step: Step) -> None: - if step.status == StepStatus.Successful and isinstance(step, InspectionStep): - self._queue_inspections_for_upload(current_step=step) + @staticmethod + def _mission_finished(mission: Mission) -> bool: + if ( + mission.status == MissionStatus.Successful + or mission.status == MissionStatus.PartiallySuccessful + or mission.status == MissionStatus.Failed + ): + return True + return False + + def _should_upload_inspections(self) -> bool: + if self.state_machine.stepwise_mission: + step: Step = self.state_machine.current_step + return ( + self._step_finished(step) + and step.status == StepStatus.Successful + and isinstance(step, InspectionStep) + ) + else: + mission_status: MissionStatus = self.state_machine.current_mission.status + if ( + mission_status == MissionStatus.Successful + or mission_status == MissionStatus.PartiallySuccessful + ): + return True + return False diff --git a/src/robot_interface/models/exceptions/__init__.py b/src/robot_interface/models/exceptions/__init__.py index 7949c063..34ebba27 100644 --- a/src/robot_interface/models/exceptions/__init__.py +++ b/src/robot_interface/models/exceptions/__init__.py @@ -1,6 +1,7 @@ from .robot_exceptions import ( RobotCommunicationException, RobotException, + RobotInfeasibleMissionException, RobotInfeasibleStepException, RobotInvalidResponseException, RobotLowBatteryException, diff --git a/src/robot_interface/robot_interface.py b/src/robot_interface/robot_interface.py index 9464c239..515d118c 100644 --- a/src/robot_interface/robot_interface.py +++ b/src/robot_interface/robot_interface.py @@ -6,7 +6,7 @@ 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 RobotStatus, StepStatus +from robot_interface.models.mission.status import MissionStatus, RobotStatus, StepStatus from robot_interface.models.mission.step import InspectionStep, Step @@ -17,6 +17,10 @@ class RobotInterface(metaclass=ABCMeta): 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 + configuration. + Parameters ---------- mission: Mission @@ -29,14 +33,41 @@ def initiate_mission(self, mission: Mission) -> None: ------ RobotException If the mission is not initiated. + NotImplementedError + If the robot is designed for stepwise mission execution. """ raise NotImplementedError + def mission_status(self) -> MissionStatus: + """Gets the status of the currently active mission on the robot + + This function should be used in combination with the initiate_mission function + if the robot is designed to run full missions and not in a stepwise + configuration. + + Returns + ------- + MissionStatus + Status of the executing mission on the robot. + + Raises + ------ + RobotException + If the mission status could not be retrieved. + NotImplementedError + If the robot is designed for stepwise mission execution. + + """ + @abstractmethod def initiate_step(self, step: Step) -> None: """Send a step to the robot and initiate the execution of the step + 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 + configuration. + Parameters ---------- step : Step @@ -50,6 +81,8 @@ def initiate_step(self, step: Step) -> None: ------ RobotException If the step is not initiated. + NotImplementedError + If the robot is designed for full mission execution. """ raise NotImplementedError @@ -58,19 +91,21 @@ def initiate_step(self, step: Step) -> None: def step_status(self) -> StepStatus: """Gets the status of the currently active step on robot. - Parameters - ---------- - None + 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 + configuration. Returns ------- StepStatus Status of the execution of current step. - Raises: + Raises ------ RobotException - If the step status can't be retrieved. + If the step status could not be retrieved. + NotImplementedError + If the robot is designed for full mission execution. """ raise NotImplementedError @@ -79,10 +114,6 @@ def step_status(self) -> StepStatus: def stop(self) -> None: """Stops the execution of the current step and stops the movement of the robot. - Parameters - ---------- - None - Returns ------- None diff --git a/tests/isar/state_machine/states/test_monitor.py b/tests/isar/state_machine/states/test_monitor.py index fa79c9e8..c3a141ef 100644 --- a/tests/isar/state_machine/states/test_monitor.py +++ b/tests/isar/state_machine/states/test_monitor.py @@ -42,7 +42,12 @@ def test_should_only_upload_if_status_is_completed( mission: Mission = Mission(tasks=[task]) monitor.state_machine.current_mission = mission - monitor._process_finished_step(step) + monitor.state_machine.current_task = task + monitor.state_machine.current_step = step + + if monitor._should_upload_inspections(): + monitor._queue_inspections_for_upload(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 65d6afa4..96b4f9af 100644 --- a/tests/isar/state_machine/test_state_machine.py +++ b/tests/isar/state_machine/test_state_machine.py @@ -90,8 +90,41 @@ def test_reset_state_machine(state_machine) -> None: def test_state_machine_transitions(injector, state_machine_thread) -> None: - step: Step = DriveToPose(pose=MockPose.default_pose) - mission: Mission = Mission(tasks=[Task(steps=[step])]) # type: ignore + 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 + metadata: MissionMetadata = MissionMetadata(mission.id) + scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) + scheduling_utilities.start_mission( + mission=mission, initial_pose=None, mission_metadata=metadata + ) + + time.sleep(3) + 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_transitions_when_running_full_mission( + injector, state_machine_thread +) -> None: + state_machine_thread.state_machine.stepwise_mission = False + + 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 metadata: MissionMetadata = MissionMetadata(mission.id) scheduling_utilities: SchedulingUtilities = injector.get(SchedulingUtilities) scheduling_utilities.start_mission( diff --git a/tests/mocks/robot_interface.py b/tests/mocks/robot_interface.py index f5394815..c13f7c57 100644 --- a/tests/mocks/robot_interface.py +++ b/tests/mocks/robot_interface.py @@ -12,7 +12,7 @@ Inspection, ) from robot_interface.models.mission.mission import Mission -from robot_interface.models.mission.status import RobotStatus, StepStatus +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 @@ -20,7 +20,7 @@ class MockRobot(RobotInterface): def __init__( self, - initiate_step: bool = True, + mission_status: MissionStatus = MissionStatus.Successful, step_status: StepStatus = StepStatus.Successful, stop: bool = True, pose: Pose = Pose( @@ -30,7 +30,7 @@ def __init__( ), robot_status: RobotStatus = RobotStatus.Available, ): - self.initiate_step_return_value: bool = initiate_step + 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 @@ -39,6 +39,9 @@ def __init__( def initiate_mission(self, mission: Mission) -> None: return + def mission_status(self) -> MissionStatus: + return self.mission_status_return_value + def initiate_step(self, step: Step) -> None: return