Add support for publishing telemetry

Author: aeshub

src/isar_turtlebot/robotinterface.py

@@ -1,12 +1,15 @@
 import os
 from pathlib import Path
-from typing import Sequence
+from queue import Queue
+from threading import Thread
+from typing import List, Sequence
 
 from alitra import MapAlignment, Transform, align_maps
 from robot_interface.models.initialize import InitializeParams
 from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission import InspectionStep, Step, StepStatus
 from robot_interface.robot_interface import RobotInterface
+from robot_interface.telemetry.mqtt_client import MqttTelemetryPublisher
 
 from isar_turtlebot.ros_bridge.ros_bridge import RosBridge, RosBridgeInterface
 from isar_turtlebot.turtlebot import Turtlebot
@@ -41,3 +44,38 @@ def get_inspections(self, step: InspectionStep) -> Sequence[Inspection]:
     def initialize(self, params: InitializeParams) -> None:
         if params.initial_pose:
             self.turtlebot.set_initial_pose(params.initial_pose)
+
+    def get_telemetry_publishers(self, queue: Queue, robot_id: str) -> List[Thread]:
+        publisher_threads: List[Thread] = []
+
+        pose_publisher: MqttTelemetryPublisher = MqttTelemetryPublisher(
+            mqtt_queue=queue,
+            telemetry_method=self.turtlebot.get_pose_telemetry,
+            topic=f"isar/{robot_id}/pose",
+            interval=1,
+            retain=True,
+        )
+        pose_thread: Thread = Thread(
+            target=pose_publisher.run,
+            args=[robot_id],
+            name="ISAR Turtlebot Pose Publisher",
+            daemon=True,
+        )
+        publisher_threads.append(pose_thread)
+
+        battery_publisher: MqttTelemetryPublisher = MqttTelemetryPublisher(
+            mqtt_queue=queue,
+            telemetry_method=self.turtlebot.get_battery_telemetry,
+            topic=f"isar/{robot_id}/battery",
+            interval=1,
+            retain=True,
+        )
+        battery_thread: Thread = Thread(
+            target=battery_publisher.run,
+            args=[robot_id],
+            name="ISAR Turtlebot Battery Publisher",
+            daemon=True,
+        )
+        publisher_threads.append(battery_thread)
+
+        return publisher_threads

src/isar_turtlebot/turtlebot/turtlebot.py

@@ -1,4 +1,6 @@
+import json
 import logging
+from datetime import datetime
 from logging import Logger
 from pathlib import Path
 from typing import Optional, Sequence
@@ -11,6 +13,11 @@
 )
 from robot_interface.models.inspection.inspection import Inspection
 from robot_interface.models.mission import InspectionStep, Step, StepStatus
+from robot_interface.telemetry.payloads import (
+    TelemetryBatteryPayload,
+    TelemetryPosePayload,
+)
+from robot_interface.utilities.json_service import EnhancedJSONEncoder
 
 from isar_turtlebot.models.turtlebot_status import Status
 from isar_turtlebot.ros_bridge import RosBridge
@@ -86,3 +93,17 @@ def _read_data(self, inspection_id: UUID) -> bytes:
 
     def set_initial_pose(self, pose: Pose) -> None:
         self.bridge.initial_pose.publish(encode_initial_pose(pose=pose))
+
+    def get_pose_telemetry(self, robot_id: str) -> str:
+        pose_payload: TelemetryPosePayload = TelemetryPosePayload(
+            pose=self.bridge.pose.get_value(),
+            robot_id=robot_id,
+            timestamp=datetime.utcnow(),
+        )
+        return json.dumps(pose_payload, cls=EnhancedJSONEncoder)
+
+    def get_battery_telemetry(self, robot_id: str) -> str:
+        battery_payload: TelemetryBatteryPayload = TelemetryBatteryPayload(
+            battery_level=95.2, robot_id=robot_id, timestamp=datetime.utcnow()
+        )
+        return json.dumps(battery_payload, cls=EnhancedJSONEncoder)