From b0ae76f861172a9b0feab2182cac9d41f4e739ac Mon Sep 17 00:00:00 2001
From: "Mariana R. Santos" <mrica@equinor.com>
Date: Tue, 9 Apr 2024 13:53:46 +0200
Subject: [PATCH] Implement robot telemetry exception

---
 src/robot_interface/telemetry/mqtt_client.py | 18 +++++++++++++-----
 src/robot_interface/telemetry/payloads.py    |  7 +++++++
 2 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/src/robot_interface/telemetry/mqtt_client.py b/src/robot_interface/telemetry/mqtt_client.py
index efe024bc..0838809e 100644
--- a/src/robot_interface/telemetry/mqtt_client.py
+++ b/src/robot_interface/telemetry/mqtt_client.py
@@ -1,11 +1,13 @@
 import time
 from abc import ABCMeta, abstractmethod
 from queue import Queue
-from typing import Callable, Tuple
+from typing import Callable, Tuple, Type
 
 from robot_interface.models.exceptions.robot_exceptions import (
     RobotTelemetryException,
 )
+from robot_interface.telemetry.payloads import CloudHealthPayload
+from datetime import datetime
 
 
 class MqttClientInterface(metaclass=ABCMeta):
@@ -61,17 +63,23 @@ def __init__(
         self.retain: bool = retain
 
     def run(self, isar_id: str, robot_name: str) -> None:
+        self.cloud_healt_topic: str = f"isar/{isar_id}/cloud_health"
+        topic: str
+
         while True:
             try:
                 payload: str = self.telemetry_method(
                     isar_id=isar_id, robot_name=robot_name
                 )
+                topic = self.topic
             except RobotTelemetryException:
-                continue
+                payload: Type[CloudHealthPayload] = CloudHealthPayload(
+                    isar_id, robot_name, datetime.utcnow()
+                )
+                topic = self.cloud_healt_topic
+
+            self.publish(topic=topic, payload=payload, qos=self.qos, retain=self.retain)
 
-            self.publish(
-                topic=self.topic, payload=payload, qos=self.qos, retain=self.retain
-            )
             time.sleep(self.interval)
 
     def publish(
diff --git a/src/robot_interface/telemetry/payloads.py b/src/robot_interface/telemetry/payloads.py
index cea1e1d7..a557d882 100644
--- a/src/robot_interface/telemetry/payloads.py
+++ b/src/robot_interface/telemetry/payloads.py
@@ -15,6 +15,13 @@ class TelemetryPayload:
     timestamp: datetime
 
 
+@dataclass
+class CloudHealthPayload:
+    isar_id: str
+    robot_name: str
+    timestamp: datetime
+
+
 @dataclass
 class TelemetryPosePayload(TelemetryPayload):
     pose: Pose