diff --git a/config/esw.yaml b/config/esw.yaml
index 511d9698b..beea9cbe4 100644
--- a/config/esw.yaml
+++ b/config/esw.yaml
@@ -1,5 +1,5 @@
right_gps_driver:
- port: "/dev/ttyACM1"
+ port: "/dev/gps"
baud: 38400
frame_id: "base_link"
diff --git a/launch/localization.launch b/launch/localization.launch
index f9491cafa..86cabfdae 100644
--- a/launch/localization.launch
+++ b/launch/localization.launch
@@ -2,6 +2,7 @@
+
@@ -26,8 +27,13 @@
-
-
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py
index 5ac0728f1..61b519132 100755
--- a/src/localization/gps_driver.py
+++ b/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()
diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py
index 3d76c5244..69926a3c0 100755
--- a/src/localization/gps_linearization.py
+++ b/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()