nmea_gps_driver.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Software License Agreement (BSD License)
00004 #
00005 # Copyright (c) 2012, Steven Martin, Eric Perko
00006 # All rights reserved.
00007 #
00008 # Redistribution and use in source and binary forms, with or without
00009 # modification, are permitted provided that the following conditions
00010 # are met:
00011 #
00012 #  * Redistributions of source code must retain the above copyright
00013 #    notice, this list of conditions and the following disclaimer.
00014 #  * Redistributions in binary form must reproduce the above
00015 #    copyright notice, this list of conditions and the following
00016 #    disclaimer in the documentation and/or other materials provided
00017 #    with the distribution.
00018 #  * Neither the names of the authors nor the names of their
00019 #    affiliated organizations may be used to endorse or promote products derived
00020 #    from this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 
00035 import roslib
00036 roslib.load_manifest('nmea_gps_driver')
00037 import rospy
00038 from sensor_msgs.msg import NavSatFix
00039 from sensor_msgs.msg import NavSatStatus
00040 from sensor_msgs.msg import TimeReference
00041 from geometry_msgs.msg import TwistStamped
00042 
00043 import serial, string, math, time, calendar
00044 
00045 #nmea_utc should be a string of form hhmmss
00046 def convertNMEATimeToROS(nmea_utc):
00047     #Get current time in UTC for date information
00048     utc_struct = time.gmtime() #immutable, so cannot modify this one
00049     utc_list = list(utc_struct)
00050     hours = int(nmea_utc[0:2])
00051     minutes = int(nmea_utc[2:4])
00052     seconds = int(nmea_utc[4:6])
00053     utc_list[3] = hours
00054     utc_list[4] = minutes
00055     utc_list[5] = seconds
00056     unix_time = calendar.timegm(tuple(utc_list))
00057     return rospy.Time.from_sec(unix_time)
00058 
00059 #Add the tf_prefix to the given frame id
00060 def addTFPrefix(frame_id):
00061     prefix = ""
00062     prefix_param = rospy.search_param("tf_prefix")
00063     if prefix_param:
00064         prefix = rospy.get_param(prefix_param)
00065         if prefix[0] != "/":
00066             prefix = "/%s" % prefix
00067 
00068     return "%s/%s" % (prefix, frame_id)
00069 
00070 #Check the NMEA sentence checksum. Return True if passes and False if failed
00071 def check_checksum(nmea_sentence):
00072     split_sentence = nmea_sentence.split('*')
00073     transmitted_checksum = split_sentence[1].strip()
00074     
00075     #Remove the $ at the front
00076     data_to_checksum = split_sentence[0][1:]
00077     checksum = 0
00078     for c in data_to_checksum:
00079         checksum ^= ord(c)
00080 
00081     return ("%02X" % checksum)  == transmitted_checksum.upper()
00082 
00083 if __name__ == "__main__":
00084     #init publisher
00085     rospy.init_node('nmea_gps_driver')
00086     gpspub = rospy.Publisher('fix', NavSatFix)
00087     gpsVelPub = rospy.Publisher('vel',TwistStamped)
00088     gpstimePub = rospy.Publisher('time_reference', TimeReference)
00089     #Init GPS port
00090     GPSport = rospy.get_param('~port','/dev/ttyUSB0')
00091     GPSrate = rospy.get_param('~baud',4800)
00092     frame_id = rospy.get_param('~frame_id','gps')
00093     if frame_id[0] != "/":
00094         frame_id = addTFPrefix(frame_id)
00095 
00096     time_ref_source = rospy.get_param('~time_ref_source', frame_id)
00097     useRMC = rospy.get_param('~useRMC', False)
00098     #useRMC == True -> generate info from RMC+GSA
00099     #useRMC == False -> generate info from GGA
00100     navData = NavSatFix()
00101     gpsVel = TwistStamped()
00102     gpstime = TimeReference()
00103     gpstime.source = time_ref_source
00104     navData.header.frame_id = frame_id
00105     gpsVel.header.frame_id = frame_id
00106     GPSLock = False
00107     try:
00108         GPS = serial.Serial(port=GPSport, baudrate=GPSrate, timeout=2)
00109         #Read in GPS
00110         while not rospy.is_shutdown():
00111             #read GPS line
00112             data = GPS.readline()
00113 
00114             if not check_checksum(data):
00115                 rospy.logerr("Received a sentence with an invalid checksum. Sentence was: %s" % data)
00116                 continue
00117 
00118             timeNow = rospy.get_rostime()
00119             fields = data.split(',')
00120             for i in fields:
00121                 i = i.strip(',')
00122             try:
00123                 if useRMC:
00124                     #Check for satellite lock
00125                     if 'GSA' in fields[0]:
00126                         lockState = int(fields[2])
00127                         #print 'lockState=',lockState
00128                         if lockState == 3:
00129                             GPSLock = True
00130                         else:
00131                             GPSLock = False
00132                     #if not satellite lock message parse it separately
00133                     else:
00134                         if GPSLock == True:
00135                             if 'RMC' in fields[0]:
00136                                 #print fields
00137                                 gpsVel.header.stamp = timeNow
00138                                 gpsVel.twist.linear.x = float(fields[7])*0.514444444444*math.sin(float(fields[8]))
00139                                 gpsVel.twist.linear.y = float(fields[7])*0.514444444444*math.cos(float(fields[8]))
00140                                 gpsVelPub.publish(gpsVel)
00141 
00142                                 navData.status.status = NavSatStatus.STATUS_FIX
00143                                 navData.header.stamp = gpsVel.header.stamp
00144                                 navData.status.service = NavSatStatus.SERVICE_GPS
00145 
00146                                 gpstime.header.stamp = gpsVel.header.stamp
00147                                 gpstime.time_ref = convertNMEATimeToROS(fields[1])
00148 
00149                                 longitude = float(fields[5][0:3]) + float(fields[5][3:])/60
00150                                 if fields[6] == 'W':
00151                                     longitude = -longitude
00152 
00153                                 latitude = float(fields[3][0:2]) + float(fields[3][2:])/60
00154                                 if fields[4] == 'S':
00155                                     latitude = -latitude
00156 
00157                                 #publish data
00158                                 navData.latitude = latitude
00159                                 navData.longitude = longitude
00160                                 navData.altitude = float('NaN')
00161                                 navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
00162                                 gpspub.publish(navData)
00163                                 gpstimePub.publish(gpstime)
00164                         else:
00165                             pass
00166                             #print data
00167                 else:
00168                     #Use GGA
00169                     #No /vel output from just GGA
00170                     if 'GGA' in fields[0]:
00171                         gps_quality = int(fields[6])
00172                         if gps_quality == 0:
00173                             navData.status.status = NavSatStatus.STATUS_NO_FIX
00174                         elif gps_quality == 1:
00175                             navData.status.status = NavSatStatus.STATUS_FIX
00176                         elif gps_quality == 2:
00177                             navData.status.status = NavSatStatus.STATUS_SBAS_FIX
00178                         elif gps_quality in (4,5):
00179                             #Maybe 2 should also sometimes be GBAS... but pretty
00180                             #sure RTK has to have a base station
00181                             navData.status.status = NavSatStatus.STATUS_GBAS_FIX
00182                         else:
00183                             navData.status.status = NavSatStatus.STATUS_NO_FIX
00184                         navData.status.service = NavSatStatus.SERVICE_GPS
00185 
00186                         navData.header.stamp = timeNow
00187 
00188                         latitude = float(fields[2][0:2]) + float(fields[2][2:])/60
00189                         if fields[3] == 'S':
00190                             latitude = -latitude
00191                         navData.latitude = latitude
00192 
00193                         longitude = float(fields[4][0:3]) + float(fields[4][3:])/60
00194                         if fields[5] == 'W':
00195                             longitude = -longitude
00196                         navData.longitude = longitude
00197 
00198                         hdop = float(fields[8])
00199                         navData.position_covariance[0] = hdop**2
00200                         navData.position_covariance[4] = hdop**2
00201                         navData.position_covariance[8] = (2*hdop)**2 #FIX ME
00202                         navData.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
00203 
00204                         #Altitude is above ellipsoid, so adjust for mean-sea-level
00205                         altitude = float(fields[9]) + float(fields[11])
00206                         navData.altitude = altitude
00207 
00208 
00209                         gpstime.header.stamp = timeNow
00210                         gpstime.time_ref = convertNMEATimeToROS(fields[1])
00211 
00212                         gpspub.publish(navData)
00213                         gpstimePub.publish(gpstime)
00214             except ValueError as e:
00215                 rospy.logwarn("Value error, likely due to missing fields in the NMEA messages. Error was: %s" % e)
00216 
00217     except rospy.ROSInterruptException:
00218         GPS.close() #Close GPS serial port


nmea_gps_driver
Author(s): Steven Martin, Eric Perko
autogenerated on Thu Jan 2 2014 11:31:42