mirror of
https://github.com/Mobile-Robotics-W20-Team-9/UMICH-NCLT-SLAP.git
synced 2025-09-08 20:13:13 +00:00
I commit these to the wrong branchgit status! Whoops
This commit is contained in:
@@ -1,65 +0,0 @@
|
||||
import sys
|
||||
import numpy as np
|
||||
import pickle
|
||||
import math
|
||||
|
||||
def gpstoLocalFrame(lat, lng, alt):
|
||||
lat0 = 0.7381566413
|
||||
lng0 = -1.4610097151
|
||||
alt0 = 265.8
|
||||
|
||||
dLat = np.deg2rad(lat) - lat0
|
||||
dLng = np.deg2rad(lng) - lng0
|
||||
dAlt = alt - alt0
|
||||
|
||||
r = 6400000 # approx. radius of earth (m)
|
||||
y = r * np.cos(lat0) * np.sin(dLng)
|
||||
x = r * np.sin(dLat)
|
||||
z = dAlt
|
||||
|
||||
return [x,y,z]
|
||||
#Example
|
||||
# x = gpstoLocalFrame(42.29360387311647,-83.71222615242006,272)
|
||||
# print(x)
|
||||
|
||||
def buildingtoGPS(building):
|
||||
pickle_in = open('pickles/BuildingMappings.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
for place in currDict:
|
||||
if place == building:
|
||||
return currDict.get(building)
|
||||
return 0
|
||||
|
||||
def findClosestEntrance(building1, building2):
|
||||
gps1 = buildingtoGPS(building1)
|
||||
gps2 = buildingtoGPS(building2)
|
||||
|
||||
x = [0,0,0,0]
|
||||
x[0] = calculateDistance(gps1[0][0],gps1[0][1],gps2[0][0],gps2[0][1])
|
||||
x[1] = calculateDistance(gps1[0][0],gps1[0][1],gps2[1][0],gps2[1][1])
|
||||
x[2] = calculateDistance(gps1[1][0],gps1[1][1],gps2[0][0],gps2[0][1])
|
||||
x[3] = calculateDistance(gps1[1][0],gps1[1][1],gps2[1][0],gps2[1][1])
|
||||
index = np.argmin(x)
|
||||
if index == 0:
|
||||
return [gps1[0],gps2[0]]
|
||||
elif index == 1:
|
||||
return [gps1[0],gps2[1]]
|
||||
elif index == 2:
|
||||
return [gps1[1],gps2[0]]
|
||||
else:
|
||||
return [gps1[1],gps2[1]]
|
||||
# Example
|
||||
# print(findClosestEntrance("BBB", "EECS"))
|
||||
|
||||
def calculateDistance(x1,y1,x2,y2):
|
||||
dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
|
||||
return dist
|
||||
|
||||
# Example usage of overall file
|
||||
'''
|
||||
GPScoords = findClosestEntrance("BBB", "EECS")
|
||||
Building1 = gpstoLocalFrame(GPScoords[0][0], GPScoords[0][1], GPScoords[0][2])
|
||||
Building2 = gpstoLocalFrame(GPScoords[1][0], GPScoords[1][1], GPScoords[1][2])
|
||||
print(Building1)
|
||||
print(Building2)
|
||||
'''
|
@@ -1,57 +0,0 @@
|
||||
(dp0
|
||||
S'EECS'
|
||||
p1
|
||||
(lp2
|
||||
(lp3
|
||||
F42.29259200117389
|
||||
aF-83.71376574039459
|
||||
aI266
|
||||
aa(lp4
|
||||
F42.29250866981882
|
||||
aF-83.71487081050874
|
||||
aI264
|
||||
aasS'Duderstadt'
|
||||
p5
|
||||
(lp6
|
||||
(lp7
|
||||
F42.29067138383511
|
||||
aF-83.7162870168686
|
||||
aI261
|
||||
aa(lp8
|
||||
F42.29158011297468
|
||||
aF-83.71514439582826
|
||||
aI263
|
||||
aasS'FXB'
|
||||
p9
|
||||
(lp10
|
||||
(lp11
|
||||
F42.29360387311647
|
||||
aF-83.71222615242006
|
||||
aI272
|
||||
aa(lp12
|
||||
F42.29359196883519
|
||||
aF-83.71161460876466
|
||||
aI274
|
||||
aasS'Pierpont'
|
||||
p13
|
||||
(lp14
|
||||
(lp15
|
||||
F42.291536462529756
|
||||
aF-83.71705412864686
|
||||
aI261
|
||||
aa(lp16
|
||||
F42.29065947899958
|
||||
aF-83.7178158760071
|
||||
aI258
|
||||
aasS'BBB'
|
||||
p17
|
||||
(lp18
|
||||
(lp19
|
||||
F42.292667396114446
|
||||
aF-83.71626019477846
|
||||
aI264
|
||||
aa(lp20
|
||||
F42.2933737232794
|
||||
aF-83.71622264385225
|
||||
aI271
|
||||
aas.
|
@@ -1,5 +0,0 @@
|
||||
EECS
|
||||
Duderstadt
|
||||
Pierpont
|
||||
BBB
|
||||
FXB
|
@@ -1,30 +0,0 @@
|
||||
import pickle
|
||||
|
||||
# file to print current pickle files to text file
|
||||
# this allows us to monitor current dictionaries
|
||||
def printPickle(filename):
|
||||
pickle_in = open(filename + '.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
f = open(filename + '.txt',"w")
|
||||
for x in currDict:
|
||||
f.write('%s\n' % x )
|
||||
f.close()
|
||||
|
||||
# update pickle files to update dictionaries
|
||||
# example: grades = {'Bart', 'Lisa', 'Milhouse', 'Nelson'}
|
||||
def createPickle(filename, pklList):
|
||||
f = open(filename + '.pkl', 'wb') # Pickle file is newly created where foo1.py is
|
||||
pickle.dump(pklList, f) # dump data to f
|
||||
f.close()
|
||||
|
||||
def updatePickle(filename, pklList):
|
||||
pickle_in = open(filename + '.pkl',"rb")
|
||||
currDict = pickle.load(pickle_in)
|
||||
f = open(filename + '.pkl', 'wb') # Pickle file is newly created where foo1.py is
|
||||
pickle.dump(currDict + pklList, f) # dump data to f
|
||||
f.close()
|
||||
|
||||
# Example usage
|
||||
# createPickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
|
||||
# updatePickle('test', {'Theo'})
|
||||
# printPickle("test")
|
Reference in New Issue
Block a user