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))