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