misc cleanup

src/localization/basestation_gps_driver.py

@@ -37,11 +37,11 @@ def main() -> None:
                         svin_complete = True
                         rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}")
                     if svin_started and not svin_complete:
-                        print(f"current accuracy: {msg.meanAcc}")
+                        rospy.loginfo(f"current accuracy: {msg.meanAcc}")
 
                 # fix quality information
                 elif msg.identity == "NAV-PVT":
-                    print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used")
+                    rospy.loginfo(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used")
 
 
 if __name__ == "__main__":

src/localization/gps_driver.py

@@ -6,16 +6,10 @@
 acquire an RTK fix.
 """
 import serial
-import asyncio
 import rospy
 import threading
-import numpy as np
-from os import getenv
-
 import rospy
-
-# from rover_msgs import GPS, RTCM
-from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage
+from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL
 from std_msgs.msg import Header
 from sensor_msgs.msg import NavSatFix
 from rtcm_msgs.msg import Message
@@ -24,6 +18,17 @@
 
 
 class GPS_Driver:
+    port: str
+    baud: int
+    base_station_sub: rospy.Subscriber
+    gps_pub: rospy.Publisher
+    rtk_fix_pub: rospy.Publisher
+    lock: threading.Lock
+    valid_offset: bool
+    time_offset: float
+    ser: serial.Serial
+    reader: UBXReader
+
     def __init__(self):
         rospy.init_node("gps_driver")
         self.port = rospy.get_param("port")
@@ -47,33 +52,30 @@ def exit(self) -> None:
         # close connection
         self.ser.close()
 
+    # rospy subscriber automatically runs this callback in separate thread
     def process_rtcm(self, data) -> None:
         print("processing RTCM")
         with self.lock:
-            # rtcm_data = RTCM.decode(data)
             self.ser.write(data.message)
 
-    def parse_rover_gps_data(self, rover_gps_data) -> None:
-        msg = rover_gps_data
+    def parse_ubx_message(self, msg) -> None:
         # skip if message could not be parsed
         if not msg:
             return
 
-        msg_used = None
-
-        if rover_gps_data.identity == "RXM-RTCM":
+        if msg.identity == "RXM-RTCM":
             rospy.loginfo("RXM")
+            msg_used = msg.msgUsed
 
             if msg_used == 0:
                 rospy.logwarn("RTCM Usage unknown\n")
             elif msg_used == 1:
                 rospy.logwarn("RTCM message not used\n")
             elif msg_used == 2:
                 rospy.loginfo("RTCM message successfully used by receiver\n")
-
             # rospy.loginfo(vars(rover_gps_data))
 
-        if rover_gps_data.identity == "NAV-PVT":
+        elif msg.identity == "NAV-PVT":
             rospy.loginfo("PVT")
             parsed_latitude = msg.lat
             parsed_longitude = msg.lon
@@ -85,9 +87,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None:
                 self.valid_offset = True
 
             time = time + self.time_offset
-
-            rospy.loginfo_throttle(3, f"{time} {rospy.Time.now()} {time-rospy.Time.now()} {self.time_offset}")
-
+            # rospy.loginfo_throttle(3, f"{time} {rospy.Time.now()} {time-rospy.Time.now()} {self.time_offset}")
             message_header = Header(stamp=time, frame_id="base_link")
 
             self.gps_pub.publish(
@@ -98,37 +98,31 @@ def parse_rover_gps_data(self, rover_gps_data) -> None:
                     altitude=parsed_altitude,
                 )
             )
-            self.rtk_fix_pub.publish(rtkStatus(msg_used))
+            self.rtk_fix_pub.publish(rtkStatus(msg.carrSoln))
 
             if msg.difSoln == 1:
                 rospy.loginfo_throttle(3, "Differential correction applied")
-
-            # publidh to navsatstatus in navsatfix
             if msg.carrSoln == 0:
                 rospy.logwarn_throttle(3, "No RTK")
             elif msg.carrSoln == 1:
                 rospy.loginfo_throttle(3, "Floating RTK Fix")
             elif msg.carrSoln == 2:
                 rospy.loginfo_throttle(3, "RTK FIX")
 
-        if rover_gps_data.identity == "NAV-STATUS":
+        elif msg.identity == "NAV-STATUS":
             pass
 
     def gps_data_thread(self) -> None:
-        # TODO: add more message checks if needed
         while not rospy.is_shutdown():
             with self.lock:
                 if self.ser.in_waiting:
-                    raw, rover_gps_data = self.reader.read()
-                    parsed_gps_data = self.parse_rover_gps_data(rover_gps_data)
+                    raw, msg = self.reader.read()
+                    self.parse_ubx_message(msg)
 
 
 def main():
-    # change values
     rtk_manager = GPS_Driver()
     rtk_manager.connect()
-    # rover_gps_thread = threading.Thread(target=rtk_manager.gps_data_thread)
-    # rover_gps_thread.start()
     rtk_manager.gps_data_thread()
     rtk_manager.exit()
 

src/localization/gps_linearization.py

@@ -21,8 +21,9 @@ class GPSLinearization:
     orientation into a pose estimate for the rover and publishes it.
     """
 
-    last_gps_msg: Optional[np.ndarray]
+    last_gps_msg: Optional[NavSatFix]
     last_imu_msg: Optional[ImuAndMag]
+    last_pose: Optional[SE3]
     pose_publisher: rospy.Publisher
 
     # reference coordinates
@@ -59,7 +60,7 @@ def __init__(self):
             right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix)
             left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix)
             sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5)
-            sync_gps_sub.registerCallback(self.duo_gps_callback)
+            sync_gps_sub.registerCallback(self.dual_gps_callback)
         else:
             rospy.Subscriber("left_gps_driver/fix", NavSatFix, self.single_gps_callback)
 
@@ -85,7 +86,7 @@ def single_gps_callback(self, msg: NavSatFix):
             self.last_pose = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord)
             self.publish_pose()
 
-    def duo_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix):
+    def dual_gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix):
         """
         Callback function that receives GPS messages, assigns their covariance matrix,
         and then publishes the linearized pose.
@@ -153,10 +154,8 @@ def publish_pose(self):
         self.pose_publisher.publish(pose_msg)
 
     @staticmethod
-    def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray:
+    def compute_gps_pose(right_cartesian, left_cartesian) -> SE3:
         # TODO: give simulated GPS non zero altitude so we can stop erasing the z component
-        # left_cartesian[2] = 0
-        # right_cartesian[2] = 0
         vector_connecting = left_cartesian - right_cartesian
         vector_connecting[2] = 0
         magnitude = np.linalg.norm(vector_connecting)
@@ -179,7 +178,7 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray:
         return pose
 
     @staticmethod
-    def get_linearized_pose_in_world(gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray) -> np.ndarray:
+    def get_linearized_pose_in_world(gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray) -> SE3:
         """
         Linearizes the GPS geodetic coordinates into ENU cartesian coordinates,
         then combines them with the IMU orientation into a pose estimate relative