From cbd84f20875ded6ac762ae983e1aff6b7322b5c1 Mon Sep 17 00:00:00 2001
From: Sondre Olsen <35999713+sondreo@users.noreply.github.com>
Date: Mon, 14 Oct 2024 14:21:31 +0200
Subject: [PATCH] Catch telemetry pose exception

---
 .../models/exceptions/robot_exceptions.py           | 13 +++++++++++++
 src/robot_interface/telemetry/mqtt_client.py        |  8 +++++++-
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/robot_interface/models/exceptions/robot_exceptions.py b/src/robot_interface/models/exceptions/robot_exceptions.py
index 9e66090c..1cbf9c96 100644
--- a/src/robot_interface/models/exceptions/robot_exceptions.py
+++ b/src/robot_interface/models/exceptions/robot_exceptions.py
@@ -16,6 +16,7 @@ class ErrorReason(str, Enum):
     RobotRetrieveDataException: str = "robot_retrieve_data_exception"
     RobotRetrieveInspectionException: str = "robot_retrieve_inspection_exception"
     RobotTelemetryException: str = "robot_telemetry_exception"
+    RobotTelemetryPoseException: str = "robot_telemetry_pose_exception"
     RobotMapException: str = "robot_map_exception"
     RobotTransformException: str = "robot_transform_exception"
     RobotUnknownErrorException: str = "robot_unknown_error_exception"
@@ -189,6 +190,18 @@ def __init__(self, error_description: str) -> None:
     pass
 
 
+# An exception which should be thrown by the robot package if it is unable to retrieve
+# telemetry pose data. It should be used exclusively by the telemetry pose publisher.
+class RobotTelemetryPoseException(RobotException):
+    def __init__(self, error_description: str) -> None:
+        super().__init__(
+            error_reason=ErrorReason.RobotTelemetryPoseException,
+            error_description=error_description,
+        )
+
+    pass
+
+
 # An exception which should be thrown by the robot package if it is unable to load the
 # configuration for maps and transformations. This could be caused by faulty
 # configuration and this exception will cause ISAR to crash as further execution is not
diff --git a/src/robot_interface/telemetry/mqtt_client.py b/src/robot_interface/telemetry/mqtt_client.py
index d3029e18..6264a211 100644
--- a/src/robot_interface/telemetry/mqtt_client.py
+++ b/src/robot_interface/telemetry/mqtt_client.py
@@ -5,7 +5,10 @@
 from queue import Queue
 from typing import Callable, Tuple
 
-from robot_interface.models.exceptions.robot_exceptions import RobotTelemetryException
+from robot_interface.models.exceptions.robot_exceptions import (
+    RobotTelemetryException,
+    RobotTelemetryPoseException,
+)
 from robot_interface.telemetry.payloads import CloudHealthPayload
 from robot_interface.utilities.json_service import EnhancedJSONEncoder
 
@@ -71,6 +74,9 @@ def run(self, isar_id: str, robot_name: str) -> None:
             try:
                 payload = self.telemetry_method(isar_id=isar_id, robot_name=robot_name)
                 topic = self.topic
+            except RobotTelemetryPoseException:
+                time.sleep(self.interval)
+                continue
             except RobotTelemetryException:
                 payload = json.dumps(
                     CloudHealthPayload(isar_id, robot_name, datetime.now(timezone.utc)),