Ejemplo n.º 1
0
 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 ) )
Ejemplo n.º 2
0
                    # 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"]