isar 1.33.9__py3-none-any.whl → 1.34.1__py3-none-any.whl

isar/robot/robot_monitor_mission.py

@@ -0,0 +1,399 @@
+import json
+import logging
+import time
+from datetime import datetime, timezone
+from threading import Event, Thread
+from typing import Iterator, Optional
+
+from isar.config.settings import settings
+from isar.models.events import RobotServiceEvents, SharedState
+from isar.services.service_connections.mqtt.mqtt_client import props_expiry
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    ErrorReason,
+    RobotCommunicationException,
+    RobotCommunicationTimeoutException,
+    RobotException,
+    RobotMissionStatusException,
+    RobotTaskStatusException,
+)
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.status import MissionStatus, TaskStatus
+from robot_interface.models.mission.task import TASKS, InspectionTask
+from robot_interface.robot_interface import RobotInterface
+from robot_interface.telemetry.mqtt_client import MqttClientInterface
+from robot_interface.telemetry.payloads import MissionPayload, TaskPayload
+from robot_interface.utilities.json_service import EnhancedJSONEncoder
+
+
+def get_next_task(task_iterator: Iterator[TASKS]) -> Optional[TASKS]:
+    try:
+        return next(task_iterator)
+    except StopIteration:
+        return None
+
+
+def is_finished(task_status: TaskStatus) -> bool:
+    if (
+        task_status == TaskStatus.Successful
+        or task_status == TaskStatus.PartiallySuccessful
+        or task_status == TaskStatus.Cancelled
+        or task_status == TaskStatus.Failed
+    ):
+        return True
+    return False
+
+
+def should_upload_inspections(task: TASKS) -> bool:
+    if settings.UPLOAD_INSPECTIONS_ASYNC:
+        return False
+
+    return task.status == TaskStatus.Successful and isinstance(task, InspectionTask)
+
+
+class RobotMonitorMissionThread(Thread):
+    def __init__(
+        self,
+        robot_service_events: RobotServiceEvents,
+        shared_state: SharedState,
+        robot: RobotInterface,
+        mqtt_publisher: MqttClientInterface,
+        signal_thread_quitting: Event,
+        signal_mission_stopped: Event,
+        mission: Mission,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.signal_mission_stopped: Event = signal_mission_stopped
+        self.mqtt_publisher = mqtt_publisher
+        self.current_mission: Optional[Mission] = mission
+        self.shared_state: SharedState = shared_state
+        self.shared_state.mission_id.trigger_event(mission.id)
+
+        Thread.__init__(self, name="Robot mission monitoring thread")
+
+    def _get_task_status(self, task_id) -> TaskStatus:
+        task_status: TaskStatus = TaskStatus.NotStarted
+        failed_task_error: Optional[ErrorMessage] = None
+        request_status_failure_counter: int = 0
+
+        while (
+            request_status_failure_counter
+            < settings.REQUEST_STATUS_FAILURE_COUNTER_LIMIT
+        ):
+            if self.signal_thread_quitting.wait(0) or self.signal_mission_stopped.wait(
+                0
+            ):
+                break
+            if request_status_failure_counter > 0:
+                time.sleep(settings.REQUEST_STATUS_COMMUNICATION_RECONNECT_DELAY)
+
+            try:
+                task_status = self.robot.task_status(task_id)
+                request_status_failure_counter = 0
+            except (
+                RobotCommunicationTimeoutException,
+                RobotCommunicationException,
+            ) as e:
+                request_status_failure_counter += 1
+                self.logger.error(
+                    f"Failed to get task status "
+                    f"{request_status_failure_counter} times because: "
+                    f"{e.error_description}"
+                )
+
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                continue
+
+            except RobotTaskStatusException as e:
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            except RobotException as e:
+                failed_task_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            except Exception as e:
+                failed_task_error = ErrorMessage(
+                    error_reason=ErrorReason.RobotUnknownErrorException,
+                    error_description=str(e),
+                )
+                break
+
+            return task_status
+
+        raise RobotTaskStatusException(
+            error_description=failed_task_error.error_description
+        )
+
+    def _get_mission_status(self, mission_id) -> MissionStatus:
+        mission_status: MissionStatus = MissionStatus.NotStarted
+        failed_mission_error: Optional[ErrorMessage] = None
+        request_status_failure_counter: int = 0
+
+        while (
+            request_status_failure_counter
+            < settings.REQUEST_STATUS_FAILURE_COUNTER_LIMIT
+        ):
+            if self.signal_thread_quitting.wait(0) or self.signal_mission_stopped.wait(
+                0
+            ):
+                break
+            if request_status_failure_counter > 0:
+                time.sleep(settings.REQUEST_STATUS_COMMUNICATION_RECONNECT_DELAY)
+
+            try:
+                mission_status = self.robot.mission_status(mission_id)
+                request_status_failure_counter = 0
+            except (
+                RobotCommunicationTimeoutException,
+                RobotCommunicationException,
+            ) as e:
+                request_status_failure_counter += 1
+                self.logger.error(
+                    f"Failed to get task status "
+                    f"{request_status_failure_counter} times because: "
+                    f"{e.error_description}"
+                )
+
+                failed_mission_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                continue
+
+            except RobotMissionStatusException as e:
+                failed_mission_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            except RobotException as e:
+                failed_mission_error = ErrorMessage(
+                    error_reason=e.error_reason,
+                    error_description=e.error_description,
+                )
+                break
+
+            except Exception as e:
+                failed_mission_error = ErrorMessage(
+                    error_reason=ErrorReason.RobotUnknownErrorException,
+                    error_description=str(e),
+                )
+                break
+
+            return mission_status
+
+        raise RobotMissionStatusException(
+            error_description=(
+                failed_mission_error.error_description
+                if failed_mission_error
+                else "Mission status thread cancelled"
+            )
+        )
+
+    def publish_task_status(self, task: TASKS) -> None:
+        """Publishes the task status to the MQTT Broker"""
+        if not self.mqtt_publisher:
+            return
+
+        error_message: Optional[ErrorMessage] = None
+        if task:
+            if task.error_message:
+                error_message = task.error_message
+
+        payload: TaskPayload = TaskPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            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 if task 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),
+        )
+
+        self.mqtt_publisher.publish(
+            topic=settings.TOPIC_ISAR_TASK + f"/{task.id}",
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
+            qos=1,
+            retain=True,
+            properties=props_expiry(settings.MQTT_MISSION_AND_TASK_EXPIRY),
+        )
+
+    def _report_task_status(self, task: TASKS) -> None:
+        if task.status == TaskStatus.Failed:
+            self.logger.warning(
+                f"Task: {str(task.id)[:8]} was reported as failed by the robot"
+            )
+        elif task.status == TaskStatus.Successful:
+            self.logger.info(
+                f"{type(task).__name__} task: {str(task.id)[:8]} completed"
+            )
+        else:
+            self.logger.info(
+                f"Task: {str(task.id)[:8]} was reported as {task.status} by the robot"
+            )
+
+    def publish_mission_status(self) -> None:
+        if not self.mqtt_publisher:
+            return
+
+        error_message: Optional[ErrorMessage] = None
+        if self.current_mission:
+            if self.current_mission.error_message:
+                error_message = self.current_mission.error_message
+
+        payload: MissionPayload = MissionPayload(
+            isar_id=settings.ISAR_ID,
+            robot_name=settings.ROBOT_NAME,
+            mission_id=self.current_mission.id if self.current_mission else None,
+            status=self.current_mission.status if self.current_mission 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),
+        )
+
+        self.mqtt_publisher.publish(
+            topic=settings.TOPIC_ISAR_MISSION + f"/{self.current_mission.id}",
+            payload=json.dumps(payload, cls=EnhancedJSONEncoder),
+            qos=1,
+            retain=True,
+            properties=props_expiry(settings.MQTT_MISSION_AND_TASK_EXPIRY),
+        )
+
+    def _finalize_mission_status(self):
+        fail_statuses = [
+            TaskStatus.Cancelled,
+            TaskStatus.Failed,
+        ]
+        partially_fail_statuses = fail_statuses + [TaskStatus.PartiallySuccessful]
+
+        if len(self.current_mission.tasks) == 0:
+            self.current_mission.status = MissionStatus.Successful
+        elif all(task.status in fail_statuses for task in self.current_mission.tasks):
+            self.current_mission.error_message = ErrorMessage(
+                error_reason=None,
+                error_description="The mission failed because all tasks in the mission "
+                "failed",
+            )
+            self.current_mission.status = MissionStatus.Failed
+        elif any(
+            task.status in partially_fail_statuses
+            for task in self.current_mission.tasks
+        ):
+            self.current_mission.status = MissionStatus.PartiallySuccessful
+        else:
+            self.current_mission.status = MissionStatus.Successful
+
+    def run(self) -> None:
+
+        self.task_iterator: Iterator[TASKS] = iter(self.current_mission.tasks)
+        current_task: Optional[TASKS] = get_next_task(self.task_iterator)
+        current_task.status = TaskStatus.NotStarted
+
+        last_mission_status: MissionStatus = MissionStatus.NotStarted
+
+        while not self.signal_thread_quitting.wait(
+            0
+        ) or self.signal_mission_stopped.wait(0):
+
+            if current_task:
+                try:
+                    new_task_status = self._get_task_status(current_task.id)
+                except RobotTaskStatusException as e:
+                    self.logger.error(
+                        "Failed to collect task status", e.error_description
+                    )
+                    break
+                except Exception:
+                    break
+
+                if current_task.status != new_task_status:
+                    current_task.status = new_task_status
+                    self._report_task_status(current_task)
+                    self.publish_task_status(task=current_task)
+
+                if is_finished(new_task_status):
+                    if should_upload_inspections(current_task):
+                        self.robot_service_events.request_inspection_upload.trigger_event(
+                            (current_task, self.current_mission)
+                        )
+                    current_task = get_next_task(self.task_iterator)
+                    if current_task is not None:
+                        # This is not required, but does make reporting more responsive
+                        current_task.status = TaskStatus.InProgress
+                        self._report_task_status(current_task)
+                        self.publish_task_status(task=current_task)
+
+            if self.signal_thread_quitting.wait(0) or self.signal_mission_stopped.wait(
+                0
+            ):
+                break
+
+            try:
+                new_mission_status = self._get_mission_status(self.current_mission.id)
+            except RobotMissionStatusException as e:
+                self.logger.error("Failed to collect mission status", e)
+                break
+            if new_mission_status != last_mission_status:
+                self.current_mission.status = new_mission_status
+                last_mission_status = new_mission_status
+                self.publish_mission_status()
+                self.robot_service_events.mission_status_updated.trigger_event(
+                    new_mission_status
+                )
+
+            if new_mission_status == MissionStatus.Cancelled or (
+                new_mission_status
+                not in [MissionStatus.NotStarted, MissionStatus.InProgress]
+                and current_task is None
+            ):
+                # Standardises final mission status report
+                mission_status = self.current_mission.status
+                self._finalize_mission_status()
+                if mission_status != self.current_mission.status:
+                    self.publish_mission_status()
+                break
+
+            if self.signal_thread_quitting.wait(0) or self.signal_mission_stopped.wait(
+                0
+            ):
+                break
+
+            time.sleep(settings.FSM_SLEEP_TIME)
+        self.shared_state.mission_id.trigger_event(None)
+
+        if not self.signal_mission_stopped.wait(0):
+            self.logger.info("Done monitoring current mission")
+            return
+
+        if current_task:
+            current_task.status = TaskStatus.Cancelled
+            self.publish_task_status(task=current_task)
+        if new_mission_status not in [
+            MissionStatus.Cancelled,
+            MissionStatus.PartiallySuccessful,
+            MissionStatus.Failed,
+            MissionStatus.Successful,
+        ]:
+            self.current_mission.status = MissionStatus.Cancelled
+            self.publish_mission_status()
+        self.logger.info("Stopped monitoring mission due to mission stop")

isar/robot/robot_status.py

@@ -25,17 +25,12 @@ class RobotStatusThread(Thread):
         self.signal_thread_quitting: Event = signal_thread_quitting
         self.robot_status_poll_interval: float = settings.ROBOT_API_STATUS_POLL_INTERVAL
         self.last_robot_status_poll_time: float = time.time()
-        self.force_status_poll_next_iteration: bool = True
         Thread.__init__(self, name="Robot status thread")
 
     def stop(self) -> None:
         return
 
     def _is_ready_to_poll_for_status(self) -> bool:
-        if self.force_status_poll_next_iteration:
-            self.force_status_poll_next_iteration = False
-            return True
-
         time_since_last_robot_status_poll = (
             time.time() - self.last_robot_status_poll_time
         )
@@ -48,11 +43,6 @@ class RobotStatusThread(Thread):
         thread_check_interval = settings.THREAD_CHECK_INTERVAL
 
         while not self.signal_thread_quitting.wait(thread_check_interval):
-            if self.state_machine_events.clear_robot_status.consume_event() is not None:
-                self.shared_state.robot_status.clear_event()
-                self.robot_service_events.robot_status_changed.clear_event()
-                self.robot_service_events.robot_status_cleared.trigger_event(True)
-                self.force_status_poll_next_iteration = True
 
             if not self._is_ready_to_poll_for_status():
                 continue

isar/robot/robot_stop_mission.py

@@ -4,9 +4,9 @@ from threading import Event, Thread
 from typing import Optional
 
 from isar.config.settings import settings
-from isar.models.events import RobotServiceEvents
 from robot_interface.models.exceptions.robot_exceptions import (
     ErrorMessage,
+    ErrorReason,
     RobotActionException,
     RobotException,
 )
@@ -16,14 +16,13 @@ from robot_interface.robot_interface import RobotInterface
 class RobotStopMissionThread(Thread):
     def __init__(
         self,
-        robot_service_events: RobotServiceEvents,
         robot: RobotInterface,
         signal_thread_quitting: Event,
     ):
         self.logger = logging.getLogger("robot")
-        self.robot_service_events: RobotServiceEvents = robot_service_events
         self.robot: RobotInterface = robot
         self.signal_thread_quitting: Event = signal_thread_quitting
+        self.error_message: Optional[ErrorMessage] = None
         Thread.__init__(self, name="Robot stop mission thread")
 
     def run(self) -> None:
@@ -31,6 +30,10 @@ class RobotStopMissionThread(Thread):
         error: Optional[ErrorMessage] = None
         while retries < settings.STOP_ROBOT_ATTEMPTS_LIMIT:
             if self.signal_thread_quitting.wait(0):
+                self.error_message = ErrorMessage(
+                    error_reason=ErrorReason.RobotUnknownErrorException,
+                    error_description="Stop mission thread cancelled",
+                )
                 return
 
             try:
@@ -47,7 +50,6 @@ class RobotStopMissionThread(Thread):
                 time.sleep(settings.FSM_SLEEP_TIME)
                 continue
 
-            self.robot_service_events.mission_successfully_stopped.trigger_event(True)
             return
 
         error_description = (
@@ -57,9 +59,7 @@ class RobotStopMissionThread(Thread):
             "been attempted"
         )
 
-        error_message = ErrorMessage(
+        self.error_message = ErrorMessage(
             error_reason=error.error_reason,
             error_description=error_description,
         )
-
-        self.robot_service_events.mission_failed_to_stop.trigger_event(error_message)

isar/robot/robot_upload_inspection.py

@@ -0,0 +1,80 @@
+import logging
+from queue import Queue
+from threading import Thread
+from typing import Tuple
+
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotException,
+    RobotRetrieveInspectionException,
+)
+from robot_interface.models.inspection.inspection import Inspection
+from robot_interface.models.mission.mission import Mission
+from robot_interface.models.mission.task import TASKS
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotUploadInspectionThread(Thread):
+    def __init__(
+        self,
+        upload_queue: Queue,
+        robot: RobotInterface,
+        task: TASKS,
+        mission: Mission,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot: RobotInterface = robot
+        self.task: TASKS = task
+        self.upload_queue = upload_queue
+        self.mission: Mission = mission
+        self._is_done = False
+        Thread.__init__(self, name=f"Robot inspection upload thread - {task.id}")
+
+    def stop(self) -> None:
+        return
+
+    def is_done(self) -> bool:
+        return self._is_done
+
+    def run(self):
+        try:
+            inspection: Inspection = self.robot.get_inspection(task=self.task)
+            if self.task.inspection_id != inspection.id:
+                self.logger.warning(
+                    f"The inspection_id of task ({self.task.inspection_id}) "
+                    f"and result ({inspection.id}) is not matching. "
+                    f"This may lead to confusions when accessing the inspection later"
+                )
+
+        except (RobotRetrieveInspectionException, RobotException) as e:
+            error_message: ErrorMessage = ErrorMessage(
+                error_reason=e.error_reason, error_description=e.error_description
+            )
+            self.task.error_message = error_message
+            self.logger.error(
+                f"Failed to retrieve inspections because: {e.error_description}"
+            )
+            return
+
+        except Exception as e:
+            self.logger.error(
+                f"Failed to retrieve inspections because of unexpected error: {e}"
+            )
+            return
+
+        if not inspection:
+            self.logger.warning(
+                f"No inspection result data retrieved for task {str(self.task.id)[:8]}"
+            )
+
+        inspection.metadata.tag_id = self.task.tag_id
+
+        message: Tuple[Inspection, Mission] = (
+            inspection,
+            self.mission,
+        )
+        self.upload_queue.put(message)
+        self.logger.info(
+            f"Inspection result: {str(inspection.id)[:8]} queued for upload"
+        )
+        self._is_done = True

isar/services/utilities/scheduling_utilities.py

@@ -23,7 +23,6 @@ from isar.models.events import (
 )
 from isar.state_machine.states_enum import States
 from robot_interface.models.mission.mission import Mission
-from robot_interface.models.mission.status import MissionStatus
 
 T1 = TypeVar("T1")
 T2 = TypeVar("T2")
@@ -167,6 +166,17 @@ class SchedulingUtilities:
         self.logger.warning(error_message)
         raise HTTPException(status_code=HTTPStatus.CONFLICT, detail=error_message)
 
+    def log_mission_overview(self, mission: Mission) -> None:
+        """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}"
+            )
+        log_statement: str = "\n".join(log_statements)
+
+        self.logger.info("Started mission:\n%s", log_statement)
+
     def start_mission(
         self,
         mission: Mission,
@@ -183,6 +193,12 @@ class SchedulingUtilities:
             If there is an unexpected error while sending the mission to the state machine
         """
         try:
+            self.logger.info(
+                "Requesting to start mission:\n"
+                f"  Mission ID: {mission.id}\n"
+                f"  Mission Name: {mission.name}\n"
+                f"  Number of Tasks: {len(mission.tasks)}"
+            )
             mission_start_response = self._send_command(
                 deepcopy(mission),
                 self.api_events.start_mission,
@@ -213,6 +229,7 @@ class SchedulingUtilities:
             raise HTTPException(
                 status_code=HTTPStatus.INTERNAL_SERVER_ERROR, detail=error_message
             )
+        self.log_mission_overview(mission)
         self.logger.info("OK - Mission started in ISAR")
 
     def return_home(
@@ -262,6 +279,14 @@ class SchedulingUtilities:
         """
         try:
             response = self._send_command(True, self.api_events.pause_mission)
+            if not response.success:
+                self.logger.warning(
+                    f"Mission failed to pause - {response.failure_reason}"
+                )
+                raise HTTPException(
+                    status_code=HTTPStatus.CONFLICT,
+                    detail=response.failure_reason,
+                )
             self.logger.info("OK - Mission successfully paused")
             return response
         except EventConflictError:
@@ -335,18 +360,13 @@ class SchedulingUtilities:
                 mission_id, self.api_events.stop_mission
             )
 
-            if stop_mission_response.mission_not_found:
-                error_message = f"Mission ID {stop_mission_response.mission_id} is not currently running"
-                self.logger.error(error_message)
-                raise HTTPException(
-                    status_code=HTTPStatus.NOT_FOUND, detail=error_message
+            if not stop_mission_response.success:
+                error_message = (
+                    f"Failed to stop mission: {stop_mission_response.failure_reason}"
                 )
-
-            if stop_mission_response.mission_status != MissionStatus.Cancelled.value:
-                error_message = f"Failed to stop mission, mission status is {stop_mission_response.mission_status}"
                 self.logger.error(error_message)
                 raise HTTPException(
-                    status_code=HTTPStatus.CONFLICT, detail=error_message
+                    status_code=HTTPStatus.SERVICE_UNAVAILABLE, detail=error_message
                 )
         except EventConflictError:
             error_message = "Previous stop mission request is still being processed"