def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_pose_utm = GpsLocal() current_pose_utm.header.stamp = current_time current_pose_utm.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: self.seq = self.seq + 1 current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] gps_qual = data['fix_type'] try: # Unpack the fix params for this quality value current_fix.status.status, self.default_epe, self.fix_type = self.fix_mappings[ gps_qual] except KeyError: current_fix.status.status = NavSatStatus.STATUS_NO_FIX self.fix_type = 'Unknown' if gps_qual == 0: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.using_receiver_epe = False self.invalid_cnt += 1 current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = self.default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = self.default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (hdop * self.alt_std_dev)**2 # FIXME # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(latitude) and not math.isnan(longitude) and ( gps_qual == 5 or gps_qual == 4): UTMNorthing, UTMEasting = LLtoUTM(latitude, longitude)[0:2] current_pose_utm.position.x = UTMEasting current_pose_utm.position.y = UTMNorthing current_pose_utm.position.z = altitude # Pose x/y/z covariance is whatever we decided h & v covariance is. # Here is it the same as for ECEF coordinates current_pose_utm.covariance[0] = (hdop * self.lon_std_dev)**2 current_pose_utm.covariance[4] = (hdop * self.lat_std_dev)**2 current_pose_utm.covariance[8] = (hdop * self.alt_std_dev)**2 current_pose_utm.rtk_fix = True if gps_qual == 4 else False self.local_pub.publish(current_pose_utm) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) if (self.diag_pub_time < rospy.get_time()): self.diag_pub_time += 1 diag_arr = DiagnosticArray() diag_arr.header.stamp = rospy.get_rostime() diag_arr.header.frame_id = frame_id diag_msg = DiagnosticStatus() diag_msg.name = 'GPS_status' diag_msg.hardware_id = 'GPS' diag_msg.level = DiagnosticStatus.OK diag_msg.message = 'Received GGA Fix' diag_msg.values.append( KeyValue('Sequence number', str(self.seq))) diag_msg.values.append( KeyValue('Invalid fix count', str(self.invalid_cnt))) diag_msg.values.append(KeyValue('Latitude', str(latitude))) diag_msg.values.append(KeyValue('Longitude', str(longitude))) diag_msg.values.append(KeyValue('Altitude', str(altitude))) diag_msg.values.append(KeyValue('GPS quality', str(gps_qual))) diag_msg.values.append(KeyValue('Fix type', self.fix_type)) diag_msg.values.append( KeyValue('Number of satellites', str(data['num_satellites']))) diag_msg.values.append( KeyValue('Receiver providing accuracy', str(self.using_receiver_epe))) diag_msg.values.append(KeyValue('Hdop', str(hdop))) diag_msg.values.append( KeyValue('Latitude std dev', str(hdop * self.lat_std_dev))) diag_msg.values.append( KeyValue('Longitude std dev', str(hdop * self.lon_std_dev))) diag_msg.values.append( KeyValue('Altitude std dev', str(hdop * self.alt_std_dev))) diag_arr.status.append(diag_msg) self.diag_pub.publish(diag_arr) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = GPS() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if data['fix_valid']: self.speed = data['speed'] if not math.isnan(data['true_course']): self.ground_course = data['true_course'] if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual > 0: current_fix.fix = True current_fix.NumSat = data['num_satellites'] # current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.covariance = hdop**2 # current_fix.position_covariance[4] = hdop ** 2 # current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME # current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude current_fix.speed = self.speed current_fix.ground_course = self.ground_course self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: current_fix.fix = True current_fix.NumSat = data['num_satellites'] # current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') # current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_UNKNOWN if data['fix_valid']: current_fix.speed = data['speed'] if not math.isnan(data['true_course']): current_fix.ground_course = data['true_course'] else: current_fix.ground_course = self.ground_course self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = GPS() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if "RMC" in parsed_sentence: data = parsed_sentence["RMC"] if data["fix_valid"]: self.speed = data["speed"] if not math.isnan(data["true_course"]): self.ground_course = data["true_course"] if not self.use_RMC and "GGA" in parsed_sentence: data = parsed_sentence["GGA"] gps_qual = data["fix_type"] if gps_qual > 0: current_fix.fix = True current_fix.NumSat = 4 # current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data["latitude"] if data["latitude_direction"] == "S": latitude = -latitude current_fix.latitude = latitude longitude = data["longitude"] if data["longitude_direction"] == "W": longitude = -longitude current_fix.longitude = longitude hdop = data["hdop"] current_fix.covariance = hdop ** 2 # current_fix.position_covariance[4] = hdop ** 2 # current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME # current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data["altitude"] + data["mean_sea_level"] current_fix.altitude = altitude current_fix.speed = self.speed current_fix.ground_course = self.ground_course self.fix_pub.publish(current_fix) if not math.isnan(data["utc_time"]): current_time_ref.time_ref = rospy.Time.from_sec(data["utc_time"]) self.time_ref_pub.publish(current_time_ref) elif "RMC" in parsed_sentence: data = parsed_sentence["RMC"] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: current_fix.fix = True current_fix.NumSat = 4 # current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data["latitude"] if data["latitude_direction"] == "S": latitude = -latitude current_fix.latitude = latitude longitude = data["longitude"] if data["longitude_direction"] == "W": longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float("NaN") # current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_UNKNOWN if data["fix_valid"]: current_fix.speed = data["speed"] if not math.isnan(data["true_course"]): current_fix.ground_course = data["true_course"] else: current_fix.ground_course = self.ground_course self.fix_pub.publish(current_fix) if not math.isnan(data["utc_time"]): current_time_ref.time_ref = rospy.Time.from_sec(data["utc_time"]) self.time_ref_pub.publish(current_time_ref) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] self.valid_fix = (fix_type > 0) current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time(data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin( data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos( data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): """Public method to provide a new NMEA sentence to the driver. Args: nmea_string (str): NMEA sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp(rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. Returns: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not check_nmea_checksum(nmea_string): #rospy.logwarn("Received a sentence with an invalid checksum. " + # "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: #rospy.logdebug( # "Failed to parse NMEA sentence. Sentence was: %s" % # nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): #rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False self.current_nmea.data = nmea_string if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() self.current_fix.header.stamp = current_time self.current_fix.header.frame_id = frame_id self.current_fix.status.header.stamp = current_time self.current_fix.status.header.frame_id = frame_id if 'GGA' in parsed_sentence: self.current_fix.position_covariance_type = \ GPSFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] self.current_fix.status.status = gps_qual[1] self.current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude self.current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude self.current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] self.current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 self.current_fix.hdop = data['hdop'] self.current_fix.position_covariance[0] = (self.current_fix.hdop * self.lon_std_dev)**2 self.current_fix.position_covariance[4] = (self.current_fix.hdop * self.lat_std_dev)**2 self.current_fix.position_covariance[8] = ( 2 * self.current_fix.hdop * self.alt_std_dev)**2 # FIXME if not math.isnan(data['utc_time']): dt_time = datetime.fromtimestamp(data['utc_time']) self.current_fix.time = dt_time.hour * 10000 + dt_time.minute * 100 + dt_time.second self.last_valid_fix_time = rospy.Time.from_sec( data['utc_time']) if 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: self.current_fix.track = data['true_course'] self.current_fix.speed = data['speed'] if 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: self.current_fix.status.status = GPSStatus.STATUS_FIX else: self.current_fix.status.status = GPSStatus.STATUS_NO_FIX latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude self.current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude self.current_fix.longitude = longitude self.current_fix.altitude = float('NaN') self.current_fix.position_covariance_type = \ GPSFix.COVARIANCE_TYPE_UNKNOWN if not math.isnan(data['utc_time']): self.current_fix.time = data['utc_time'] # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: self.current_fix.track = data['true_course'] self.current_fix.speed = data['speed'] if 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] if 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: self.current_fix.dip = math.radians(data['heading']) # current_heading = QuaternionStamped() # current_heading.header.stamp = current_time # current_heading.header.frame_id = frame_id # q = quaternion_from_euler(0, 0, math.radians(data['heading'])) # current_heading.quaternion.x = q[0] # current_heading.quaternion.y = q[1] # current_heading.quaternion.z = q[2] # current_heading.quaternion.w = q[3] # self.heading_pub.publish(current_heading) # else: # return False self.fix_pub.publish(self.current_fix) self.nmea_pub.publish(self.current_nmea)
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and "GGA" in parsed_sentence: data = parsed_sentence["GGA"] gps_qual = data["fix_type"] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data["latitude"] if data["latitude_direction"] == "S": latitude = -latitude current_fix.latitude = latitude longitude = data["longitude"] if data["longitude_direction"] == "W": longitude = -longitude current_fix.longitude = longitude hdop = data["hdop"] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level # altitude = data['altitude'] + data['mean_sea_level'] altitude = data["altitude"] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data["utc_time"]): current_time_ref.time_ref = rospy.Time.from_sec(data["utc_time"]) self.time_ref_pub.publish(current_time_ref) elif "RMC" in parsed_sentence: data = parsed_sentence["RMC"] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data["fix_valid"]: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data["latitude"] if data["latitude_direction"] == "S": latitude = -latitude current_fix.latitude = latitude longitude = data["longitude"] if data["longitude_direction"] == "W": longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float("NaN") current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data["utc_time"]): current_time_ref.time_ref = rospy.Time.from_sec(data["utc_time"]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data["fix_valid"]: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data["speed"] * math.sin(data["true_course"]) current_vel.twist.linear.y = data["speed"] * math.cos(data["true_course"]) current_vel.twist.angular.z = data["true_course"] self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # elif 'RMC' in parsed_sentence: # data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. # if self.use_RMC: # if data['fix_valid']: # current_fix.status.status = NavSatStatus.STATUS_FIX # else: # current_fix.status.status = NavSatStatus.STATUS_NO_FIX # current_fix.status.service = NavSatStatus.SERVICE_GPS # latitude = data['latitude'] # if data['latitude_direction'] == 'S': # latitude = -latitude # current_fix.latitude = latitude # longitude = data['longitude'] # if data['longitude_direction'] == 'W': # longitude = -longitude # current_fix.longitude = longitude # current_fix.altitude = float('NaN') # current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_UNKNOWN # self.fix_pub.publish(current_fix) # if not math.isnan(data['utc_time']): # current_time_ref.time_ref = rospy.Time.from_sec( # data['utc_time']) # self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: self.heading = math.radians(data['heading']) current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'HDG' in parsed_sentence: data = parsed_sentence['HDG'] if data['heading']: magnetic_current_heading = QuaternionStamped() magnetic_current_heading.header.stamp = current_time magnetic_current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) magnetic_current_heading.quaternion.x = q[0] magnetic_current_heading.quaternion.y = q[1] magnetic_current_heading.quaternion.z = q[2] magnetic_current_heading.quaternion.w = q[3] self.magnetic_heading_pub.publish(magnetic_current_heading) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] if data['speed']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] self.vel_pub.publish(current_vel) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: #rospy.loginfo('GGA') data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude #hdop = data['hdop'] #current_fix.position_covariance[0] = hdop ** 2 #current_fix.position_covariance[4] = hdop ** 2 #current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME #current_fix.position_covariance_type = \ # NavSatFix.COVARIANCE_TYPE_APPROXIMATED # covariances diagonals as rospy arguments current_fix.position_covariance[0] = self.gps_covariance_pos[0] current_fix.position_covariance[4] = self.gps_covariance_pos[1] current_fix.position_covariance[8] = self.gps_covariance_pos[2] current_fix.position_covariance_type =\ NavSatFix.COVARIANCE_TYPE_DIAGONAL_KNOWN # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(int(int(timestamp.to_sec())/(60*60))*(60*60)+data['utc_time']) self.time_ref_pub.publish(current_time_ref) if self.use_GPS_time: self.gps_time = current_time_ref.time_ref current_fix.header.stamp = current_time_ref.time_ref current_fix.header.stamp = current_fix.header.stamp+rospy.Duration(self.time_delay) #print(timestamp.to_sec()) self.fix_pub.publish(current_fix) #if not math.isnan(data['utc_time']): # current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) # self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] #rospy.loginfo('RMC') # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] track_made_good_degrees_true = data['track_made_good_degrees_true'] track_made_good_degrees_magnetic = data['track_made_good_degrees_magnetic'] speed = data['speed'] SPEED_OVER_GROUND = data['speed_over_ground'] if not math.isnan(track_made_good_degrees_true): DIRECTION_REFERENCE = "True" COURSE_OVER_GROUND = track_made_good_degrees_true elif not math.isnan(track_made_good_degrees_magnetic): DIRECTION_REFERENCE = "Magnetic" COURSE_OVER_GROUND = track_made_good_degrees_magnetic else: DIRECTION_REFERENCE = "Null" COURSE_OVER_GROUND = float(0) SPEED_OVER_GROUND = float('NaN') current_vtg = Course_Speed() current_vtg.header.stamp = current_time current_vtg.header.frame_id = frame_id current_vtg.DIRECTION_REFERENCE = DIRECTION_REFERENCE current_vtg.COURSE_OVER_GROUND = COURSE_OVER_GROUND current_vtg.SPEED_OVER_GROUND = SPEED_OVER_GROUND self.vtg_pub.publish(current_vtg) #rospy.loginfo(track_made_good_degrees_magnetic) elif 'HDT' in parsed_sentence: #rospy.loginfo('HDT') data = parsed_sentence['HDT'] heading_degrees = data['heading_degrees'] current_hdt = GPHDT() current_hdt.header.stamp = current_time current_hdt.header.frame_id = frame_id current_hdt.HEADING_DEGREES = heading_degrees if self.use_GPS_time and not self.gps_time==None: current_hdt.header.stamp = self.gps_time-rospy.Duration(0,1000000) elif self.use_GPS_time: current_hdt.header.stamp.secs = 0 current_hdt.header.stamp = current_hdt.header.stamp+rospy.Duration(self.time_delay)+rospy.Duration(self.time_delay_heading) #print(current_hdt.header.stamp.to_sec()) self.hdt_pub.publish(current_hdt) else: rospy.loginfo('Not valid') return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): global lat_error, lon_error,started, trustedCount, badFlag,fixCount, gst_rms global rmc_lon,rmc_lat, hdop_GPGSA, pdop_GPGSA, vdop_GPGSA, GPGSA_fix global hdop_GAGSA, pdop_GAGSA, vdop_GAGSA, hdop_GLGSA, pdop_GLGSA, vdop_GLGSA global SNR1,SNR2,SNR3, SNR4, hdop_GGA global rmc_longitude, rmc_latitude, rmc_altitude, gpsQuality global gga_longitude, gga_latitude , gga_altitude global kph_speed, age_of_diff global out_cov, utc_time, satNo, gps_qual global gps_line, gps_line_first_half, gps_line_second_half global received_time_is_nan, received_utc_time, previous_received_utc_time global VTG_received, GGA_received, use_gps_date_once, file_name, publish_gps #if rospy.has_param('publish_gps'): # publish_gps = rospy.get_param('publish_gps') trustedFix = NavSatFix() unTrustedFix = NavSatFix() trustedFloat = NavSatFix() semiTrustedFloat = NavSatFix() unTrustedFloat = NavSatFix() allMsg = Odometry() trustedFix.header.frame_id = frame_id unTrustedFix.header.frame_id = frame_id trustedFloat.header.frame_id = frame_id semiTrustedFloat.header.frame_id = frame_id unTrustedFloat.header.frame_id = frame_id allMsg.header.frame_id = frame_id if not check_nmea_checksum(nmea_string): self.get_logger().warn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False ''' Publish gps lines to Ros string Msg ''' if "$GN" in nmea_string: gps_line_first_half += nmea_string + "," + str(time.time()) + "," else: gps_line_second_half += nmea_string + "," + str(time.time()) + "," if 'GNVTG' in nmea_string: gps_line += gps_line_first_half + gps_line_second_half + str(self.get_clock().now().to_msg()) + "\n" with open(file_name, 'a') as csvfile: csvfile.write(gps_line) gps_string_msg = String() gps_string_msg.data = gps_line self.string_pub.publish(gps_string_msg) gps_line = '' gps_line_first_half = '' gps_line_second_half = '' if ('GPGSV' in nmea_string and len(nmea_string) < 100): # This message is received too short when it's empty causing runtime error in parsing (This error appeared in ROS2) return parsed_sentence = parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: self.get_logger().debug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if 'GAGSA' in parsed_sentence: gsa_data=parsed_sentence['GSA'] if math.isnan(gsa_data['PDop']): pdop_GAGSA = 100.0 else: pdop_GAGSA=gsa_data['PDop'] if math.isnan(gsa_data['HDop']) : hdop_GAGSA = 100.0 else: hdop_GAGSA=gsa_data['HDop'] if math.isnan(gsa_data['VDop']) : vdop_GAGSA = 100.0 else: vdop_GAGSA=gsa_data['VDop'] if 'GLGSA' in parsed_sentence: gsa_data=parsed_sentence['GSA'] if math.isnan(gsa_data['PDop']): pdop_GLGSA = 100.0 else: pdop_GLGSA=gsa_data['PDop'] if math.isnan(gsa_data['HDop']) : hdop_GLGSA = 100.0 else: hdop_GLGSA=gsa_data['HDop'] if math.isnan(gsa_data['VDop']) : vdop_GLGSA = 100.0 else: vdop_GLGSA=gsa_data['VDop'] if 'GPGSA' in parsed_sentence: gsa_data=parsed_sentence['GSA'] if math.isnan(gsa_data['PDop']): pdop_GPGSA = 100.0 else: pdop_GPGSA=gsa_data['PDop'] if math.isnan(gsa_data['HDop']) : hdop_GPGSA = 100.0 else: hdop_GPGSA=gsa_data['HDop'] if math.isnan(gsa_data['VDop']) : vdop_GPGSA = 100.0 else: vdop_GPGSA=gsa_data['VDop'] if math.isnan(gsa_data['fix']): GPGSA_fix = 100 else: GPGSA_fix = gsa_data['fix'] if 'GPGSV' in parsed_sentence: gsv_data=parsed_sentence['GSV'] msgNo=gsv_data['MSGNo'] if math.isnan(gsv_data['SNR1']) : SNR1[msgNo-1] = 100.0 else: SNR1[msgNo-1]=gsv_data['SNR1'] if math.isnan(gsv_data['SNR2']) : SNR2[msgNo-1] = 100.0 else: SNR2[msgNo-1]=gsv_data['SNR2'] if math.isnan(gsv_data['SNR3']) : SNR3[msgNo-1] = 100.0 else: SNR3[msgNo-1]=gsv_data['SNR3'] if math.isnan(gsv_data['SNR4']) : SNR4[msgNo-1] = 100.0 else: SNR4[msgNo-1]=gsv_data['SNR4'] if 'GST' in parsed_sentence: gst_data=parsed_sentence['GST'] if math.isnan(gst_data['ranges_std_dev']): gst_rms = 100.0 else: gst_rms = gst_data['ranges_std_dev'] if math.isnan(gst_data['lat_std_dev']) or math.isnan(gst_data['lon_std_dev']): lat_error = 100.0 lon_error = 100.0 else: lat_error=gst_data['lat_std_dev'] lon_error=gst_data['lon_std_dev'] if 'RMC' in parsed_sentence: #20200914 was elif data = parsed_sentence['RMC'] date_line = data['date'] + str("_") + str(int(data['utc_time'])) #gps_string_date_msg = String() #gps_string_date_msg.data = date_line #self.string_date_pub.publish(gps_string_date_msg) if (use_gps_date_once): use_gps_date_once = 0 file_name = "/home/jetson/csv/GPS_LINES_" + data['date'] + str(int(data['utc_time'])) + ".csv" rmc_longitude = data['longitude'] rmc_latitude = data['latitude'] #rmc_altitude = data['altitude'] # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) if 'VTG' in parsed_sentence: VTG_received = 1 vtg_data = parsed_sentence['VTG'] if math.isnan(vtg_data['kph_speed']): kph_speed = 100.0 else: kph_speed = vtg_data['kph_speed'] if 'GGA' in parsed_sentence: GGA_received = 1 data = parsed_sentence['GGA'] gps_qual = data['fix_type'] age_of_diff = data['age_of_diff'] gga_latitude = data['latitude'] if data['latitude_direction'] == 'S' : gga_latitude = -gga_latitude gga_longitude = data['longitude'] if data['longitude_direction'] == 'W': gga_longitude = -gga_longitude gga_altitude = data['altitude'] hdop_GGA = data['hdop'] satNo = data['num_satellites'] if not math.isnan(data['utc_time']): received_utc_time = data['utc_time'] received_time_is_nan = 0 else: received_time_is_nan = 1 if VTG_received and GGA_received: VTG_received = 0 GGA_received = 0 previous_received_utc_time = received_utc_time if gps_qual == 0: trustedFix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: trustedFix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: trustedFix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): trustedFix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ trustedFix.status.status = NavSatStatus.STATUS_SBAS_FIX else: trustedFix.status.status = NavSatStatus.STATUS_NO_FIX trustedFix.status.service = NavSatStatus.SERVICE_GPS lat_error2 = lat_error*lat_error lon_error2 = lon_error*lon_error underroot = 0.5*(lat_error2 + lon_error2) #print "ALL :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot) #print "LAT :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot) #print "LON :::::: 1sigma" , math.sqrt(underroot), ",2sigma" , 2*math.sqrt(underroot), ",3sigma" , 3*math.sqrt(underroot) latitude = gga_latitude trustedFix.latitude = latitude unTrustedFix.latitude = latitude trustedFloat.latitude = latitude semiTrustedFloat.latitude = latitude unTrustedFloat.latitude = latitude allMsg.pose.pose.position.x = latitude longitude = gga_longitude trustedFix.longitude = longitude unTrustedFix.longitude = longitude trustedFloat.longitude = longitude semiTrustedFloat.longitude = longitude unTrustedFloat.longitude = longitude allMsg.pose.pose.position.y = longitude #31Dec2019 altitude = gga_altitude trustedFix.altitude = altitude unTrustedFix.altitude = altitude trustedFloat.altitude = altitude semiTrustedFloat.altitude = altitude unTrustedFloat.altitude = altitude allMsg.pose.pose.position.z = altitude newTime = self.get_clock().now().to_msg() trustedFix.header.stamp =newTime unTrustedFix.header.stamp = newTime trustedFloat.header.stamp = newTime semiTrustedFloat.header.stamp = newTime unTrustedFloat.header.stamp = newTime allMsg.header.stamp = newTime #if (badFlag != 0): # print (" setting badFlag",badFlag) #rospy.set_param('badGpsFlag', badFlag) #print (" sdone etting badFlag") if gps_qual == 4 and lat_error <= 0.05 and lon_error <= 0.05: #TODO tuning this number 14 Oct 2019 gpsQuality = 4 out_cov = 0.00001 trustedFix.position_covariance[0] = out_cov trustedFix.position_covariance[4] = out_cov trustedFix.position_covariance[8] = 0 trustedCount += 2 if fixCount < 3: # To solve float to fix change in orientation out_cov = 3*math.sqrt(underroot) trustedFix.position_covariance[0] = out_cov trustedFix.position_covariance[4] = out_cov trustedFix.position_covariance[8] = 0 fixCount += 1 if started: print ("Staaaaaarting... fix") started=0 if publish_gps: print("trustedFix_pub") self.trustedFix_pub.publish(trustedFix) elif gps_qual == 4 : gpsQuality = 3 badFlag += 5 out_cov = 3*math.sqrt(underroot) unTrustedFix.position_covariance[0] = out_cov unTrustedFix.position_covariance[4] = out_cov unTrustedFix.position_covariance[8] = 0 trustedCount = 0 fixCount = 0 if started: print ("Staaaaaarting...fix") started=0 if publish_gps: print("unTrustedFix_pub") self.unTrustedFix_pub.publish(unTrustedFix) elif gps_qual == 5 and lat_error < 0.1 and lon_error < 0.1: gpsQuality = 5 fixCount = 0 if started: print ("Staaaaaarting... float") started=0 out_cov = 3*math.sqrt(underroot) trustedFloat.position_covariance[0] = out_cov trustedFloat.position_covariance[4] = out_cov trustedFloat.position_covariance[8] = 0 trustedCount += 1 if publish_gps: self.trustedFloat_pub.publish(trustedFloat) print("trustedFloat_pub") #trustedFloat.position_covariance[0] = 1.0 #trustedFloat.position_covariance[4] = 1.0 #trustedFloat.position_covariance[8] = 0 #self.trustedFloat2_pub.publish(trustedFloat) elif gps_qual == 5 and lat_error < 0.3 and lon_error < 0.3: gpsQuality = 6 fixCount = 0 if started: print ("Staaaaaarting... float") started=0 out_cov = 3*math.sqrt(underroot) semiTrustedFloat.position_covariance[0] = out_cov semiTrustedFloat.position_covariance[4] = out_cov semiTrustedFloat.position_covariance[8] = 0 trustedCount = 0 if publish_gps: print("semiTrustedFloat_pub") self.semiTrustedFloat_pub.publish(semiTrustedFloat) elif gps_qual == 5 : gpsQuality = 7 fixCount = 0 badFlag += 5 if started: print ("Staaaaaarting... float") started=0 out_cov = 3*math.sqrt(underroot) unTrustedFloat.position_covariance[0] = out_cov unTrustedFloat.position_covariance[4] = out_cov unTrustedFloat.position_covariance[8] = 0 trustedCount = 0 if publish_gps: print("unTrustedFloat_pub") self.unTrustedFloat_pub.publish(unTrustedFloat) else: gpsQuality = 2 fixCount = 0 badFlag += 5 if started: print ("Staaaaaarting... float") started=0 out_cov = 3*math.sqrt(underroot) unTrustedFloat.position_covariance[0] = out_cov unTrustedFloat.position_covariance[4] = out_cov unTrustedFloat.position_covariance[8] = 0 trustedCount = 0 if publish_gps: print("unTrustedFloat_pub") self.unTrustedFloat_pub.publish(unTrustedFloat) if satNo < 5 or hdop_GPGSA > 4 or pdop_GPGSA > 8 : print (" **********************************************************************") print ("satNo , hdop_GPGSA , pdop_GPGSA ",satNo , hdop_GPGSA , pdop_GPGSA) print (" Very BAD GPS *********************************************************") badFlag += 5 #os.system('spd-say "Warning, GPS is very bad" -t male1 -i -40 -r -10') elif satNo < 5 or hdop_GPGSA > 1.8 or pdop_GPGSA > 5 : print( " :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::") print ("satNo , hdop_GPGSA , pdop_GPGSA ",satNo , hdop_GPGSA , pdop_GPGSA) badFlag += 1 print (" BAD GPS :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::") #os.system('spd-say "Warning, GPS is bad" -t male1 -i -40 -r -10') elif satNo > 6 and hdop_GPGSA < 1.7 and pdop_GPGSA < 4.3 and lat_error < 0.09 and lon_error < 0.09 and badFlag>0 : badFlag -= 1 if trustedCount > 40 and badFlag > 0: badFlag = 0 print (" resetting badFlag") '''
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug( "Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = ( 2 * hdop * self.alt_std_dev) ** 2 # FIXME self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def add_sentence_original(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): self.get_logger().warn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % nmea_string) return False parsed_sentence = parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: self.get_logger().debug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = self.get_clock().now().to_msg() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if current_fix.status.status > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME print("add sentence gga") self.trustedFix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg() self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.trustedFix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rclpy.time.Time(seconds=data['utc_time']).to_msg() self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug( "Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers # which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop ** 2 current_fix.position_covariance[4] = hdop ** 2 current_fix.position_covariance[8] = (2 * hdop) ** 2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, # since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, timestamp): time = 0.0 fix = False NumSat = 0 latitude = 0.0 longitude = 0.0 altitude = 0.0 speed = 0.0 ground_course = 0.0 covariance = 0.0 if not check_nmea_checksum(nmea_string): logger.debug("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: logger.debug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if data['fix_valid']: self.speed = data['speed'] if not math.isnan(data['true_course']): self.ground_course = data['true_course'] if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual > 0: fix = True NumSat = data['num_satellites'] time = timestamp latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude longitude = longitude hdop = data['hdop'] covariance = hdop**2 # position_covariance[4] = hdop ** 2 # position_covariance[8] = (2 * hdop) ** 2 # FIXME # position_covariance_type = \ # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] altitude = altitude speed = self.speed ground_course = self.ground_course return (time, fix, NumSat, latitude, longitude, altitude, speed, ground_course, covariance) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: time = timestamp fix = True NumSat = data['num_satellites'] latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude longitude = longitude altitude = float('NaN') # position_covariance_type = \ if data['fix_valid']: speed = data['speed'] if not math.isnan(data['true_course']): ground_course = data['true_course'] else: ground_course = self.ground_course return (time, fix, NumSat, latitude, longitude, altitude, speed, ground_course, covariance) return False else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id gpsfix = self.gpsfix gpsfix.header.stamp = current_time gpsfix.header.frame_id = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) gpsfix.hdop = data['hdop'] gpsfix.time = data['utc_time'] elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) gpsfix.speed = data['speed'] gpsfix.track = math.degrees(data['true_course']) elif 'GSV' in parsed_sentence: data = parsed_sentence['GSV'] msg = Gpgsv() msg.header.stamp = rospy.Time.now() msg.n_msgs = data['n_msgs'] msg.msg_number = data['msg_number'] msg.n_satellites = data['n_satellites'] gpsfix.status.satellites_visible = msg.n_satellites if data['msg_number'] == 1: gpsfix.status.satellite_visible_prn = [] gpsfix.status.satellite_visible_z = [] gpsfix.status.satellite_visible_azimuth = [] gpsfix.status.satellite_visible_snr = [] for i in range(0, 4): try: sat = GpgsvSatellite(data['prn%d' % i], data['elevation%d' % i], data['azimuth%d' % i], data['snr%d' % i]) msg.satellites.append(sat) gpsfix.status.satellite_visible_prn.append(data['prn%d' % i]) gpsfix.status.satellite_visible_z.append( data['elevation%d' % i]) gpsfix.status.satellite_visible_azimuth.append( data['azimuth%d' % i]) gpsfix.status.satellite_visible_snr.append(data['snr%d' % i]) except: pass self.sat_pub.publish(msg) elif 'GSA' in parsed_sentence: data = parsed_sentence['GSA'] gpsfix.pdop = data['pdop'] gpsfix.hdop = data['hdop'] gpsfix.vdop = data['vdop'] gpsfix.status.satellites_used = len(data['prns']) gpsfix.status.satellite_used_prn = data['prns'] else: return False if ('RMC' in parsed_sentence and self.use_RMC) or ('GGA' in parsed_sentence and not self.use_RMC): gpsfix.latitude = current_fix.latitude gpsfix.longitude = current_fix.longitude gpsfix.altitude = current_fix.altitude gpsfix.status.status = current_fix.status.status gpsfix.status.position_source = gpsfix.status.orientation_source = gpsfix.status.motion_source = current_fix.status.service self.gpsfix_pub.publish(gpsfix)
def add_sentence(self, message, frame_id, timestamp=None, sentence_type="NMEA", ubx_message_definitions=None): """Public method to provide a new sentence to the driver. Args: message (str): NMEA or UBX sentence in string form. frame_id (str): TF frame ID of the GPS receiver. timestamp (rospy.Time, optional): Time the sentence was received. If timestamp is not specified, the current time is used. sentence_type (str, optional): NMEA or UBX ubx_message_definitions (str, optional): Message definitions for UBX sentences Return: bool: True if the NMEA string is successfully processed, False if there is an error. """ if not "UBX" == sentence_type: if not check_nmea_checksum(message): rospy.logwarn( "Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(message)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( message) if not parsed_sentence: rospy.logdebug( "Failed to parse NMEA sentence. Sentence was: %s" % message) return False elif "UBX" == sentence_type: parsed_sentence = libnmea_navsat_driver.parser_ubx.parse_ubx_sentence( message, ubx_message_definitions) if not parsed_sentence: rospy.logdebug( "Failed to parse UBX sentence. Sentence was: %s" % message) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id if not self.use_GNSS_time: current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] self.valid_fix = (fix_type > 0) current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time(data['utc_time'][0], data['utc_time'][1]) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) if self.use_extended_nmea_messages: info = NavSatInfo() info.header.stamp = current_time info.header.frame_id = frame_id info.num_satellites = data['num_satellites'] info.last_dgps_update = data['age_dgps'] self.age_dgps = data['age_dgps'] info.hdop = data['hdop'] self.info_pub.publish(info) elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * math.sin( data['true_course']) current_vel.twist.linear.y = data['speed'] * math.cos( data['true_course']) self.vel_pub.publish(current_vel) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] if self.use_GNSS_time: if math.isnan(data['utc_time'][0]): rospy.logwarn("Time in the NMEA sentence is NOT valid") return False current_fix.header.stamp = rospy.Time(data['utc_time'][0], data['utc_time'][1]) # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not (math.isnan(data['utc_time'][0]) or self.use_GNSS_time): current_time_ref.time_ref = rospy.Time( data['utc_time'][0], data['utc_time'][1]) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GRS' in parsed_sentence and self.use_extended_nmea_messages: data = parsed_sentence['GRS'] msg = NavSatGRS() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = data['utc_time'] msg.mode = data['mode'] msg.residual_01 = data['residual_01'] msg.residual_02 = data['residual_02'] msg.residual_03 = data['residual_03'] msg.residual_04 = data['residual_04'] msg.residual_05 = data['residual_05'] msg.residual_06 = data['residual_06'] msg.residual_07 = data['residual_07'] msg.residual_08 = data['residual_08'] msg.residual_09 = data['residual_09'] msg.residual_10 = data['residual_10'] msg.residual_11 = data['residual_11'] # The following two attributes have been introduced with NMEA 4.11 msg.system_id = data['system_id'] msg.signal_id = data['signal_id'] self.grs_pub.publish(msg) elif 'GST' in parsed_sentence and self.use_extended_nmea_messages: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] msg = NavSatGST() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = data['utc_time'] msg.ranges_std_dev = data['ranges_std_dev'] msg.semi_major_ellipse_std_dev = data['semi_major_ellipse_std_dev'] msg.semi_minor_ellipse_std_dev = data['semi_minor_ellipse_std_dev'] msg.semi_major_orientation = data['semi_major_orientation'] msg.lat_std_dev = data['lat_std_dev'] msg.lon_std_dev = data['lon_std_dev'] msg.alt_std_dev = data['alt_std_dev'] self.gst_pub.publish(msg) elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif self.use_trimble_messages and 'PTNL,VHD' in parsed_sentence: data = parsed_sentence['PTNL,VHD'] msg = NavSatTrimbleHeading() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.azimuth = data['azimuth'] msg.azimuth_rate = data['azimuth_rate'] msg.vertical_angle = data['vertical_angle'] msg.vertical_angle_rate = data['vertical_angle_rate'] msg.range = data['range'] msg.range_rate = data['range_rate'] # explanation of the status codes in message definition msg.status = data['fix_type'] msg.num_satellites = data['num_satellites'] msg.pdop = data['pdop'] self.trimble_heading_pub.publish(msg) elif self.use_trimble_messages and 'PTNL,AVR' in parsed_sentence: data = parsed_sentence['PTNL,AVR'] msg = NavSatTrimbleMovingBase() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.yaw = data['yaw'] if self.ccw_heading: msg.yaw = -msg.yaw msg.yaw += self.heading_offset # wrap yaw angle to [-pi, pi) msg.yaw = (msg.yaw + math.pi) % (2 * math.pi) - math.pi msg.tilt = data['tilt'] msg.roll = data['roll'] msg.range = data['range'] # explanation of the status codes in message definition msg.status = data['fix_type'] msg.pdop = data['pdop'] msg.num_satellites = data['num_satellites'] self.trimble_moving_base_pub.publish(msg) # An dual antenna system can only measure tow out of the # three angles tilt, roll, yaw. Yaw is always measured. # Assuming all not measured angles are zero. if math.isnan(msg.tilt): msg.tilt = 0. if math.isnan(msg.roll): msg.roll = 0. msg2 = Imu() msg2.header.stamp = current_time msg2.header.frame_id = frame_id q = quaternion_from_euler(msg.tilt, msg.roll, msg.yaw) msg2.orientation.x = q[0] msg2.orientation.y = q[1] msg2.orientation.z = q[2] msg2.orientation.w = q[3] # Calculate the orientation error estimate based on the position # error estimates and the baseline. Very conservative by taking the # largest possible error at both ends in both directions. Set high # error if there is no proper vector fix (msg.status is not 3). if msg.status == 3: total_position_error = math.sqrt(self.lat_std_dev**2 + self.lon_std_dev**2) angular_error_estimate = math.atan2(total_position_error * 2, msg.range) angular_error_cov = angular_error_estimate**2 else: angular_error_cov = 1 msg2.orientation_covariance = [ angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov ] msg2.angular_velocity_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] msg2.linear_acceleration_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] self.trimble_moving_base_imu_pub.publish(msg2) # Documentation source: # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf # Some comments have been directly copied from the documentation elif self.use_ublox_messages and 'PUBX,00' in parsed_sentence: data = parsed_sentence['PUBX,00'] msg = NavSatUbloxPubxPosition() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.utc_time = rospy.Time(data['utc_time'][0], data['utc_time'][1]) msg.latitude = data['latitude'] if data['latitude_direction'] == 'S': msg.latitude = -msg.latitude msg.longitude = data['longitude'] if data['longitude_direction'] == 'W': msg.longitude = -msg.longitude msg.altitude = data['altitude'] msg.nav_stat = data['nav_stat'] msg.h_acc = data['h_acc'] msg.v_acc = data['v_acc'] msg.sog = data['sog'] / 3.6 msg.cog = data['cog'] / 180.0 * math.pi msg.v_vel = data['v_vel'] msg.diff_age = data['diff_age'] msg.hdop = data['hdop'] msg.vdop = data['vdop'] msg.tdop = data['tdop'] msg.num_svs = data['num_svs'] msg.dr = data['dr'] self.ublox_pubx_position_pub.publish(msg) # Documentation source: # https://www.u-blox.com/sites/default/files/u-blox_ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf # Some comments have been directly copied from the documentation elif self.use_ublox_messages and 'UBX-NAV-RELPOSNED' in parsed_sentence: data = parsed_sentence['UBX-NAV-RELPOSNED'] msg = NavSatUbloxRelPos() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.n = data['relPosN'] * 0.01 + data['relPosHPN'] * 0.0001 msg.e = data['relPosE'] * 0.01 + data['relPosHPE'] * 0.0001 msg.d = data['relPosD'] * 0.01 + data['relPosHPD'] * 0.0001 msg.length = data['relPosLength'] * 0.01 + data[ 'relPosHPLength'] * 0.0001 msg.heading = data['relPosHeading'] * 0.00001 / 180.0 * math.pi if self.ccw_heading: msg.heading = -msg.heading # only add offset if heading is valid - invalid heading should always be 0.0 data['flags'] = format(data['flags'], '032b') if 1 == int(data['flags'][23], 2): msg.heading += self.heading_offset # wrap yaw angle to [-pi, pi) msg.heading = (msg.heading + math.pi) % (2 * math.pi) - math.pi msg.acc_n = data['accN'] * 0.0001 msg.acc_e = data['accE'] * 0.0001 msg.acc_d = data['accD'] * 0.0001 msg.acc_length = data['accLength'] * 0.0001 msg.acc_heading = data['accHeading'] * 0.00001 / 180.0 * math.pi msg.rel_pos_heading_valid = int(data['flags'][23], 2) msg.ref_obs_miss = int(data['flags'][24], 2) msg.ref_pos_miss = int(data['flags'][25], 2) msg.is_moving = int(data['flags'][26], 2) msg.carr_soln = int(data['flags'][27:29], 2) msg.rel_pos_valid = int(data['flags'][29], 2) msg.diff_soln = int(data['flags'][30], 2) msg.gnss_fix_ok = int(data['flags'][31], 2) # accuracy estimates in non RTK fixed modes shouldn't be relied on if msg.carr_soln == 2 and msg.rel_pos_valid == 1: self.lat_std_dev = msg.acc_n self.lon_std_dev = msg.acc_e self.alt_std_dev = msg.acc_d elif msg.carr_soln == 1 and msg.rel_pos_valid == 1: self.lat_std_dev = self.default_epe_quality5 self.lon_std_dev = self.default_epe_quality5 self.alt_std_dev = self.default_epe_quality5 else: self.lat_std_dev = self.default_epe_quality1 self.lon_std_dev = self.default_epe_quality1 self.alt_std_dev = self.default_epe_quality1 self.ublox_relpos_pub.publish(msg) # report erroneous messages if msg.carr_soln != 0: if msg.ref_obs_miss == 1 \ or msg.ref_pos_miss == 1 \ or msg.rel_pos_valid == 0 \ or msg.gnss_fix_ok == 0: rospy.logerr( "GNSS receiver failed to calculate GNSS fix despite available correction data. " "Consider lowering the frequency, receiver load might be too high." ) return False # publish odometry and IMU messages for fusion in case message is fine if msg.carr_soln == 0: if msg.diff_soln == 1: rospy.logwarn_throttle( 10, "Unable to calculate RTK solution.") return False msg2 = Odometry() msg2.header.stamp = current_time msg2.header.frame_id = frame_id # follow enu conventions msg2.pose.pose.position.x = msg.e msg2.pose.pose.position.y = msg.n msg2.pose.pose.position.z = -msg.d msg2.pose.covariance[0] = self.lon_std_dev**2 msg2.pose.covariance[7] = self.lat_std_dev**2 msg2.pose.covariance[14] = self.alt_std_dev**2 # output heading once in moving base mode if msg.is_moving == 1: # take the already inverted heading from above and not the raw # value q = quaternion_from_euler(0., 0., msg.heading) msg2.pose.pose.orientation.x == q[0] msg2.pose.pose.orientation.y == q[1] msg2.pose.pose.orientation.z == q[2] msg2.pose.pose.orientation.w == q[3] # degrade estimated when not in RTK fixed mode msg2.pose.covariance[21] = msg.acc_heading**2 + (2 - msg.carr_soln) msg2.pose.covariance[28] = msg.acc_heading**2 + (2 - msg.carr_soln) msg2.pose.covariance[35] = msg.acc_heading**2 + (2 - msg.carr_soln) else: msg2.pose.covariance[21] = -1.0 msg2.pose.covariance[28] = -1.0 msg2.pose.covariance[35] = -1.0 self.ublox_relpos_odom_pub.publish(msg2) # Publish IMU message if in moving base mode if msg.is_moving == 1: msg3 = Imu() msg3.header.stamp = current_time msg3.header.frame_id = frame_id xyzw = quaternion_from_euler(0.0, 0.0, msg.heading) msg3.orientation.x = xyzw[0] msg3.orientation.y = xyzw[1] msg3.orientation.z = xyzw[2] msg3.orientation.w = xyzw[3] if msg.carr_soln == 2: angular_error_cov = msg.acc_heading**2 else: angular_error_cov = msg.acc_heading**2 + (2 - msg.carr_soln) msg3.orientation_covariance = [ angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov, 0.0, 0.0, 0.0, angular_error_cov ] msg3.angular_velocity_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] msg3.linear_acceleration_covariance = [ -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] self.ublox_relpos_imu_pub.publish(msg3) elif self.use_ublox_messages and 'UBX-NAV-GEOFENCE' in parsed_sentence: data = parsed_sentence['UBX-NAV-GEOFENCE'] msg = NavSatUbloxGeoFence() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.status = data['status'] msg.num_fences = data['numFences'] msg.comb_state = data['combState'] msg.status = data['status'] # The original message is variable length, so all fences are # initialized with "deactivated" state. msg.fence1 = NavSatUbloxGeoFence.DEACTIVATED msg.fence2 = NavSatUbloxGeoFence.DEACTIVATED msg.fence3 = NavSatUbloxGeoFence.DEACTIVATED msg.fence4 = NavSatUbloxGeoFence.DEACTIVATED try: msg.fence1 = data['state'][0] msg.fence2 = data['state'][1] msg.fence3 = data['state'][2] msg.fence4 = data['state'][3] except KeyError: pass self.ublox_geofence_pub.publish(msg) elif self.use_ublox_messages and 'UBX-NAV-PVT' in parsed_sentence: data = parsed_sentence['UBX-NAV-PVT'] msg = NavSatUbloxPositionVelocityTime() msg.header.stamp = current_time msg.header.frame_id = frame_id msg.tow = data['iTOW'] * 0.001 msg.year = data['year'] msg.month = data['month'] msg.day = data['day'] msg.hour = data['hour'] msg.min = data['min'] msg.sec = data['sec'] bitfield_valid = format(data['valid'], '08b') msg.valid_date = int(bitfield_valid[7], 2) msg.valid_time = int(bitfield_valid[6], 2) msg.fully_resolved = int(bitfield_valid[5], 2) msg.valid_mag = int(bitfield_valid[4], 2) msg.t_acc = data['tAcc'] / 1.e9 msg.nano = data['nano'] msg.fix_type = data['fixType'] bitfield_flags = format(data['flags'], '08b') msg.gnss_fix_ok = int(bitfield_flags[7], 2) msg.diff_soln = int(bitfield_flags[6], 2) msg.psm_state = int(bitfield_flags[3:6], 2) msg.head_veh_valid = int(bitfield_flags[2], 2) msg.carr_soln = int(bitfield_flags[0:2], 2) bitfield_flags2 = format(data['flags2'], '08b') msg.confirmed_avai = int(bitfield_flags2[2], 2) msg.confirmed_date = int(bitfield_flags2[1], 2) msg.confirmed_time = int(bitfield_flags2[0], 2) msg.num_sv = data['numSV'] msg.lon = data['lon'] / 1.e7 msg.lat = data['lat'] / 1.e7 msg.height = data['height'] / 1.e3 msg.h_msl = data['hMSL'] / 1.e3 msg.h_acc = data['hAcc'] / 1.e3 msg.v_acc = data['vAcc'] / 1.e3 msg.vel_n = data['velN'] / 1.e3 msg.vel_e = data['velE'] / 1.e3 msg.vel_d = data['velD'] / 1.e3 msg.g_speed = data['gSpeed'] / 1.e3 msg.head_mot = data['headMot'] / 1.e5 / 180.0 * math.pi msg.s_acc = data['sAcc'] / 1.e3 msg.head_acc = data['headAcc'] / 1.e5 / 180.0 * math.pi msg.p_dop = data['pDOP'] / 1.e2 msg.head_veh = data['headVeh'] / 1.e5 / 180.0 * math.pi msg.mag_dec = data['magDec'] / 1.e2 / 180.0 * math.pi msg.mag_acc = data['magAcc'] / 1.e2 / 180.0 * math.pi self.ublox_position_velocity_time_pub.publish(msg) elif self.use_ublox_messages and 'UBX-RAW' in parsed_sentence: data = parsed_sentence['UBX-RAW'] msg = String() # transmitting binary data (bytes) directly is not possible anymore # with ROS and Python3, therefore base64 encoding is applied msg.data = base64.b64encode(data['raw']).decode('ascii') self.ublox_raw.publish(msg) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.seq = self.seq self.seq += 1 current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0]; current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2]; current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev) ** 2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev) ** 2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev) ** 2 # FIXME self.fix_pub.publish(current_fix) # Publish GPGGA msgs gpgga_msg = Gpgga() gpgga_msg.header.stamp = current_time gpgga_msg.header.frame_id = frame_id gpgga_msg.message_id = data['sent_id'] gpgga_msg.utc_seconds = data['utc_time'] gpgga_msg.lat = latitude gpgga_msg.lon = longitude gpgga_msg.lat_dir = data['latitude_direction'] gpgga_msg.lon_dir = data['longitude_direction'] gpgga_msg.gps_qual = data['fix_type'] gpgga_msg.num_sats = data['num_satellites'] gpgga_msg.hdop = hdop gpgga_msg.alt = data['altitude'] gpgga_msg.altitude_units = data['altitude_units'] gpgga_msg.undulation = data['mean_sea_level'] gpgga_msg.undulation_units = data['mean_sea_level_units'] gpgga_msg.diff_age = data['diff_age']*10 gpgga_msg.station_id = data['station_id'] self.gpgga_pub.publish(gpgga_msg) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. \ Sentence was: %s" % nmea_string) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_heading = Imu() current_heading.header.stamp = current_time current_heading.header.frame_id = 'base_footprint' current_direction = String() # For testing current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id # Add capture/publishing heading info if not self.use_RMC and 'HDT' in parsed_sentence: #rospy.loginfo("HDT!") data = parsed_sentence['HDT'] tempHeading = data['true_heading'] ccHeading = (2 * math.pi) - tempHeading q = tf.transformations.quaternion_from_euler(0, 0, ccHeading) current_heading.orientation.x = q[0] current_heading.orientation.y = q[1] current_heading.orientation.z = q[2] current_heading.orientation.w = q[3] #current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) #if (current_heading.data < .3927): current_direction.data = "N" #elif (current_heading.data < 1.178): current_direction.data = "NE" #elif (current_heading.data < 1.9635): current_direction.data = "E" #elif (current_heading.data < 2.74889): current_direction.data = "SE" #elif (current_heading.data < 3.53429): current_direction.data = "S" #elif (current_heading.data < 4.31969): current_direction.data = "SW" #elif (current_heading.data < 5.10509): current_direction.data = "W" #elif (current_heading.data < 5.89048): current_direction.data = "NW" #else: current_direction.data = "N" self.heading_pub.publish(current_heading) #self.direction_pub.publish(current_direction) #self.time_ref_pub.publish(current_time_ref) elif 'GGA' in parsed_sentence: #rospy.loginfo("GGA!") data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX elif gps_qual == 9: # Support specifically for NOVATEL OEM4 recievers which report WAAS fix as 9 # http://www.novatel.com/support/known-solutions/which-novatel-position-types-correspond-to-the-gga-quality-indicator/ current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel_global = Vector3Stamped() current_vel_global.header.stamp = current_time current_vel_global.header.frame_id = frame_id current_vel_global.vector.x = data['speed'] * \ math.sin(data['true_course']) current_vel_global.vector.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel_global) current_vel_local = TwistStamped() current_vel_local.header.stamp = current_time current_vel_local.header.frame_id = frame_id current_vel_local.twist.linear.x = data['speed'] self.vel_pub2.publish(current_vel_local) elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading_north']: self.hdt_pub.publish(data['heading_north']) elif 'ROT' in parsed_sentence: data = parsed_sentence['ROT'] current_rot = TwistStamped() current_rot.header.stamp = current_time current_rot.header.frame_id = frame_id current_rot.twist.angular.z = data['rate'] * 3.14 / 180 * 60 self.rot_pub.publish(current_rot) elif 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] current_speed = TwistStamped() current_speed.header.stamp = current_time current_speed.header.frame_id = frame_id current_speed.twist.linear.x = data['speed_kph'] / 360 * 1000 self.speed_pub.publish(current_speed) elif 'AVR' in parsed_sentence: data = parsed_sentence['AVR'] current_pyr = TwistStamped() current_pyr.header.stamp = current_timecur current_pyr.header.frame_id = frame_id current_pyr.twist.angular.z = data['yaw'] current_pyr.twist.angular.y = data['pitch'] current_pyr.twist.angular.x = data['roll'] if data['fix_type'] > 0: self.pyr_pub.publish(current_pyr) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. \ Sentence was: %s" % nmea_string) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_heading = Imu() current_heading.header.stamp = current_time current_heading.header.frame_id = 'base_footprint' current_direction = String() # For testing current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id # Add capture/publishing heading info if not self.use_RMC and 'HDT' in parsed_sentence: #rospy.loginfo("HDT!") data = parsed_sentence['HDT'] tempHeading = data['true_heading'] ccHeading = (2 * math.pi) - tempHeading q = tf.transformations.quaternion_from_euler(0,0,ccHeading) current_heading.orientation.x = q[0] current_heading.orientation.y = q[1] current_heading.orientation.z = q[2] current_heading.orientation.w = q[3] #current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) #if (current_heading.data < .3927): current_direction.data = "N" #elif (current_heading.data < 1.178): current_direction.data = "NE" #elif (current_heading.data < 1.9635): current_direction.data = "E" #elif (current_heading.data < 2.74889): current_direction.data = "SE" #elif (current_heading.data < 3.53429): current_direction.data = "S" #elif (current_heading.data < 4.31969): current_direction.data = "SW" #elif (current_heading.data < 5.10509): current_direction.data = "W" #elif (current_heading.data < 5.89048): current_direction.data = "NW" #else: current_direction.data = "N" self.heading_pub.publish(current_heading) #self.direction_pub.publish(current_direction) #self.time_ref_pub.publish(current_time_ref) elif 'GGA' in parsed_sentence: #rospy.loginfo("GGA!") data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2*hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.fix_pub.publish(current_fix) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: data = parsed_sentence['GGA'] gps_qual = data['fix_type'] if gps_qual == 0: current_fix.status.status = NavSatStatus.STATUS_NO_FIX elif gps_qual == 1: current_fix.status.status = NavSatStatus.STATUS_FIX elif gps_qual == 2: current_fix.status.status = NavSatStatus.STATUS_SBAS_FIX elif gps_qual in (4, 5): current_fix.status.status = NavSatStatus.STATUS_GBAS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.header.stamp = current_time latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude hdop = data['hdop'] current_fix.position_covariance[0] = hdop**2 current_fix.position_covariance[4] = hdop**2 current_fix.position_covariance[8] = (2 * hdop)**2 # FIXME current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentece was: %s" % nmea_string) return False if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if 'B' in parsed_sentence: data = parsed_sentence['B'] # Only publish a fix from RMC if the use_RMC flag is set. if True: current_fix.status.status = NavSatStatus.STATUS_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS current_fix.latitude = data['latitude'] current_fix.longitude = data['longitude'] current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec(data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide it. if True: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) else: return False
def add_sentence(self, nmea_string, frame_id, timestamp=None): if not check_nmea_checksum(nmea_string): rospy.logwarn("Received a sentence with an invalid checksum. " + "Sentence was: %s" % repr(nmea_string)) return False parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence( nmea_string) if not parsed_sentence: rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" % nmea_string) return False rospy.logdebug(parsed_sentence) if timestamp: current_time = timestamp else: current_time = rospy.get_rostime() current_fix = NavSatFix() current_fix.header.stamp = current_time current_fix.header.frame_id = frame_id current_time_ref = TimeReference() current_time_ref.header.stamp = current_time current_time_ref.header.frame_id = frame_id self.extend_fix.header.stamp = current_time self.extend_fix.header.frame_id = frame_id if self.time_ref_source: current_time_ref.source = self.time_ref_source else: current_time_ref.source = frame_id if not self.use_RMC and 'GGA' in parsed_sentence: current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_APPROXIMATED data = parsed_sentence['GGA'] fix_type = data['fix_type'] if not (fix_type in self.gps_qualities): fix_type = -1 gps_qual = self.gps_qualities[fix_type] default_epe = gps_qual[0] current_fix.status.status = gps_qual[1] current_fix.position_covariance_type = gps_qual[2] if gps_qual > 0: self.valid_fix = True else: self.valid_fix = False current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude # Altitude is above ellipsoid, so adjust for mean-sea-level altitude = data['altitude'] + data['mean_sea_level'] current_fix.altitude = altitude # use default epe std_dev unless we've received a GST sentence with # epes if not self.using_receiver_epe or math.isnan(self.lon_std_dev): self.lon_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.lat_std_dev): self.lat_std_dev = default_epe if not self.using_receiver_epe or math.isnan(self.alt_std_dev): self.alt_std_dev = default_epe * 2 hdop = data['hdop'] current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2 current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2 current_fix.position_covariance[8] = (2 * hdop * self.alt_std_dev)**2 # FIXME self.fix_pub.publish(current_fix) # set extend fix self.extend_fix.status.header.stamp = current_time self.extend_fix.status.header.frame_id = frame_id self.extend_fix.status.status = gps_qual[1] self.extend_fix.status.satellites_used = data['num_satellites'] self.extend_fix.status.motion_source = GPSStatus.SOURCE_GPS self.extend_fix.status.orientation_source = GPSStatus.SOURCE_GPS self.extend_fix.status.position_source = GPSStatus.SOURCE_GPS self.extend_fix.latitude = current_fix.latitude self.extend_fix.longitude = current_fix.longitude self.extend_fix.altitude = current_fix.altitude self.extend_fix.position_covariance = current_fix.position_covariance self.position_covariance_type = current_fix.position_covariance_type if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.last_valid_fix_time = current_time_ref self.time_ref_pub.publish(current_time_ref) self.extend_fix.time = current_time_ref.time_ref.to_sec() elif not self.use_RMC and 'VTG' in parsed_sentence: data = parsed_sentence['VTG'] # Only report VTG data when you've received a valid GGA fix as # well. if self.valid_fix: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) self.extend_fix.track = data['true_course'] self.extend_fix.speed = data['speed'] self.extend_fix_pub.publish(self.extend_fix) elif 'RMC' in parsed_sentence: data = parsed_sentence['RMC'] # Only publish a fix from RMC if the use_RMC flag is set. if self.use_RMC: if data['fix_valid']: current_fix.status.status = NavSatStatus.STATUS_FIX else: current_fix.status.status = NavSatStatus.STATUS_NO_FIX current_fix.status.service = NavSatStatus.SERVICE_GPS latitude = data['latitude'] if data['latitude_direction'] == 'S': latitude = -latitude current_fix.latitude = latitude longitude = data['longitude'] if data['longitude_direction'] == 'W': longitude = -longitude current_fix.longitude = longitude current_fix.altitude = float('NaN') current_fix.position_covariance_type = \ NavSatFix.COVARIANCE_TYPE_UNKNOWN self.fix_pub.publish(current_fix) if not math.isnan(data['utc_time']): current_time_ref.time_ref = rospy.Time.from_sec( data['utc_time']) self.time_ref_pub.publish(current_time_ref) # Publish velocity from RMC regardless, since GGA doesn't provide # it. if data['fix_valid']: current_vel = TwistStamped() current_vel.header.stamp = current_time current_vel.header.frame_id = frame_id current_vel.twist.linear.x = data['speed'] * \ math.sin(data['true_course']) current_vel.twist.linear.y = data['speed'] * \ math.cos(data['true_course']) self.vel_pub.publish(current_vel) self.extend_fix.track = data['true_course'] elif 'GST' in parsed_sentence: data = parsed_sentence['GST'] # Use receiver-provided error estimate if available self.using_receiver_epe = True self.lon_std_dev = data['lon_std_dev'] self.lat_std_dev = data['lat_std_dev'] self.alt_std_dev = data['alt_std_dev'] elif 'HDT' in parsed_sentence: data = parsed_sentence['HDT'] if data['heading']: current_heading = QuaternionStamped() current_heading.header.stamp = current_time current_heading.header.frame_id = frame_id q = quaternion_from_euler(0, 0, math.radians(data['heading'])) current_heading.quaternion.x = q[0] current_heading.quaternion.y = q[1] current_heading.quaternion.z = q[2] current_heading.quaternion.w = q[3] self.heading_pub.publish(current_heading) elif 'GSA' in parsed_sentence: data = parsed_sentence['GSA'] self.star_use_gps = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_gps = filter(lambda star: star != 0, self.star_use_gps) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] elif 'BDGSA' in parsed_sentence: data = parsed_sentence['BDGSA'] self.star_use_bd = [ data['sate_id1'], data['sate_id2'], data['sate_id3'], data['sate_id4'], data['sate_id5'], data['sate_id6'], data['sate_id7'], data['sate_id8'], data['sate_id9'], data['sate_id10'], data['sate_id11'], data['sate_id12'] ] self.star_use_bd = filter(lambda star: star != 0, self.star_use_bd) self.extend_fix.pdop = data['pdop'] self.extend_fix.hdop = data['hdop'] self.extend_fix.vdop = data['vdop'] self.extend_fix.status.satellite_used_prn = self.star_use_gps + self.star_use_bd elif 'GSV' in parsed_sentence: data = parsed_sentence['GSV'] if data['index'] == 1: self.star_map_gps = [] self.star_map_gps.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_gps.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_gps.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_gps.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_gps = filter(lambda star: star['id'] != 0, self.star_map_gps) elif 'BDGSV' in parsed_sentence: data = parsed_sentence['BDGSV'] if data['index'] == 1: self.star_map_bd = [] self.star_map_bd.append({ 'id': data['id_satellites1'], 'elevation': data['elevation_satellites1'], 'azimuth': data['azimuth_satellites1'], 'snr': data['snr1'] }) self.star_map_bd.append({ 'id': data['id_satellites2'], 'elevation': data['elevation_satellites2'], 'azimuth': data['azimuth_satellites2'], 'snr': data['snr2'] }) self.star_map_bd.append({ 'id': data['id_satellites3'], 'elevation': data['elevation_satellites3'], 'azimuth': data['azimuth_satellites3'], 'snr': data['snr3'] }) self.star_map_bd.append({ 'id': data['id_satellites4'], 'elevation': data['elevation_satellites4'], 'azimuth': data['azimuth_satellites4'], 'snr': data['snr4'] }) self.star_map_bd = filter(lambda star: star['id'] != 0, self.star_map_bd) self.star_map = self.star_map_gps + self.star_map_bd if data['length'] == data['index']: self.extend_fix.status.satellites_visible = len(self.star_map) self.extend_fix.status.satellite_visible_prn = [ star['id'] for star in self.star_map ] self.extend_fix.status.satellite_visible_snr = [ star['snr'] for star in self.star_map ] self.extend_fix.status.satellite_visible_azimuth = [ star['azimuth'] for star in self.star_map ] self.extend_fix.status.satellite_visible_z = [ star['elevation'] for star in self.star_map ] else: return False
#useRMC == False -> generate info from GGA navData = NavSatFix() gpsVel = TwistStamped() gpstime = TimeReference() gpstime.source = time_ref_source navData.header.frame_id = frame_id gpsVel.header.frame_id = frame_id GPSLock = False try: GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2) #Read in GPS while not rospy.is_shutdown(): #read GPS line data = GPS.readline().strip() if not check_nmea_checksum(data): rospy.logwarn("Received a sentence with an invalid checksum. Sentence was: %s" % data) continue timeNow = rospy.get_rostime() fields = data.split(',') for i in fields: i = i.strip(',') try: if useRMC: #Check for satellite lock if 'GSA' in fields[0]: lockState = int(fields[2]) #print 'lockState=',lockState if lockState == 3: GPSLock = True