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()