def nmea_sentence_callback(nmea_sentence, driver): try: driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id, timestamp=nmea_sentence.header.stamp) except ValueError as e: rospy.logwarn("Error:%s" % e)
def nmea_sentence_callback(nmea_sentence, driver): try: driver.add_sentence(nmea_sentence.sentence, frame_id=nmea_sentence.header.frame_id, timestamp=nmea_sentence.header.stamp) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e)
def startgpslog(self): # parse arguments if len(sys.argv) < 2: print( 'Please specify the port (e.g. python gpslog.py /dev/ttyUSB0)') sys.exit(0) else: serial_port = sys.argv[1] # set up port reading GPS = serial.Serial(port=serial_port, baudrate=57600, timeout=2) driver = libnmea_navsat_driver.driver.NMEADriver() # set filename and open csv file timestr = time.strftime("%Y%m%d-%H%M%S.csv") csvfile = open(timestr, "w") csvfile.write( "time,fix,NumSat,latitude,longitude,altitude,speed,ground_course,covariance\n" ) # set time to zero t0 = time.time() try: while True: data = GPS.readline().strip() try: out = driver.add_sentence(data.decode(), time.time() - t0) if out != False: self.usrfun(*out) if not (isnan(out[3]) or isnan(out[4]) or isnan(out[5])): csvfile.write(str(out[0])+","+str(out[1])+","+str(out[2])+","+ \ str(out[3])+","+str(out[4])+","+str(out[5])+","+ \ str(out[6])+","+str(out[7])+","+str(out[8])+"\n") except ValueError as e: warnings.warn( "Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag csvfile with the NMEA sentences that caused it." % e) except KeyboardInterrupt: GPS.close() #Close GPS serial port csvfile.close() # Close CSV file
# POSSIBILITY OF SUCH DAMAGE. import socket import rospy import libnmea_navsat_driver.driver if __name__ == '__main__': rospy.init_node('nmea_udp_driver') bind_ip = rospy.get_param('~bind_ip','10.0.20.254') port = rospy.get_param('~port',1111) frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() try: sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock.bind((bind_ip, port)) driver = libnmea_navsat_driver.driver.RosNMEADriver() while not rospy.is_shutdown(): data = sock.recvfrom(1000)[0].strip() print data try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn("Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) except rospy.ROSInterruptException: sock.close() #Close GPS udp
def gps(): pub = rospy.Publisher('gps_data', String, queue_size=10) rospy.init_node('gps', anonymous=True, log_level=rospy.INFO) rate = rospy.Rate( 10) # 1hz loop runs at this rate checking for stuff on serial port serialport = serial.Serial('/dev/ttyAMA0', baudrate=4800, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS) rospy.loginfo( "Serial connection established; starting GPS...sleeping a bit") rospy.sleep(1) rospy.loginfo("Sending initialization commands") sendcmd(serialport, rate, "$PTNLSRT,H" ) # reset, hot start, uses SRAM data that was loaded from flash hwVersion = sendcmd(serialport, rate, "$PTNLQVR,H") # query hardware version info swVersion = sendcmd(serialport, rate, "$PTNLQVR,S") # query software version navVersion = sendcmd(serialport, rate, "$PTNLQVR,Nn") # query nav version rospy.loginfo("GPS version info: HW=" + hwVersion + " SW=" + swVersion + " NAV=" + navVersion) sendcmd( serialport, rate, "$PTNLSCR,0.60,5.00,12.00,6.00,0.0000060,0,2,1,1") # config reciever #sendcmd(serialport,rate,"$PTNLSDM,0,0.0,0.0,0.0,0.0,0.0")# command not in user guide sendcmd(serialport, rate, "$PTNLSFS,S,0" ) # H = high sensitivity aquisition mode, but slower (S=standard) #sendcmd(serialport,rate,"$PTNLQCR") # query receiver config #sendcmd(serialport,rate,"$PTNLQDM") # unknown command #sendcmd(serialport,rate,"$PTNLQFS") # query aquistion sensotivity mode #sendcmd(serialport,rate,"$PTNLQTF") # query status and position fix sendcmd( serialport, rate, "$PTNLSNM,010D,01" ) # set automatic message output to 0x0107=0xhhh b0111=GGA,VTG every 1 second #sendcmd(serialport,rate,"$PTNLQNM") # query automatic reporting rospy.loginfo("Set up completed") track_made_good_true = 0 track_made_good_magnetic = 0 speed_over_ground = 0 mode_indicator = 0 numSatelites_inview = 0 utc = 0 latitude = 0 longitude = 0 gps_quality = 0 valid = 0 numSatelites_inuse = 0 altitude = 0 magnetic_variation_deg = 0 position_system_mode_indicator = 0 # make driver to parse the sentences as they come from Copernicus II, then publish them driver = libnmea_navsat_driver.driver.RosNMEADriver() gps_frame_id = "argo_gps" # main loop while not rospy.is_shutdown(): if serialport.inWaiting( ): # returns num bytes, so skips to rate.sleep if nothing there rospy.logdebug( "*******************************************************************" ) data = serialport.readline() rospy.logdebug(data.strip()) pub.publish(data) try: # parse NMEA sentence and publish to other topic driver.add_sentence(data.strip(), gps_frame_id, rospy.get_rostime()) except ValueError as e: rospy.logwarn( "Value error, likely due to missing NMEA fields in messge. Error was %s." % e) try: if data.startswith('$GPVTG'): track_made_good_true = data.strip().split(',')[1] track_made_good_magnetic = data.strip().split(',')[3] speed_over_ground = data.strip().split(',')[7] mode_indicator = data.strip().split(',')[9] rospy.logdebug("Track made good true: %s" % track_made_good_true) rospy.logdebug("Track made good magnetic: %s" % track_made_good_magnetic) rospy.logdebug("Speed over ground[km/h]: %s" % speed_over_ground) rospy.logdebug("Mode: %s" % mode_indicator) if data.startswith('$GPGSV'): numSatelites_inview = data.strip().split(',')[3] rospy.logdebug("Satelites in view: %s" % numSatelites_inview) if data.startswith('$GPGGA'): utc = data.strip().split(',')[1] latitude = data.strip().split(',')[2] longitude = data.strip().split(',')[4] gps_quality = data.strip().split(',')[6] numSatelites_inuse = data.strip().split(',')[7] altitude = data.strip().split(',')[9] rospy.logdebug("UTC: %s" % utc) rospy.logdebug("Latitude: %s" % latitude) rospy.logdebug("Longitude: %s" % longitude) rospy.logdebug("GPS quality: %s" % gps_quality) rospy.logdebug("Satelites in use: %s" % numSatelites_inuse) rospy.logdebug("Altitude: %s" % altitude) if data.startswith('$GPRMC'): utc = data.strip().split(',')[1] valid = data.strip().split(',')[2] latitude = data.strip().split(',')[3:4] latitude = ' '.join(latitude) longitude = data.strip().split(',')[5:6] longitude = ' '.join(longitude) speed_over_ground = data.strip().split(',')[7] track_made_good_true = data.strip().split(',')[8] magnetic_variation_deg = data.strip().split(',')[10:11] magnetic_variation_deg = ' '.join(magnetic_variation_deg) position_system_mode_indicator = data.strip().split( ',')[12] rospy.logdebug("UTC: %s" % utc) rospy.logdebug("Valid: %s" % valid) rospy.logdebug("Latitude: %s" % latitude) rospy.logdebug("Longitude: %s" % longitude) rospy.logdebug("Speed over ground [km/h]: %s" % speed_over_ground) rospy.logdebug("Track made good true (deg): %s" % track_made_good_true) rospy.logdebug("Magnetic variation (deg): %s" % magnetic_variation_deg) rospy.logdebug("Position system mode indicator: %s" % position_system_mode_indicator) except IndexError as e: rospy.logwarn( data % ":IndexError, likely due to missing NMEA fields in messge. Error was %s." % e) rate.sleep( ) # sleep for 1/rate (1/10) second, then check again for data # shutdown storing data to flash to enable faster startup next time rospy.loginfo("rospy is shutdown, gracefully shutting down GPS") sendcmd( serialport, rate, "$PTNLSRT,S,2,3\r\n" ) # shutdown, store data to flash, wakeup on next serial port on A or B ports
import sys if __name__ == '__main__': rospy.init_node('nmea_serial_driver') serial_port = rospy.get_param('~port', '/dev/ttyUSB0') serial_baud = rospy.get_param('~baud', 4800) frame_id = libnmea_navsat_driver.driver.RosNMEADriver.get_frame_id() try: GPS = serial.Serial(port=serial_port, baudrate=serial_baud, timeout=2) try: driver = libnmea_navsat_driver.driver.RosNMEADriver() while not rospy.is_shutdown(): data = GPS.readline().strip() try: driver.add_sentence(data, frame_id) except ValueError as e: rospy.logwarn( "Value error, likely due to missing fields in the NMEA message. Error was: %s. Please report this issue at github.com/ros-drivers/nmea_navsat_driver, including a bag file with the NMEA sentences that caused it." % e) except (rospy.ROSInterruptException, serial.serialutil.SerialException): GPS.close() #Close GPS serial port except serial.SerialException as ex: rospy.logfatal( "Could not open serial port: I/O error({0}): {1}".format( ex.errno, ex.strerror))