forked from EveRobotics/bauxi_system_controller
-
Notifications
You must be signed in to change notification settings - Fork 0
/
static_maps.py
95 lines (79 loc) · 3.16 KB
/
static_maps.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
"""
Created on Sep 27, 2015
@author: weston
"""
import matplotlib.pyplot as plt
from motionless import LatLonMarker, DecoratedMap
import urllib
import cv2
class StaticMap(object):
"""
Show the robot's location and sensor data, blocked versus unblocked areas.
"""
def __init__(self):
"""What would be good initialization parameters to pass here?
"""
self.__sizeX = 640
self.__sizeY = 480
# This scale may need to change if all the points don't fit in the
# image.
self.__zoom = 18
self.__mapType = 'hybrid'
plt.ion()
return
def getMapImage(self, currentLocation, missionLocations, mapName='location'
, showMap=False):
"""Use the static maps API, add the waypoints to the map as markers,
and return the image.
"""
if currentLocation[0] != 0 or currentLocation[1] != 0:
print 'Current location is initialized:'
print 'Lat:', currentLocation[0], 'Lon:', currentLocation[1]
coor1 = round(currentLocation[0], 4)
coor2 = round(currentLocation[1], 4)
dmap = DecoratedMap(lat=coor1,lon=coor2
, size_x=self.__sizeX, size_y=self.__sizeY
, zoom=self.__zoom, maptype=self.__mapType)
marker = LatLonMarker(lat=coor1, lon=coor2, color='green', label='P')
print 'Drawing waypoint position (P):', (coor1, coor2), '(green).'
dmap.add_marker(marker)
else:
print 'Current location is uninitialized:'
location = missionLocations[0]
coor1 = round(location[0], 4)
coor2 = round(location[1], 4)
dmap = DecoratedMap(lat=coor1,lon=coor2
, size_x=self.__sizeX, size_y=self.__sizeY
, zoom=self.__zoom, maptype=self.__mapType)
index = 1
for location in missionLocations:
coor1 = round(location[0], 4)
coor2 = round(location[1], 4)
print 'Lat:', coor1, 'Lon:', coor2
if location[2] == 'waypoint':
print 'Drawing waypoint marker:', index, '(blue).'
color = 'blue'
elif location[2] == 'target':
print 'Drawing target marker:', index, '(red).'
color = 'red'
else:
print 'Drawing other marker:', index, '(green).'
color = 'green'
label = str(index)
index += 1
marker = LatLonMarker(lat=coor1, lon=coor2, color=color, label=label)
dmap.add_marker(marker)
imageFileName = mapName + ".png"
url = dmap.generate_url()
print url
urllib.urlretrieve(url, imageFileName)
# Load an color image in grayscale
img = cv2.imread(imageFileName, cv2.IMREAD_COLOR)
if showMap:
plt.clf()
# cmap = 'gray', # TODO: figure out what color map to use.
plt.imshow(img, interpolation = 'bicubic')
plt.show()
plt.draw()
#cv2.destroyAllWindows()
return img