isar 1.20.2__py3-none-any.whl → 1.34.13__py3-none-any.whl

isar/robot/robot_monitor_mission.py

@@ -0,0 +1,357 @@
+import logging
+import time
+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.utilities.mqtt_utilities import (
+    publish_mission_status,
+    publish_task_status,
+)
+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
+
+
+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
+            ):
+                failed_task_error = ErrorMessage(
+                    error_reason=ErrorReason.RobotTaskStatusException,
+                    error_description="Task status collection was cancelled by monitor thread exit",
+                )
+                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 _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 _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. Error description: %s",
+                        e.error_description,
+                    )
+                    break
+                except Exception:
+                    self.logger.exception("Failed to collect task status")
+                    break
+
+                if current_task.status != new_task_status:
+                    current_task.status = new_task_status
+                    self._report_task_status(current_task)
+                    publish_task_status(
+                        self.mqtt_publisher, current_task, self.current_mission
+                    )
+
+                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)
+                        publish_task_status(
+                            self.mqtt_publisher, current_task, self.current_mission
+                        )
+
+            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.exception("Failed to collect mission status")
+                self.robot_service_events.mission_failed.trigger_event(
+                    ErrorMessage(
+                        error_reason=e.error_reason,
+                        error_description=e.error_description,
+                    )
+                )
+                break
+            if new_mission_status != last_mission_status:
+                self.current_mission.status = new_mission_status
+                last_mission_status = new_mission_status
+                publish_mission_status(self.mqtt_publisher, self.current_mission)
+                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:
+                    publish_mission_status(self.mqtt_publisher, self.current_mission)
+                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)
+
+        mission_stopped = self.signal_mission_stopped.wait(0)
+
+        if current_task:
+            current_task.status = (
+                TaskStatus.Cancelled if mission_stopped else TaskStatus.Failed
+            )
+            publish_task_status(self.mqtt_publisher, current_task, self.current_mission)
+        if self.current_mission.status not in [
+            MissionStatus.Cancelled,
+            MissionStatus.PartiallySuccessful,
+            MissionStatus.Failed,
+            MissionStatus.Successful,
+        ]:
+            self.current_mission.status = MissionStatus.Cancelled
+            publish_mission_status(self.mqtt_publisher, self.current_mission)
+            if not mission_stopped:
+                self.robot_service_events.mission_status_updated.trigger_event(
+                    self.current_mission.status
+                )
+        self.logger.info("Stopped monitoring mission")

isar/robot/robot_pause_mission.py

@@ -0,0 +1,74 @@
+import logging
+import time
+from threading import Event, Thread
+from typing import Optional
+
+from isar.config.settings import settings
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    ErrorReason,
+    RobotActionException,
+    RobotException,
+    RobotNoMissionRunningException,
+)
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotPauseMissionThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.error_message: Optional[ErrorMessage] = None
+        Thread.__init__(self, name="Robot pause mission thread")
+
+    def run(self) -> None:
+        retries = 0
+        error: Optional[ErrorMessage] = None
+        while retries < settings.STATE_TRANSITION_NUM_RETIRES:
+            if self.signal_thread_quitting.wait(0):
+                return
+            try:
+                self.robot.pause()
+            except RobotNoMissionRunningException as e:
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                break
+            except (RobotActionException, RobotException) as e:
+                self.logger.warning(
+                    f"\nFailed to pause robot because: {e.error_description}"
+                    f"\nAttempting to pause the robot again"
+                )
+                retries += 1
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                time.sleep(settings.FSM_SLEEP_TIME)
+                continue
+            except Exception as e:
+                self.logger.error(
+                    f"\nAn unexpected error occurred while pausing the robot: {e}"
+                )
+                error = ErrorMessage(
+                    error_reason=ErrorReason.RobotUnknownErrorException,
+                    error_description=str(e),
+                )
+                break
+            return
+
+        error_description = (
+            f"\nFailed to pause the robot after {retries} attempts because: "
+            f"{error.error_description}"
+            f"\nBe aware that the robot may still be moving even though a pause has "
+            "been attempted"
+        )
+
+        self.error_message = ErrorMessage(
+            error_reason=error.error_reason,
+            error_description=error_description,
+        )

isar/robot/robot_resume_mission.py

@@ -0,0 +1,67 @@
+import logging
+import time
+from threading import Event, Thread
+from typing import Optional
+
+from isar.config.settings import settings
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    RobotActionException,
+    RobotException,
+    RobotNoMissionRunningException,
+)
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotResumeMissionThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.error_message: Optional[ErrorMessage] = None
+        Thread.__init__(self, name="Robot resume mission thread")
+
+    def run(self) -> None:
+        retries = 0
+        error: Optional[ErrorMessage] = None
+        while retries < settings.STATE_TRANSITION_NUM_RETIRES:
+            if self.signal_thread_quitting.wait(0):
+                return
+            try:
+                self.robot.resume()
+                return
+            except (RobotActionException, RobotException) as e:
+                self.logger.warning(
+                    f"Attempt {retries + 1} to resume mission failed: {e.error_description}"
+                    f"\nAttempting to resume the robot again"
+                )
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                time.sleep(settings.FSM_SLEEP_TIME)
+                retries += 1
+                continue
+            except RobotNoMissionRunningException as e:
+                self.logger.error(
+                    f"Failed to resume mission: {e.error_reason}. {e.error_description}"
+                )
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                break
+
+        error_description = (
+            f"\nFailed to resume the robot after {retries + 1} attempts because: "
+            f"{error.error_description}"
+            f"\nBe aware that the robot may still be moving even though a resume has "
+            "been attempted"
+        )
+
+        self.error_message = ErrorMessage(
+            error_reason=error.error_reason,
+            error_description=error_description,
+        )

isar/robot/robot_start_mission.py

@@ -0,0 +1,66 @@
+import logging
+from threading import Event, Thread
+from typing import Optional
+
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    ErrorReason,
+    RobotAlreadyHomeException,
+    RobotException,
+    RobotInfeasibleMissionException,
+)
+from robot_interface.models.mission.mission import Mission
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStartMissionThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        mission: Mission,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.robot: RobotInterface = robot
+        self.signal_thread_quitting: Event = signal_thread_quitting
+        self.mission = mission
+        self.error_message: Optional[ErrorMessage] = None
+        Thread.__init__(self, name="Robot start mission thread")
+
+    def run(self):
+        if self.signal_thread_quitting.wait(0):
+            return
+        try:
+            self.robot.initiate_mission(self.mission)
+        except RobotAlreadyHomeException as e:
+            self.logger.info(
+                "Robot disregarded return to home mission as its already at home. Return home mission will be assumed successful without running."
+            )
+            self.error_message = ErrorMessage(
+                error_reason=e.error_reason,
+                error_description=e.error_description,
+            )
+        except RobotInfeasibleMissionException as e:
+            self.logger.error(
+                f"Mission is infeasible and cannot be scheduled because: {e.error_description}"
+            )
+            self.error_message = ErrorMessage(
+                error_reason=e.error_reason,
+                error_description=e.error_description,
+            )
+        except RobotException as e:
+            self.logger.warning(
+                f"Initiating mission failed " f"because: {e.error_description}"
+            )
+            self.error_message = ErrorMessage(
+                error_reason=e.error_reason,
+                error_description=e.error_description,
+            )
+        except Exception as e:
+            self.logger.warning(
+                f"Initiating mission failed due to unknown exception: {e}"
+            )
+            self.error_message = ErrorMessage(
+                error_reason=ErrorReason.RobotUnknownErrorException,
+                error_description=str(e),
+            )

isar/robot/robot_status.py

@@ -0,0 +1,61 @@
+import logging
+import time
+from threading import Event, Thread
+
+from isar.config.settings import settings
+from isar.models.events import RobotServiceEvents, SharedState, StateMachineEvents
+from robot_interface.models.exceptions.robot_exceptions import RobotException
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStatusThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+        shared_state: SharedState,
+        robot_service_events: RobotServiceEvents,
+        state_machine_events: StateMachineEvents,
+    ):
+        self.logger = logging.getLogger("robot")
+        self.shared_state: SharedState = shared_state
+        self.robot_service_events: RobotServiceEvents = robot_service_events
+        self.state_machine_events: StateMachineEvents = state_machine_events
+        self.robot: RobotInterface = robot
+        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()
+        Thread.__init__(self, name="Robot status thread")
+
+    def stop(self) -> None:
+        return
+
+    def _is_ready_to_poll_for_status(self) -> bool:
+        time_since_last_robot_status_poll = (
+            time.time() - self.last_robot_status_poll_time
+        )
+        return time_since_last_robot_status_poll > self.robot_status_poll_interval
+
+    def run(self):
+        if self.signal_thread_quitting.is_set():
+            return
+
+        thread_check_interval = settings.THREAD_CHECK_INTERVAL
+
+        while not self.signal_thread_quitting.wait(thread_check_interval):
+
+            if not self._is_ready_to_poll_for_status():
+                continue
+
+            try:
+                self.last_robot_status_poll_time = time.time()
+
+                robot_status = self.robot.robot_status()
+
+                if robot_status is not self.shared_state.robot_status.check():
+                    self.shared_state.robot_status.update(robot_status)
+                    self.robot_service_events.robot_status_changed.trigger_event(True)
+            except RobotException as e:
+                self.logger.error(f"Failed to retrieve robot status: {e}")
+                continue
+        self.logger.info("Exiting robot status thread")

isar/robot/robot_stop_mission.py

@@ -0,0 +1,68 @@
+import logging
+import time
+from threading import Event, Thread
+from typing import Optional
+
+from isar.config.settings import settings
+from robot_interface.models.exceptions.robot_exceptions import (
+    ErrorMessage,
+    ErrorReason,
+    RobotActionException,
+    RobotException,
+    RobotNoMissionRunningException,
+)
+from robot_interface.robot_interface import RobotInterface
+
+
+class RobotStopMissionThread(Thread):
+    def __init__(
+        self,
+        robot: RobotInterface,
+        signal_thread_quitting: Event,
+    ):
+        self.logger = logging.getLogger("robot")
+        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:
+        retries = 0
+        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:
+                self.robot.stop()
+            except RobotNoMissionRunningException:
+                return
+            except (RobotActionException, RobotException) as e:
+                self.logger.warning(
+                    f"\nFailed to stop robot because: {e.error_description}"
+                    f"\nAttempting to stop the robot again"
+                )
+                retries += 1
+                error = ErrorMessage(
+                    error_reason=e.error_reason, error_description=e.error_description
+                )
+                time.sleep(settings.FSM_SLEEP_TIME)
+                continue
+
+            return
+
+        error_description = (
+            f"\nFailed to stop the robot after {retries} attempts because: "
+            f"{error.error_description}"
+            f"\nBe aware that the robot may still be moving even though a stop has "
+            "been attempted"
+        )
+
+        self.error_message = ErrorMessage(
+            error_reason=error.error_reason,
+            error_description=error_description,
+        )