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