added buggystate - NOTE: BREAKING COMMIT (haven't replaced instances …

…of deleted files)

rb_ws/src/buggy/scripts/auton/autonsystem.py

@@ -70,7 +70,7 @@ def __init__(self,
 
         # TODO: will need to be updated when we add namespaces
         self.sc_state = BuggyState("sc")
-        self.nand_state = BuggyState("sc")
+        self.nand_state = BuggyState("nand")
 
         # rospy.Subscriber(self_name + "/nav/odom", Odometry, self.update_self_odom)
 

rb_ws/src/buggy/scripts/auton/buggystate.py

@@ -1,12 +1,17 @@
 import numpy as np
 import utm
 from nav_msgs.msg import Odometry as ROSOdom
+from sensor_msgs.msg import NavSatFix
+
 from tf.transformations import euler_from_quaternion
 from tf.transformations import quaternion_from_euler
 
 from geometry_msgs.msg import PoseStamped
 from std_msgs.msg import Float32, Float64, Bool
 
+from microstrain_inertial_msgs.msg import GNSSFixInfo
+
+
 
 import rospy
 
@@ -17,41 +22,75 @@ class BuggyState:
 
     Other files should ONLY interface with the nav/odom messages through this file!
 
-    (functionally: this should replace pose and world)
+    (functionally: this should replace telematics, pose and world)
 
     """
-    # TODO: point of discussion: do we want to create a custom message for "utm_pose" since we are constantly passing around easting, northing, yaw? or do we just want to create a really simple wrapper for utm_pose with some translators to more publishable ros poses? (kind of in favor of this...)
-    #
+
     def __init__(self, name = "sc"):
-        self.use_gps = False
-        self.rosodom : ROSOdom = None
-        self.gpspose : PoseStamped = None
+        self.filter_odom : ROSOdom = None
+        self.gnss_1 : ROSOdom = None
+        self.gnss_2 : ROSOdom = None
+        self.gps_fix : int = 0
 
-        # to report if the filter position has separated (so we need to use the antenna position)
+
+        # to report if the filter position has separated (so we need to break)
         rospy.Subscriber(name + "/debug/filter_gps_seperation_status", Bool, self.update_use_gps)
 
         rospy.Subscriber(name + "/nav/odom", ROSOdom, self.update_odom)
+        rospy.Subscriber("/gnss1/odom", ROSOdom, self.update_gnss1)
+        rospy.Subscriber("/gnss2/odom", ROSOdom, self.update_gnss2)
+        rospy.Subscriber("/gnss1/fix_info", GNSSFixInfo, self.update_gnss1_fix)
 
-        # TODO: this node is reading in a posestamped, do we want to translate HERE or translate in telematics?
-        self.gnss_subscriber = rospy.Subscriber(name + "/gnss1/fix_Pose", PoseStamped, self.update_gnss)
 
     def update_use_gps(self, msg):
         self.use_gps = msg
-
     def update_odom(self, msg):
-        self.rosodom = msg
-
-    def update_gnss(self, msg):
-        self.gpspose = msg
+        self.filter_odom = msg
+    def update_gnss1(self, msg):
+        self.gnss_1 = msg
+    def update_gnss2(self, msg):
+        self.gnss_2 = msg
+    def update_gnss1_fix(self, msg):
+        self.gps_fix = msg.fix_type
+
+    def odom_to_navsatfix(self, odom):
+        """Convert Odometry-type to NavSatFix-type for plotting on Foxglove
+        Args:
+            odom (Odometry): odometry object to convert
+        """
+        lat = odom.pose.pose.position.y
+        long = odom.pose.pose.position.x
+        down = odom.pose.pose.position.z
+        new_odom = NavSatFix()
+        new_odom.header = odom.header
+        new_odom.latitude = lat
+        new_odom.longitude = long
+        new_odom.altitude = down
+        return new_odom
+
+    def get_gps_fix(self):
+        fix_string = "fix type: "
+        if (self.gps_fix == 0):
+            fix_string += "FIX_3D"
+        elif (self.gps_fix == 1):
+            fix_string += "FIX_2D"
+        elif (self.gps_fix == 2):
+            fix_string += "FIX_TIME_ONLY"
+        elif (self.gps_fix == 3):
+            fix_string += "FIX_NONE"
+        elif (self.gps_fix == 4):
+            fix_string += "FIX_INVALID"
+        elif (self.gps_fix == 5):
+            fix_string += "FIX_RTK_FLOAT"
+        else:
+            fix_string += "FIX_RTK_FIXED"
+        return fix_string
 
     def get_pose(self):
-        if self.rosodom == None:
+        if self.filter_odom == None:
             return None
 
-        if self.use_gps:
-            return self.gpspose.pose
-
-        rospose = self.rosodom.pose.pose
+        rospose = self.filter_odom.pose.pose
         yaw = (_, _, yaw) = euler_from_quaternion(
             [
                 rospose.orientation.x,
@@ -66,16 +105,12 @@ def get_pose(self):
         return (easting, northing, yaw)
 
     def get_pos_covariance(self):
-        if self.rosodom == None:
+        if self.filter_odom == None:
             return None
-        return self.rosodom.pose.covariance
+        return self.filter_odom.pose.covariance
 
-    # TODO: finish this!
     def get_velocity(self):
-        if self.rosodom == None:
+        if self.filter_odom == None:
             return None
-        return None
+        return self.filter_odom.twist.twist.linear
 
-    # TODO: finish this!
-    def create_odom(self):
-        return None

rb_ws/src/buggy/scripts/auton/pose.py

@@ -10,7 +10,7 @@ class Pose:
     A data structure for storing 2D poses, as well as a set of
     convenience methods for transforming/manipulating poses
 
-    TODO: All Pose objects are with respect to World coordinates.
+    NOTE: this class should be deprecated eventually! it is currently ONLY to be used in the MPC calculations where relevant, for all other purposes, use buggystate!
     """
 
     __x = None
@@ -49,12 +49,10 @@ def __init__(self, x, y, theta):
         self.y = y
         self.theta = theta
 
+    # NOTE: what is this O_o it is referenced in a file i do not understand.
     def __repr__(self) -> str:
         return f"Pose(x={self.x}, y={self.y}, theta={self.theta})"
 
-    def copy(self):
-        return Pose(self.x, self.y, self.theta)
-
     @property
     def x(self):
         return self.__x
@@ -107,20 +105,6 @@ def invert(self):
         """
         return Pose.from_mat(np.linalg.inv(self.to_mat()))
 
-    def convert_pose_from_global_to_local_frame(self, pose):
-        """
-        Converts the inputted pose from the global frame to the local frame
-        (relative to the current pose)
-        """
-        return pose / self
-
-    def convert_pose_from_local_to_global_frame(self, pose):
-        """
-        Converts the inputted pose from the local frame to the global frame
-        (relative to the current pose)
-        """
-        return pose * self
-
     def convert_point_from_global_to_local_frame(self, point):
         """
         Converts the inputted point from the global frame to the local frame
@@ -133,77 +117,6 @@ def convert_point_from_global_to_local_frame(self, point):
             point_hg_local[1] / point_hg_local[2],
         )
 
-    def convert_point_from_local_to_global_frame(self, point):
-        """
-        Converts the inputted point from the local frame to the global frame
-        (relative to the current pose)
-        """
-        point_hg = np.array([point[0], point[1], 1])
-        point_hg_global = self.to_mat() @ point_hg
-        return (
-            point_hg_global[0] / point_hg_global[2],
-            point_hg_global[1] / point_hg_global[2],
-        )
-
-    def convert_point_array_from_global_to_local_frame(self, points):
-        """
-        Converts the inputted point array from the global frame to the local frame
-        (relative to the current pose)
-
-        Args:
-            points (np.ndarray) [Size: (N,2)]: array of points to convert
-        """
-        points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
-        points_hg_local = np.linalg.inv(self.to_mat()) @ points_hg.T
-        return (points_hg_local[:2, :] / points_hg_local[2, :]).T
-
-    def convert_point_array_from_local_to_global_frame(self, points):
-        """
-        Converts the inputted point array from the local frame to the global frame
-        (relative to the current pose)
-
-        Args:
-            points (np.ndarray) [Size: (N,2)]: array of points to convert
-        """
-        points_hg = np.array([points[:, 0], points[:, 1], np.ones(len(points))])
-        points_hg_global = self.to_mat() @ points_hg.T
-        return (points_hg_global[:2, :] / points_hg_global[2, :]).T
-
-    def rotate(self, angle):
-        """
-        Rotates the pose by the given angle
-        """
-        self.theta += angle
-
-    def translate(self, x, y):
-        """
-        Translates the pose by the given x and y distances
-        """
-        self.x += x
-        self.y += y
-
-    def __add__(self, other):
-        return Pose(self.x + other.x, self.y + other.y, self.theta + other.theta)
-
-    def __sub__(self, other):
-        return Pose(self.x - other.x, self.y - other.y, self.theta - other.theta)
-
-    def __neg__(self):
-        return Pose(-self.x, -self.y, -self.theta)
-
-    def __mul__(self, other):
-        p1_mat = self.to_mat()
-        p2_mat = other.to_mat()
-
-        return Pose.from_mat(p1_mat @ p2_mat)
-
-    def __truediv__(self, other):
-        p1_mat = self.to_mat()
-        p2_mat = other.to_mat()
-
-        return Pose.from_mat(np.linalg.inv(p2_mat) @ p1_mat)
-
-
 if __name__ == "__main__":
     # TODO: again do we want example code in these classes
     rospose = ROSPose()