From e74ef5446dbc098c132e36ebc71480c2b358b940 Mon Sep 17 00:00:00 2001
From: TiaSinghania <samanvita.singhania@gmail.com>
Date: Mon, 10 Jun 2024 11:20:49 -0700
Subject: [PATCH] created skeleton of buggy-state

---
 rb_ws/src/buggy/scripts/auton/buggystate.py | 48 +++++++++++++++------
 1 file changed, 36 insertions(+), 12 deletions(-)

diff --git a/rb_ws/src/buggy/scripts/auton/buggystate.py b/rb_ws/src/buggy/scripts/auton/buggystate.py
index 2a3ca5a7..93183416 100644
--- a/rb_ws/src/buggy/scripts/auton/buggystate.py
+++ b/rb_ws/src/buggy/scripts/auton/buggystate.py
@@ -1,4 +1,5 @@
 import numpy as np
+import utm
 from nav_msgs.msg import Odometry as ROSOdom
 from tf.transformations import euler_from_quaternion
 from tf.transformations import quaternion_from_euler
@@ -17,18 +18,41 @@ class BuggyState:
     """
     def __init__(self):
         self.rosodom : ROSOdom = None
-        rospy.Subscriber("/nav/odom", ROSOdom, self.get_odom)
+        rospy.Subscriber("/nav/odom", ROSOdom, self.update_odom)
 
-    def get_odom(self, msg):
+    def update_odom(self, msg):
         self.rosodom = msg
 
-    """
-    FUNCTIONALITY TO ADD:
-        get pose (in utm)
-        get velocity (in utm)
-        get covariance (in whatever units)
-        create a new rosodom message using some (maybe not all) information (by default set missing info to garbage vals)
-
-        how does this interact with gps coords and all
-        can we just make this combine with that
-    """
\ No newline at end of file
+    def get_pose(self):
+        if self.rosodom == None:
+            return None
+
+        rospose = self.rosodom.pose.pose
+        yaw = (_, _, yaw) = euler_from_quaternion(
+            [
+                rospose.orientation.x,
+                rospose.orientation.y,
+                rospose.orientation.z,
+                rospose.orientation.w,
+            ]
+        )
+
+        easting, northing = utm.from_latlon(rospose.position.x, rospose.position.y)
+
+        return (easting, northing, yaw)
+
+
+    def get_pos_covariance(self):
+        if self.rosodom == None:
+            return None
+        return self.rosodom.pose.covariance
+
+    # TODO: finish this!
+    def get_velocity(self):
+        if self.rosodom == None:
+            return None
+        return None
+
+    # TODO: finish this!
+    def create_odom(self):
+        return None