|
| 1 | +from datetime import UTC, datetime |
| 2 | +from queue import Empty, Queue |
| 3 | + |
| 4 | +from isar.config.settings import settings |
| 5 | +from isar.models.status import IsarStatus |
| 6 | +from paho.mqtt.properties import Properties |
| 7 | +from robot_interface.models.exceptions.event_exceptions import EventTimeoutError |
| 8 | +from robot_interface.models.exceptions.robot_exceptions import ErrorMessage |
| 9 | +from robot_interface.models.mission.status import MissionStatus |
| 10 | +from robot_interface.models.mission.task import TASKS |
| 11 | +from robot_interface.telemetry.mqtt_client import ( |
| 12 | + MQTTQueueMessage, |
| 13 | + props_expiry, |
| 14 | +) |
| 15 | +from robot_interface.telemetry.payloads import ( |
| 16 | + InterventionNeededPayload, |
| 17 | + IsarStatusPayload, |
| 18 | + MissionAbortedPayload, |
| 19 | + MissionPayload, |
| 20 | + TaskPayload, |
| 21 | +) |
| 22 | + |
| 23 | + |
| 24 | +class MQTTQueue: |
| 25 | + queue: Queue[MQTTQueueMessage] |
| 26 | + |
| 27 | + def __init__(self, maxsize: int = 10) -> None: |
| 28 | + self.queue = Queue(maxsize=maxsize) |
| 29 | + self.name = "MQTT queue" |
| 30 | + |
| 31 | + def qsize(self) -> int: |
| 32 | + return self.queue.qsize() |
| 33 | + |
| 34 | + def empty(self) -> bool: |
| 35 | + return self.queue.empty() |
| 36 | + |
| 37 | + def publish( |
| 38 | + self, |
| 39 | + topic: str, |
| 40 | + payload: str, |
| 41 | + qos: int = 0, |
| 42 | + retain: bool = False, |
| 43 | + properties: Properties | None = None, |
| 44 | + ) -> None: |
| 45 | + queue_message: MQTTQueueMessage = MQTTQueueMessage( |
| 46 | + topic=topic, |
| 47 | + payload=payload, |
| 48 | + qos=qos, |
| 49 | + retain=retain, |
| 50 | + properties=properties, |
| 51 | + ) |
| 52 | + self.queue.put(queue_message) |
| 53 | + |
| 54 | + def get(self, timeout: int = 0) -> MQTTQueueMessage: |
| 55 | + try: |
| 56 | + return self.queue.get(block=timeout is not None, timeout=timeout) |
| 57 | + except Empty: |
| 58 | + if timeout is not None: |
| 59 | + raise EventTimeoutError |
| 60 | + return None |
| 61 | + |
| 62 | + def publish_task_status(self, task: TASKS, mission_id: str | None) -> None: |
| 63 | + """Publishes the task status to the MQTT Broker""" |
| 64 | + |
| 65 | + error_message: ErrorMessage | None = task.error_message |
| 66 | + |
| 67 | + payload: TaskPayload = TaskPayload( |
| 68 | + isar_id=settings.ISAR_ID, |
| 69 | + robot_name=settings.ROBOT_NAME, |
| 70 | + mission_id=mission_id, |
| 71 | + task_id=task.id if task else None, |
| 72 | + status=task.status if task else None, |
| 73 | + task_type=task.type if task else None, |
| 74 | + error_reason=error_message.error_reason if error_message else None, |
| 75 | + error_description=( |
| 76 | + error_message.error_description if error_message else None |
| 77 | + ), |
| 78 | + timestamp=datetime.now(UTC), |
| 79 | + ) |
| 80 | + |
| 81 | + self.publish( |
| 82 | + topic=settings.TOPIC_ISAR_TASK + f"/{task.id}", |
| 83 | + payload=payload.model_dump_json(), |
| 84 | + qos=1, |
| 85 | + retain=True, |
| 86 | + properties=props_expiry(settings.MQTT_MISSION_TASK_AND_STATUS_EXPIRY), |
| 87 | + ) |
| 88 | + |
| 89 | + def publish_mission_status( |
| 90 | + self, |
| 91 | + mission_id: str, |
| 92 | + mission_status: MissionStatus, |
| 93 | + error_message: ErrorMessage | None, |
| 94 | + ) -> None: |
| 95 | + payload: MissionPayload = MissionPayload( |
| 96 | + isar_id=settings.ISAR_ID, |
| 97 | + robot_name=settings.ROBOT_NAME, |
| 98 | + mission_id=mission_id, |
| 99 | + status=mission_status, |
| 100 | + error_reason=error_message.error_reason if error_message else None, |
| 101 | + error_description=( |
| 102 | + error_message.error_description if error_message else None |
| 103 | + ), |
| 104 | + timestamp=datetime.now(UTC), |
| 105 | + ) |
| 106 | + |
| 107 | + self.publish( |
| 108 | + topic=settings.TOPIC_ISAR_MISSION + f"/{mission_id}", |
| 109 | + payload=payload.model_dump_json(), |
| 110 | + qos=1, |
| 111 | + retain=True, |
| 112 | + properties=props_expiry(settings.MQTT_MISSION_TASK_AND_STATUS_EXPIRY), |
| 113 | + ) |
| 114 | + |
| 115 | + def publish_isar_status(self, status: IsarStatus) -> None: |
| 116 | + payload: IsarStatusPayload = IsarStatusPayload( |
| 117 | + isar_id=settings.ISAR_ID, |
| 118 | + robot_name=settings.ROBOT_NAME, |
| 119 | + status=status, |
| 120 | + timestamp=datetime.now(UTC), |
| 121 | + ) |
| 122 | + |
| 123 | + self.publish( |
| 124 | + topic=settings.TOPIC_ISAR_STATUS, |
| 125 | + payload=payload.model_dump_json(), |
| 126 | + qos=1, |
| 127 | + retain=True, |
| 128 | + properties=props_expiry(settings.MQTT_MISSION_TASK_AND_STATUS_EXPIRY), |
| 129 | + ) |
| 130 | + |
| 131 | + def publish_mission_aborted( |
| 132 | + self, current_mission_id: str | None, reason: str |
| 133 | + ) -> None: |
| 134 | + payload: MissionAbortedPayload = MissionAbortedPayload( |
| 135 | + isar_id=settings.ISAR_ID, |
| 136 | + robot_name=settings.ROBOT_NAME, |
| 137 | + mission_id=current_mission_id, |
| 138 | + reason=reason, |
| 139 | + timestamp=datetime.now(UTC), |
| 140 | + ) |
| 141 | + |
| 142 | + self.publish( |
| 143 | + topic=settings.TOPIC_ISAR_MISSION_ABORTED, |
| 144 | + payload=payload.model_dump_json(), |
| 145 | + qos=1, |
| 146 | + retain=True, |
| 147 | + properties=props_expiry(settings.MQTT_MISSION_TASK_AND_STATUS_EXPIRY), |
| 148 | + ) |
| 149 | + |
| 150 | + def publish_intervention_needed(self, error_message: str) -> None: |
| 151 | + """Publishes the intervention needed message to the MQTT Broker""" |
| 152 | + payload: InterventionNeededPayload = InterventionNeededPayload( |
| 153 | + isar_id=settings.ISAR_ID, |
| 154 | + robot_name=settings.ROBOT_NAME, |
| 155 | + reason=error_message, |
| 156 | + timestamp=datetime.now(UTC), |
| 157 | + ) |
| 158 | + |
| 159 | + self.publish( |
| 160 | + topic=settings.TOPIC_ISAR_INTERVENTION_NEEDED, |
| 161 | + payload=payload.model_dump_json(), |
| 162 | + qos=1, |
| 163 | + retain=True, |
| 164 | + properties=props_expiry(settings.MQTT_MISSION_TASK_AND_STATUS_EXPIRY), |
| 165 | + ) |
0 commit comments