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"]