Added converstion from utm -> latlon

rb_ws/src/buggy/buggy/visualization/telematics.py

@@ -2,6 +2,7 @@
 # Runs the conversion script for all telematics data
 
 import rclpy
+import utm
 from nav_msgs.msg import Odometry
 from rclpy.node import Node
 from sensor_msgs.msg import NavSatFix
@@ -35,15 +36,22 @@ def convert_buggystate(self, msg, publisher):
             msg (Odometry): Buggy state to convert
             publisher (Publisher): Publisher to send NavSatFix message to
         """
-        lat = msg.pose.pose.position.y
-        long = msg.pose.pose.position.x
-        down = msg.pose.pose.position.z
-        new_msg = NavSatFix()
-        new_msg.header = msg.header
-        new_msg.latitude = lat
-        new_msg.longitude = long
-        new_msg.altitude = down
-        publisher.publish(new_msg)
+        try:
+            y = msg.pose.pose.position.y
+            x = msg.pose.pose.position.x
+            lat, long = utm.to_latlon(x, y, 17, "T")
+            down = msg.pose.pose.position.z
+            new_msg = NavSatFix()
+            new_msg.header = msg.header
+            new_msg.latitude = lat
+            new_msg.longitude = long
+            new_msg.altitude = down
+            publisher.publish(new_msg)
+
+        except Exception as e:
+            self.get_logger().warn(
+                "Unable to convert other buggy position to lon lat" + str(e)
+            )
 
 
 if __name__ == "__main__":