From 7a169c18e21ebd96127224dae101c386b7b2f799 Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Fri, 5 Apr 2024 20:53:38 -0400
Subject: [PATCH] Fixed quaternion translation

---
 rb_ws/src/buggy/scripts/auton/pose.py           |  5 +++++
 .../src/buggy/scripts/serial/ros_to_bnyahaj.py  | 17 +++++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/rb_ws/src/buggy/scripts/auton/pose.py b/rb_ws/src/buggy/scripts/auton/pose.py
index e08aa7a8..ccf1ae05 100755
--- a/rb_ws/src/buggy/scripts/auton/pose.py
+++ b/rb_ws/src/buggy/scripts/auton/pose.py
@@ -2,6 +2,7 @@
 
 from geometry_msgs.msg import Pose as ROSPose
 from tf.transformations import euler_from_quaternion
+from tf.transformations import quaternion_from_euler
 
 
 class Pose:
@@ -38,6 +39,10 @@ def rospose_to_pose(rospose: ROSPose):
         p = Pose(rospose.position.x, rospose.position.y, yaw)
         return p
 
+    def heading_to_quaternion(heading):
+        q = quaternion_from_euler(0, 0, heading)
+        return q
+
     def __init__(self, x, y, theta):
         self.x = x
         self.y = y
diff --git a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
index d5b70ccb..44764cca 100755
--- a/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
+++ b/rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
@@ -9,6 +9,7 @@
 import argparse
 import rospy
 
+
 #Ros Message Imports
 from std_msgs.msg import Float64, Bool, Int8, UInt8
 from nav_msgs.msg import Odometry as ROSOdom
@@ -16,6 +17,7 @@
 from host_comm import *
 
 from world import World
+from pose import Pose
 
 class Translator:
     def __init__(self, self_name, other_name, teensy_name):
@@ -90,6 +92,21 @@ def reader_thread(self):
                     self.odom_publisher.publish(odom)
                 except Exception as e:
                     rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
+            
+            elif isinstance(packet, BnyaTelemetry):
+                rospy.logdebug("packet", packet)
+                odom = ROSOdom()
+                try:
+                    lat, long = World.utm_to_gps(packet.y, packet.x)
+                    odom.pose.pose.position.x = long
+                    odom.pose.pose.position.y = lat
+                    odom.twist.twist.angular.z = packet.heading_rate
+                    odom.pose.pose.orientation = Pose.heading_to_quaternion(packet.heading)
+
+                    self.odom_publisher.publish(odom)
+                except Exception as e:
+                    rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
+
 
             elif isinstance(packet, tuple): #Are there any other packet that is a tuple
                 # print(packet)