Testing changes

config/esw.yaml

@@ -1,5 +1,5 @@
 right_gps_driver:
-  port: "/dev/ttyACM1"
+  port: "/dev/gps"
   baud: 38400
   frame_id: "base_link"
 

launch/localization.launch

@@ -2,6 +2,7 @@
     <arg name="sim" default="false" />
     <arg name="use_ekf" default="true" />
     <arg name="ekf_start_delay" default="0" />
+    <arg name="use_both_gps" default="true" />
 
     <rosparam command="load" file="$(find mrover)/config/localization.yaml" />
     <rosparam command="load" file="$(find mrover)/config/esw.yaml" />
@@ -26,8 +27,13 @@
     <node unless="$(arg sim)" name="imu_driver" pkg="mrover" type="imu_driver.py" output="screen" />
 
     <!-- Launch GPS driver -->
-    <group ns="gps">
-        <node unless="$(arg sim)" name="gps_driver" pkg="mrover" type="gps_driver.py" output="screen" />
+
+    <group ns="right_gps_driver">
+        <node if="$(arg use_both_gps)" pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
+    </group>
+
+    <group ns="left_gps_driver">
+        <node pkg="mrover" type="gps_driver.py" name="gps_driver" output="screen"/>
     </group>
 
 </launch>

src/localization/gps_driver.py

@@ -19,7 +19,7 @@
 from std_msgs.msg import Header
 from sensor_msgs.msg import NavSatFix
 from rtcm_msgs.msg import Message
-# from mrover.msg import rtkStatus
+from mrover.msg import rtkStatus
 import datetime
 
 
@@ -30,7 +30,7 @@ def __init__(self):
         self.baud = rospy.get_param("baud")
         self.base_station_sub = rospy.Subscriber("/rtcm", Message, self.process_rtcm)
         self.gps_pub = rospy.Publisher("fix", NavSatFix, queue_size=1)
-        # self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1)
+        self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1)
 
         self.lock = threading.Lock()
         self.valid_offset = False
@@ -58,10 +58,12 @@ def parse_rover_gps_data(self, rover_gps_data) -> None:
         # skip if message could not be parsed
         if not msg:
             return
+
+        msg_used = None
+
 
         if rover_gps_data.identity == "RXM-RTCM":
             rospy.loginfo("RXM")
-            msg_used = msg.msgUsed
 
             if msg_used == 0:
                 rospy.logwarn("RTCM Usage unknown\n")
@@ -97,7 +99,7 @@ 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_used))
 
             if msg.difSoln == 1:
                 rospy.loginfo_throttle(3, "Differential correction applied")
@@ -126,7 +128,7 @@ def main():
     # change values
     rtk_manager = GPS_Driver()
     rtk_manager.connect()
-    # rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread)
+    # 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

@@ -42,9 +42,10 @@ def __init__(self):
         self.ref_lat = rospy.get_param("gps_linearization/reference_point_latitude")
         self.ref_lon = rospy.get_param(param_name="gps_linearization/reference_point_longitude")
         self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude")
-        self.both_gps = True
+        self.both_gps = rospy.get_param("gps_linearization/use_both_gps")
         self.world_frame = rospy.get_param("world_frame")
         self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance")
+        self.ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt])
 
         # config gps and imu convariance matrices
         self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None))
@@ -79,12 +80,12 @@ def single_gps_callback(self, msg: NavSatFix):
         if not self.use_dop_covariance:
             msg.position_covariance = self.config_gps_covariance
 
-        ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt])
+
 
         print("using single callback")
 
         if self.last_imu_msg is not None:
-            self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, ref_coord)
+            self.last_gps_msg = self.get_linearized_pose_in_world(msg, self.last_imu_msg, self.ref_coord)
             self.publish_pose()