From 5479b6336fff75408417d97d552cbed61d947fd3 Mon Sep 17 00:00:00 2001 From: Rishi Kumar Date: Tue, 7 Jan 2025 19:06:23 +0000 Subject: [PATCH] Added converstion from utm -> latlon --- .../buggy/buggy/visualization/telematics.py | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/rb_ws/src/buggy/buggy/visualization/telematics.py b/rb_ws/src/buggy/buggy/visualization/telematics.py index 0a7e4d3..9d1fd80 100644 --- a/rb_ws/src/buggy/buggy/visualization/telematics.py +++ b/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__":