Merge pull request #13 from Mobile-Robotics-W20-Team-9/GPStoBuilding

Gp sto building

src/Dockerfile

@@ -14,6 +14,7 @@ RUN pip install -U pip && \
         matplotlib \
         nltk \
         setuptools \
-        pylint
+        pylint \
+        pickle-mixin
 
 CMD ["/bin/bash"]

src/dataset/dataManipulation/GPSmanip.py

@@ -0,0 +1,25 @@
+import sys
+import numpy as np
+
+# Usage: changes lat, long, alt into xyz local frame
+#           This takes in lat, long, and alt and calculates xyz
+# Example: print(gpstoLocalFrame(42.29360387311647,-83.71222615242006,272))
+# Returns: array of XYZ coordinates in radians
+def gpstoLocalFrame(lat, lng, alt):
+    lat0 = 0.7381566413
+    lng0 = -1.4610097151
+    alt0 = 265.8
+
+    print(np.deg2rad(lng))
+    dLat = np.deg2rad(lat) - lat0
+    print(dLat)
+    dLng = np.deg2rad(lng) - lng0
+    dAlt = alt - alt0
+
+    r = 6400000 # approx. radius of earth (m)
+    # WARNING: x and y may need to be flipped. Paper and example files from NCLT have contradictory usages
+    y = r * np.cos(lat0) * np.sin(dLng)
+    x = r * np.sin(dLat)
+    z = dAlt
+
+    return [x,y,z]

src/dataset/dataManipulation/findBuildingCoord.py

@@ -0,0 +1,79 @@
+import sys
+import numpy as np
+import pickle
+import math  
+
+# Example usage of overall file to find xyz coordinates of two buildings given name as string
+'''
+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)
+'''
+
+# Usage: finds building coordinates in lat, long, and alt in degrees
+#           This takes in two building names as strings and returns 
+#           closest two entrances 
+# Example: GPScoords = findClosestEntrance("BBB", "EECS")
+#           print(GPScoords[0][0]) #returns latitutde of first building
+# Returns: 2x2 array of two GPS coordinates in lat, long, and alt in degrees
+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]]
+
+# Usage: finds building coordinates in lat, long, and alt in degrees
+#           This takes in a building name and looks up coordinates in pickle file
+# Example: buildingsGPScoords = buildingtoGPS(building1)
+#           print(buildingGPScoords[0]) #returns latitutde of the building
+# Returns: array of GPS coordinates (lat, long, and alt) in degrees
+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
+
+# Usage: changes lat, long, alt into xyz local frame
+#           This takes in lat, long, and alt and calculates xyz
+# Example: print(gpstoLocalFrame(42.29360387311647,-83.71222615242006,272))
+# Returns: array of XYZ coordinates in radians
+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)
+    # WARNING: x and y may need to be flipped. Paper and example files from NCLT have contradictory usages
+    y = r * np.cos(lat0) * np.sin(dLng) 
+    x = r * np.sin(dLat)
+    z = dAlt
+
+    return [x,y,z]
+
+# Usage: Euclidean distance calculator - helper function
+def calculateDistance(x1,y1,x2,y2):  
+     dist = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)  
+     return dist  
+
+     

src/dataset/dataManipulation/pickles/BuildingMappings.pkl

@@ -0,0 +1,57 @@
+(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.

src/dataset/dataManipulation/pickles/BuildingMappings.txt

@@ -0,0 +1,5 @@
+EECS
+Duderstadt
+Pierpont
+BBB
+FXB

src/dataset/dataManipulation/pickles/pickleManage.py

@@ -0,0 +1,32 @@
+import pickle
+
+# Usage: file to print current pickle files to text file
+#           this allows us to monitor current dictionaries
+# Example: printPickle("BuildingMappings")
+# Output: Text file of dictonary keys
+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()
+
+# Usage: creates pickle files from given dictionaries
+# Example: createPickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
+# Output: new Pickle file
+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()
+
+# Usage: updates pickle files from given dictionaries
+# Example: updatePickle('test', {'Bart', 'Lisa', 'Milhouse', 'Nelson'})
+# Output: Pickle file
+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()
+

src/dataset/dataManipulation/pickles/referenceBuildings4Pickle.txt

@@ -0,0 +1,29 @@
+Reference file for coordinates found for google maps
+
+(latitude, longitude, altitude)
+FXB 
+	Beal Ave -  42.29360387311647 -83.71222615242006 272 
+	Hayward -   42.29359196883519 -83.71161460876466 274
+	
+EECS
+	Beal Ave -  42.29259200117389 -83.71376574039459 266 
+	Grove - 42.29250866981882 -83.71487081050874 264 
+
+BBB
+	Grove - 42.292667396114446 -83.71626019477846 264
+	Hayward - 42.2933737232794 -83.71622264385225 271
+	
+Pierpont
+	Grove - 42.291536462529756 -83.71705412864686 261
+	Bonisteel - 42.29065947899958  -83.7178158760071 258
+	
+Duderstadt
+	Bonisteel - 42.29067138383511 -83.7162870168686 261
+	Grover - 42.29158011297468 -83.71514439582826 263
+
+Example for making and changing buildings from pickle file
+
+from pickleManage import *
+createPickle("BuildingMappings", {'Duderstadt':[[42.29067138383511, -83.7162870168686, 261],[42.29158011297468, -83.71514439582826, 263]], 'Pierpont':[[42.291536462529756, -83.71705412864686, 261],[42.29065947899958,  -83.7178158760071, 258]], 'BBB':[[42.292667396114446, -83.71626019477846, 264],[42.2933737232794, -83.71622264385225, 271]], 'EECS':[[42.29259200117389, -83.71376574039459, 266],[42.29250866981882, -83.71487081050874, 264]],'FXB':[[42.29360387311647, -83.71222615242006, 272],[42.29359196883519, -83.71161460876466, 274]]})
+updatePickle("BuildingMappings", {'Duderstadt':[[42.29067138383511, -83.7162870168686, 261],[42.29158011297468, -83.71514439582826, 263]]})
+printPickle("BuildingMappings")