def callback( packet ) :
if rospy.is_shutdown() :
sys.exit()
message = GPSFix()
message.header.stamp = rospy.Time.now()
message.header.frame_id = 'base_link'
message.latitude = packet[ 'latitude' ] * 180 / math.pi
message.longitude = packet[ 'longitude' ] * 180 / math.pi
message.altitude = packet[ 'height' ]
message.time = packet[ 'time' ]
message.pdop = packet[ 'PDOP' ]
message.hdop = packet[ 'HDOP' ]
message.vdop = packet[ 'VDOP' ]
message.tdop = packet[ 'TDOP' ]
message.position_covariance = [ packet[ 'sigma-E' ], packet[ 'cov-EN' ] , 0,
packet[ 'cov-EN' ], packet[ 'sigma-N' ], 0,
0, 0, packet[ 'sigma-Up'] ]
if packet[ 'cov-EN' ] != 0 :
message.position_covariance_type = 3
elif packet[ 'sigma-E' ] != 0 :
message.position_covariance_type = 2
else :
message.position_covariance_type = 0
if debug :
pp.pprint( packet )
pp.pprint( message )
message.status.status = (0 if (message.position_covariance_type > 0) else -1)
message.status.position_source = 1
message.status.header = message.header
publisherGPSFix.publish( message )
if message.status >= 0 :
publisherNavSatFix.publish( NavSatFix( message.header , NavSatStatus( message.status.status, 3 ), message.latitude, message.longitude,
message.altitude, message.position_covariance, message.position_covariance_type ) )
# Now fields[0] is our trigger, try to build a GPSFix message
# will all the information we have
# TODO: complete treatment of GPGSV (satellites in view)
if '$GPGSA' in lastmsg:
fields = lastmsg["$GPGSA"]
lockState = int(fields[2])
#print 'lockState=',lockState
if lockState == 3:
GPSLock = True
navData.status.status = GPSStatus.STATUS_FIX
else:
GPSLock = False
navData.status.status = GPSStatus.STATUS_NO_FIX
navData.pdop = float(fields[15])
navData.hdop = float(fields[16])
navData.vdop = float(fields[17])
navData.status.satellite_visible_prn = []
for i in range(3, 15):
if fields[i] == "":
break
navData.status.satellite_visible_prn.append(
int(fields[i]))
navData.status.satellites_visible = len(
navData.status.satellite_visible_prn)
# In case nobody provides better data:
navData.status.satellites_used = navData.status.satellites_visible
navData.status.satellite_used_prn = navData.status.satellite_visible_prn
if ("$GPRMC" in lastmsg) and GPSLock:
# Now fields[0] is our trigger, try to build a GPSFix message
# will all the information we have
# TODO: complete treatment of GPGSV (satellites in view)
if '$GPGSA' in lastmsg:
fields = lastmsg["$GPGSA"]
lockState = int(fields[2])
#print 'lockState=',lockState
if lockState == 3:
GPSLock = True
navData.status.status = GPSStatus.STATUS_FIX
else:
GPSLock = False
navData.status.status = GPSStatus.STATUS_NO_FIX
navData.pdop = float(fields[15])
navData.hdop = float(fields[16])
navData.vdop = float(fields[17])
navData.status.satellite_visible_prn = []
for i in range(3,15):
if fields[i]=="":
break
navData.status.satellite_visible_prn.append(int(fields[i]))
navData.status.satellites_visible = len(navData.status.satellite_visible_prn)
# In case nobody provides better data:
navData.status.satellites_used = navData.status.satellites_visible
navData.status.satellite_used_prn = navData.status.satellite_visible_prn
if ("$GPRMC" in lastmsg) and GPSLock:
fields = lastmsg["$GPRMC"]