|
| 1 | +import xml.dom.minidom as md |
| 2 | +import time as _time |
| 3 | +import datetime |
| 4 | +from math import sin,cos, sqrt, hypot, radians, atan2 |
| 5 | + |
| 6 | +def GPS_DIST(wp1,wp2): |
| 7 | + """wp format: (lat,lon) as decimal degrees""" |
| 8 | + R = 6371000 #avg radius of earth in meters |
| 9 | + lon1 = radians(wp1[1]) |
| 10 | + lon2 = radians(wp2[1]) |
| 11 | + lat1 = radians(wp1[0]) |
| 12 | + lat2 = radians(wp2[0]) |
| 13 | + dlat = lat1-lat2 |
| 14 | + dlon = lon1-lon2 |
| 15 | + #sq of half the chord length |
| 16 | + a = sin(dlat/2)*sin(dlat/2)+cos(lat1)*cos(lat2)*(sin(dlon/2)*sin(dlon/2)) |
| 17 | + # angular distance in radians |
| 18 | + c = 2*atan2(sqrt(a),sqrt(1-a)) |
| 19 | + # dist |
| 20 | + return R*c |
| 21 | + |
| 22 | +def parse_GPRMC(s): |
| 23 | + CS = int(s[-2:],16) |
| 24 | + if CS != checksum(s[1:-3]): |
| 25 | + print 'CS FAIL!' |
| 26 | + return None |
| 27 | + |
| 28 | + d = RMCdat() |
| 29 | + litems = s.split(',') |
| 30 | + #check valid data |
| 31 | + if litems[2] != 'A': |
| 32 | + return None |
| 33 | + time_stamp = litems[1] |
| 34 | + hr = int(time_stamp[0:2]) |
| 35 | + mins = int(time_stamp[2:4]) |
| 36 | + sec = int(time_stamp[4:6]) |
| 37 | + usec = int(time_stamp[7:10])*1000 |
| 38 | + lat = litems[3] |
| 39 | + lon = litems[5] |
| 40 | + #convert to degrees |
| 41 | + latd = int(lat[:-7])+float(lat[-7:])/60.0 |
| 42 | + lond = int(lon[:-7])+float(lon[-7:])/60.0 |
| 43 | + #add sign |
| 44 | + if litems[4]=='S': |
| 45 | + latd = -latd |
| 46 | + if litems[6]=='W': |
| 47 | + lond = -lond |
| 48 | + d.lat = latd |
| 49 | + d.lon = lond |
| 50 | + #speed in kph |
| 51 | + d.speed = float(litems[7])*1.852 |
| 52 | + #date |
| 53 | + datestamp = litems[9] |
| 54 | + day = int(datestamp[0:2]) |
| 55 | + month = int(datestamp[2:4]) |
| 56 | + year = int(datestamp[4:])+2000 |
| 57 | + #fill in datetime object |
| 58 | + d.dtime = datetime.datetime(year,month,day,hr,mins,sec,usec) |
| 59 | + |
| 60 | + return d |
| 61 | + |
| 62 | +def checksum(s): |
| 63 | + f =map(ord,s) |
| 64 | + cs = 0 |
| 65 | + for q in f: |
| 66 | + cs = cs ^q |
| 67 | + return cs |
| 68 | + |
| 69 | +def parse_waypoints(file_name): |
| 70 | + """ returns an ordered list of waypoints from a gpx file file_name""" |
| 71 | + tree = md.parse(file_name) |
| 72 | + assert tree.documentElement.tagName == 'gpx' |
| 73 | + route = tree.getElementsByTagName('rte')[0] |
| 74 | + #TODO: check above succeeds |
| 75 | + |
| 76 | + wp_list = [] |
| 77 | + for rpoint in route.getElementsByTagName('rtept'): |
| 78 | + |
| 79 | + #print rpoint.getElementsByTagName('name')[0].nodeName |
| 80 | + #print rpoint.getElementsByTagName('name')[0].firstChild.nodeValue |
| 81 | + assert rpoint.hasAttributes() |
| 82 | + coord = (float(rpoint.getAttribute('lat')), |
| 83 | + float(rpoint.getAttribute('lon')) ) |
| 84 | + wp_list.append(coord) |
| 85 | + #print 'lat: ' + rpoint.getAttribute('lat') + \ |
| 86 | + # ', lon: ' + rpoint.getAttribute('lon') |
| 87 | + return wp_list |
| 88 | + |
| 89 | +def latlon2rect(origin,gpspoint): |
| 90 | + """Convert a gpspoint to rectangular coords, where the origin is the gps location of (0,0)""" |
| 91 | + dx = GPS_DIST(origin,(origin[0],gpspoint[1])) #change only in x |
| 92 | + dy = GPS_DIST(origin,(gpspoint[0],origin[1])) #change only in y |
| 93 | + |
| 94 | + if gpspoint[1] < origin[1]: |
| 95 | + dx = -dx |
| 96 | + if gpspoint[0] < origin[0]: |
| 97 | + dy = -dy |
| 98 | + |
| 99 | + return (dx,dy ) |
| 100 | + |
| 101 | +def _project(A, B): |
| 102 | + """ Length of A's component in B direction (ie A.B/||B||)""" |
| 103 | + Blen = _vlen(B) |
| 104 | + Bn = (B[0]/Blen, B[1]/Blen) #normalize vector B |
| 105 | + return (A[0]*Bn[0]+A[1]*Bn[1]) |
| 106 | + |
| 107 | +def _vlen(B): |
| 108 | + """return length of 2d vector B""" |
| 109 | + return sqrt(B[0]*B[0]+B[1]*B[1]) |
| 110 | + |
| 111 | +def _dist(A,B): |
| 112 | + return hypot(A[0]-B[0],A[1]-B[1]) |
| 113 | + |
| 114 | + |
| 115 | + |
| 116 | +############# DATA TYPES ####################### |
| 117 | +class RMCdat: |
| 118 | + def __init__(self): |
| 119 | + self.dtime = None |
| 120 | + self.speed = 0.0 |
| 121 | + self.lon = 0.0 |
| 122 | + self.lat = 0.0 |
| 123 | + |
| 124 | + def getepochtime(self): |
| 125 | + if self.dtime ==None: |
| 126 | + return None |
| 127 | + return (_time.mktime(self.dtime.timetuple())+ |
| 128 | + self.dtime.microsecond/1000000.0) |
| 129 | + |
| 130 | + |
| 131 | + |
| 132 | +class Chord: |
| 133 | + """ |
| 134 | + Defines a chord specified by two points p1, p2 defined by (x,y) |
| 135 | + note: p1 should always be closer (by driving distance) than p2 |
| 136 | + """ |
| 137 | + |
| 138 | + def __init__(self,p2): |
| 139 | + self.p1 = (0,0) |
| 140 | + self.p2 = p2 |
| 141 | + |
| 142 | + def __init__(self,p1,p2): |
| 143 | + self.p1 = p1 |
| 144 | + self.p2 = p2 |
| 145 | + |
| 146 | + def closest(self,p): |
| 147 | + """ returns the closest point on the chord to p """ |
| 148 | + |
| 149 | + a = _vlen((self.p1[0]-self.p2[0],self.p1[1]-self.p2[1])) |
| 150 | + b = _vlen((self.p1[0]-p[0],self.p1[1]-p[1])) |
| 151 | + c = _vlen((p[0]-self.p2[0],p[1]-self.p2[1])) |
| 152 | + |
| 153 | + #is the closest point on the line? |
| 154 | + if (a*a+b*b>c*c) and (a*a+c*c>b*b): |
| 155 | + line_p_p1 = (self.p1[0]-p[0],self.p1[1]-p[1]) |
| 156 | + v_orth = (self.p1[1]-self.p2[1],-self.p1[0]+self.p2[0]) |
| 157 | + v_orth_norm = (v_orth[0]/_vlen(v_orth), v_orth[1]/_vlen(v_orth)) |
| 158 | + dist = _project(line_p_p1, v_orth_norm) |
| 159 | + return (v_orth_norm[0]*dist+p[0],v_orth_norm[1]*dist+p[1]) |
| 160 | + else: |
| 161 | + if b<c: |
| 162 | + return self.p1 |
| 163 | + else: |
| 164 | + return self.p2 |
| 165 | + |
| 166 | + def length(self): |
| 167 | + return _dist(self.p1,self.p2) |
| 168 | + |
| 169 | + |
| 170 | +class ClosedPath: |
| 171 | + def __init__(self, points): |
| 172 | + self.chords = [] |
| 173 | + for i in range(len(points)): |
| 174 | + j = i+1 |
| 175 | + if j == len(points): |
| 176 | + j = -1 |
| 177 | + self.chords.append(Chord(points[i],points[j])) |
| 178 | + |
| 179 | + def closest(self,p): |
| 180 | + """returns the closest point on path to p""" |
| 181 | + min_coord = self.chords[0].closest(p) |
| 182 | + min_dist = _dist(p,min_coord) |
| 183 | + for c in self.chords: |
| 184 | + coord = c.closest(p) |
| 185 | + if min_dist > _dist(p,coord): |
| 186 | + min_dist = _dist(p,coord) |
| 187 | + min_coord = coord |
| 188 | + |
| 189 | + return min_coord |
| 190 | + |
| 191 | + def driving_distance(self,p): |
| 192 | + """returns the driving distance between p and Start/Finish point """ |
| 193 | + min_coord = self.chords[0].closest(p) |
| 194 | + min_dist = _dist(p,min_coord) |
| 195 | + #length from min_coord to SF line |
| 196 | + l = _dist(min_coord,self.chords[0].p1) |
| 197 | + pl = 0 #length of all previous chords |
| 198 | + for c in self.chords: |
| 199 | + coord = c.closest(p) |
| 200 | + if min_dist > _dist(p,coord): |
| 201 | + min_dist = _dist(p,coord) |
| 202 | + min_coord = coord |
| 203 | + l = pl+_dist(coord,c.p1) |
| 204 | + #print "----" + str(l) |
| 205 | + pl = pl+c.length() |
| 206 | + return l |
0 commit comments