00001
00002
00003 import math, time, serial
00004
00005 import roslib; roslib.load_manifest('clearpath_sensors')
00006 import rospy
00007
00008 from clearpath_sensors.msg import GPSFix
00009
00010
00011 METERS_PER_SEC_PER_KNOT = 0.514444444
00012 FIRST_TIMEOUT_SECS = 3.0
00013 TIMEOUT_SECS = 1.2
00014 RETRY_OPEN_SECS = 1.0
00015
00016 STATE_UNKNOWN = 0
00017 STATE_NOFIX = 1
00018 STATE_FIX = 2
00019
00020
00021 class GPS:
00022 def __init__(self):
00023 rospy.init_node('nmea_gps')
00024 self.port = rospy.get_param('~port', '/dev/ttyUSB0')
00025 self.baud = rospy.get_param('~baud', 4800)
00026 self.serial = None
00027
00028 self.start_serial()
00029 self.state = STATE_UNKNOWN;
00030 self.pub = rospy.Publisher('fix', GPSFix)
00031
00032
00033 def start_serial(self):
00034 if self.serial:
00035 self.serial.close()
00036 self.serial = None
00037
00038 error = None
00039 while True:
00040 try:
00041 self.serial = serial.Serial(self.port, self.baud, timeout=0)
00042 self.timeout = time.clock() + FIRST_TIMEOUT_SECS
00043 rospy.loginfo("Opened GPS on port: %s" % self.port)
00044 break;
00045
00046 except serial.SerialException as e:
00047 if not error:
00048 error = e
00049 rospy.logerr("Error opening GPS serial port: %s" % error)
00050 rospy.logerr("Will retry opening GPS every second.")
00051 time.sleep(RETRY_OPEN_SECS)
00052
00053
00054 def run(self):
00055 buffer = "";
00056 while not rospy.is_shutdown():
00057 time.sleep(0.01)
00058 raw = self.serial.read(255)
00059 if len(raw) > 0:
00060 self.timeout = time.clock() + TIMEOUT_SECS
00061 chunks = raw.split('\n')
00062 buffer += chunks.pop(0)
00063 while len(chunks) > 0:
00064 self.process(buffer)
00065 buffer = chunks.pop(0)
00066 else:
00067 if time.clock() > self.timeout:
00068 rospy.logerr("GPS timed out. Attempting to reopen serial port.")
00069 self.start_serial()
00070
00071
00072 def process(self, buffer):
00073 fields = buffer.split(',')
00074 if fields[0] == '$GPRMC':
00075 if fields[2] == 'A':
00076 if self.state != STATE_FIX:
00077 rospy.loginfo("GPS fix acquired. Publishing to %sfix." % rospy.get_namespace())
00078 self.state = STATE_FIX
00079 fix_msg = GPSFix()
00080 fix_msg.latitude = self._lat(fields)
00081 fix_msg.longitude = self._lon(fields)
00082 if fields[8]: fix_msg.track = float(fields[8])
00083 if fields[7]: fix_msg.speed = float(fields[7]) * METERS_PER_SEC_PER_KNOT
00084 self.pub.publish(fix_msg)
00085 else:
00086 if self.state != STATE_NOFIX:
00087 rospy.loginfo("GPS has no fix.")
00088 self.state = STATE_NOFIX
00089
00090
00091 @staticmethod
00092 def _lat(fields):
00093 inp = float(fields[3])
00094 out = math.floor(inp / 100) + (inp % 100) / 60
00095 if fields[4] == 'S':
00096 out =- out
00097 return out
00098
00099
00100 @staticmethod
00101 def _lon(fields):
00102 inp = float(fields[5])
00103 out = math.floor(inp / 100) + (inp % 100) / 60
00104 if fields[6] == 'W':
00105 out =- out
00106 return out
00107
00108
00109 GPS().run()
