Fixed quaternion translation

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

rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py

@@ -9,13 +9,15 @@
 import argparse
 import rospy
 
+
 #Ros Message Imports
 from std_msgs.msg import Float64, Bool, Int8, UInt8
 from nav_msgs.msg import Odometry as ROSOdom
 
 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)