From 9dc36e042824ff6ddf7ba2e6973fb8e6c09bdceb Mon Sep 17 00:00:00 2001
From: Mehul Goel <mehulgoel873@gmail.com>
Date: Fri, 5 Apr 2024 18:14:28 -0700
Subject: [PATCH] Ukf (#82)

* Add new message type

* Fixed quaternion translation

* Sign convention

* pylint

* Co-authored-by: Mehul Goel <mehulgoel873@users.noreply.github.com>

---------

Co-authored-by: Carson Swoveland <sciencedude2003@gmail.com>
Co-authored-by: TiaSinghania <samanvita.singhania@gmail.com>
---
 rb_ws/src/buggy/scripts/auton/pose.py         |  7 ++++++-
 rb_ws/src/buggy/scripts/serial/host_comm.py   | 13 ++++++++++++
 .../buggy/scripts/serial/ros_to_bnyahaj.py    | 20 ++++++++++++++++++-
 3 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/auton/pose.py b/rb_ws/src/buggy/scripts/auton/pose.py
index e08aa7a8..7bb09e94 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
@@ -94,7 +99,7 @@ def from_mat(mat):
         Creates a pose from a 3x3 homogeneous transformation matrix
         """
         return Pose(mat[0, 2], mat[1, 2], np.arctan2(mat[1, 0], mat[0, 0]))
-    
+
     def invert(self):
         """
         Inverts the pose
diff --git a/rb_ws/src/buggy/scripts/serial/host_comm.py b/rb_ws/src/buggy/scripts/serial/host_comm.py
index 14d4e932..0d882e33 100644
--- a/rb_ws/src/buggy/scripts/serial/host_comm.py
+++ b/rb_ws/src/buggy/scripts/serial/host_comm.py
@@ -63,6 +63,7 @@ def update(self, b):
 MSG_TYPE_STEERING = b'ST'
 MSG_TYPE_BRAKE    = b'BR'
 MSG_TYPE_ALARM    = b'AL'
+MSG_TYPE_BNYATEL  = b'BT'
 
 @dataclass
 class Odometry:
@@ -71,6 +72,15 @@ class Odometry:
     radio_seqnum: int
     gps_seqnum: int
 
+@dataclass
+class BnyaTelemetry:
+    x: float
+    y: float
+    velocity: float # In METERS / SECOND
+    steering: float # In DEGREES
+    heading: float  # In RADIANS
+    heading_rate: float  # In RADIANS / SECOND
+
 class IncompletePacket(Exception):
     pass
 
@@ -183,6 +193,9 @@ def read_packet(self):
             debug = struct.unpack('<fff??B?BBxx', payload)
             # print(debug)
             return debug
+        elif msg_type == MSG_TYPE_BNYATEL:
+            x, y, vel, steering, heading, heading_rate = struct.unpack('<ffffff', payload)
+            return BnyaTelemetry(x, y, vel, steering, heading, heading_rate)
         else:
             return None
             # print(f'Unknown packet type {msg_type}')
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..6f0aa77e 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,8 @@
 from host_comm import *
 
 from world import World
+from pose import Pose
+import numpy as np
 
 class Translator:
     def __init__(self, self_name, other_name, teensy_name):
@@ -78,7 +81,7 @@ def reader_thread(self):
         while True:
             packet = self.comms.read_packet()
             # print("trying to read odom")
-            if isinstance(packet, Odometry):
+            if isinstance(packet, Odometry) and self_name == "SC":
                 rospy.logdebug("packet", packet)
                 #Publish to odom topic x and y coord
                 odom = ROSOdom()
@@ -91,6 +94,21 @@ def reader_thread(self):
                 except Exception as e:
                     rospy.logwarn("Unable to convert other buggy position to lon lat" + e)
 
+            elif isinstance(packet, BnyaTelemetry) and self_name == "NAND":
+                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 + np.pi/2)
+
+                    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)
                 self.rc_steering_angle_publisher.publish(Float64(packet[0]))