mirror of
https://github.com/OSURoboticsClub/Rover_2017_2018.git
synced 2025-11-08 18:21:15 +00:00
change self only function in Overlay Class, and start on rover gps location
This commit is contained in:
Christopher Pham
@@ -1,448 +1,448 @@
|
||||
'''
|
||||
Mapping.py: Objected Orientated Google Maps for Python
|
||||
ReWritten by Chris Pham
|
||||
|
||||
Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy
|
||||
|
||||
This code is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as
|
||||
published by the Free Software Foundation, either version 3 of the
|
||||
License, or (at your option) any later version.
|
||||
This code is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
You should have received a copy of the GNU Lesser General Public License
|
||||
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
||||
'''
|
||||
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
import math
|
||||
import urllib2
|
||||
from io import StringIO, BytesIO
|
||||
import os
|
||||
import time
|
||||
import PIL.ImageDraw
|
||||
import signing
|
||||
import RoverMapHelper as MapHelper
|
||||
|
||||
#####################################
|
||||
# Constants
|
||||
#####################################
|
||||
_KEYS = []
|
||||
# Number of pixels in half the earth's circumference at zoom = 21
|
||||
_EARTHPIX = 268435456
|
||||
# Number of decimal places for rounding coordinates
|
||||
_DEGREE_PRECISION = 4
|
||||
# Larget tile we can grab without paying
|
||||
_TILESIZE = 640
|
||||
# Fastest rate at which we can download tiles without paying
|
||||
_GRABRATE = 4
|
||||
# Pixel Radius of Earth for calculations
|
||||
_PIXRAD = _EARTHPIX / math.pi
|
||||
_DISPLAYPIX = _EARTHPIX / 2000
|
||||
|
||||
file_pointer = open('key', 'r')
|
||||
for i in file_pointer:
|
||||
_KEYS.append(i.rstrip())
|
||||
file_pointer.close()
|
||||
|
||||
|
||||
class GMapsStitcher(object):
|
||||
def __init__(self, width, height,
|
||||
latitude, longitude, zoom,
|
||||
maptype, radius_meters=None, num_tiles=4, debug=False):
|
||||
self.helper = MapHelper.MapHelper()
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.start_latitude = latitude
|
||||
self.start_longitude = longitude
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.zoom = zoom
|
||||
self.maptype = maptype
|
||||
self.radius_meters = radius_meters
|
||||
self.num_tiles = num_tiles
|
||||
self.display_image = self.helper.new_image(width, height)
|
||||
self.debug = debug
|
||||
|
||||
# Get the big image here
|
||||
self._fetch()
|
||||
self.center_display(latitude, longitude)
|
||||
|
||||
def __str__(self):
|
||||
"""
|
||||
This string returns when used in a print statement
|
||||
Useful for debugging and to print current state
|
||||
|
||||
returns STRING
|
||||
"""
|
||||
string_builder = ""
|
||||
string_builder += ("Center of the displayed map: %4f, %4f\n" %
|
||||
(self.center_x, self.center_y))
|
||||
string_builder += ("Center of the big map: %4fx%4f\n" %
|
||||
(self.start_longitude, self.start_longitude))
|
||||
string_builder += ("Current latitude is: %4f, %4f\n" %
|
||||
(self.longitude, self.latitude))
|
||||
string_builder += ("The top-left of the box: %dx%d\n" %
|
||||
(self.left_x, self.upper_y))
|
||||
string_builder += ("Number of tiles genreated: %dx%d\n" %
|
||||
(self.num_tiles, self.num_tiles))
|
||||
string_builder += "Map Type: %s\n" % (self.maptype)
|
||||
string_builder += "Zoom Level: %s\n" % (self.zoom)
|
||||
string_builder += ("Dimensions of Big Image: %dx%d\n" %
|
||||
(self.big_image.size[0], self.big_image.size[1]))
|
||||
string_builder += ("Dimensions of Displayed Image: %dx%d\n" %
|
||||
(self.width, self.height))
|
||||
string_builder += ("LatLong of Northwest Corner: %4f, %4f\n" %
|
||||
(self.northwest))
|
||||
string_builder += ("LatLong of Southeast Corner: %4f, %4f\n" %
|
||||
(self.southeast))
|
||||
return string_builder
|
||||
|
||||
def _grab_tile(self, longitude, latitude, sleeptime=0):
|
||||
"""
|
||||
This will return the tile at location longitude x latitude.
|
||||
Includes a sleep time to allow for free use if there is no API key
|
||||
|
||||
returns PIL.IMAGE OBJECT
|
||||
"""
|
||||
# Make the url string for polling
|
||||
# GET request header gets appended to the string
|
||||
urlbase = 'https://maps.googleapis.com/maps/api/staticmap?'
|
||||
urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s'
|
||||
urlbase += '&size=%dx%d&format=png&key=%s'
|
||||
|
||||
# Fill the formatting
|
||||
specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION),
|
||||
self.helper.fast_round(longitude, _DEGREE_PRECISION),
|
||||
self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0])
|
||||
filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs)
|
||||
filename += '.png'
|
||||
|
||||
# Tile Image object
|
||||
tile_object = None
|
||||
|
||||
if os.path.isfile(filename):
|
||||
tile_object = PIL.Image.open(filename)
|
||||
|
||||
# If file on filesystem
|
||||
else:
|
||||
# make the url
|
||||
url = urlbase % specs
|
||||
url = signing.sign_url(url, _KEYS[1])
|
||||
result = urllib2.urlopen(urllib2.Request(url)).read()
|
||||
tile_object = PIL.Image.open(BytesIO(result))
|
||||
if not os.path.exists('Resources/Maps'):
|
||||
os.mkdir('Resources/Maps')
|
||||
tile_object.save(filename)
|
||||
# Added to prevent timeouts on Google Servers
|
||||
time.sleep(sleeptime)
|
||||
|
||||
return tile_object
|
||||
|
||||
def _pixels_to_lon(self, iterator, lon_pixels):
|
||||
"""
|
||||
This converts pixels to degrees to be used in
|
||||
fetching squares and generate correct squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines, no idea
|
||||
degrees = self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles / 2) * _TILESIZE, self.zoom)
|
||||
return math.degrees((lon_pixels + degrees - _EARTHPIX) / _PIXRAD)
|
||||
|
||||
def _pixels_to_lat(self, iterator, lat_pixels):
|
||||
"""
|
||||
This converts pixels to latitude using meridian projection
|
||||
to get the latitude to generate squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines
|
||||
return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
|
||||
self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles /
|
||||
2) * _TILESIZE, self.zoom)) -
|
||||
_EARTHPIX) / _PIXRAD)))
|
||||
|
||||
def fetch_tiles(self):
|
||||
"""
|
||||
Function that handles fetching of files from init'd variables
|
||||
|
||||
returns PIL.IMAGE OBJECT, (WEST, NORTH), (EAST, SOUTH)
|
||||
|
||||
North/East/South/West are in FLOAT(degrees)
|
||||
"""
|
||||
# cap floats to precision amount
|
||||
self.latitude = self.helper.fast_round(self.latitude,
|
||||
_DEGREE_PRECISION)
|
||||
self.longitude = self.helper.fast_round(self.longitude,
|
||||
_DEGREE_PRECISION)
|
||||
|
||||
# number of tiles required to go from center
|
||||
# latitude to desired radius in meters
|
||||
if self.radius_meters is not None:
|
||||
self.num_tiles = (int(
|
||||
round(2 * self.helper.pixels_to_meters(
|
||||
self.latitude, self.zoom) /
|
||||
(_TILESIZE / 2. / self.radius_meters))))
|
||||
|
||||
lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD)
|
||||
|
||||
sin_lat = math.sin(math.radians(self.latitude))
|
||||
lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2
|
||||
self.big_size = self.num_tiles * _TILESIZE
|
||||
big_image = self.helper.new_image(self.big_size, self.big_size)
|
||||
|
||||
for j in range(self.num_tiles):
|
||||
lon = self._pixels_to_lon(j, lon_pixels)
|
||||
for k in range(self.num_tiles):
|
||||
lat = self._pixels_to_lat(k, lat_pixels)
|
||||
tile = self._grab_tile(lon, lat)
|
||||
big_image.paste(tile, (j * _TILESIZE, k * _TILESIZE))
|
||||
|
||||
west = self._pixels_to_lon(0, lon_pixels)
|
||||
east = self._pixels_to_lon(self.num_tiles - 1, lon_pixels)
|
||||
|
||||
north = self._pixels_to_lat(0, lat_pixels)
|
||||
south = self._pixels_to_lat(self.num_tiles - 1, lat_pixels)
|
||||
return big_image, (north, west), (south, east)
|
||||
|
||||
def move_pix(self, dx, dy):
|
||||
"""
|
||||
Function gets change in x and y (dx, dy)
|
||||
then displaces the displayed map that amount
|
||||
|
||||
NO RETURN
|
||||
"""
|
||||
self._constrain_x(dx)
|
||||
self._constrain_y(dy)
|
||||
self.update()
|
||||
|
||||
def _constrain_x(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.left_x - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[0] - self.width)):
|
||||
return self.left_x
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def _constrain_y(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.upper_y - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[1] - self.height)):
|
||||
return self.upper_y
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
Function remakes display image using top left corners
|
||||
"""
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
# self.display_image.resize((self.image_zoom, self.image_zoom))
|
||||
|
||||
def _fetch(self):
|
||||
"""
|
||||
Function generates big image
|
||||
"""
|
||||
self.big_image, self.northwest, self.southeast = self.fetch_tiles()
|
||||
|
||||
def move_latlon(self, lat, lon):
|
||||
"""
|
||||
Function to move the object/rover
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self._constrain_x(self.center_x-x)
|
||||
self._constrain_y(self.center_y-y)
|
||||
self.update()
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
bigimage_width = self.big_image.size[0]
|
||||
bigimage_height = self.big_image.size[1]
|
||||
|
||||
pixel_per_lat = bigimage_height / viewport_lat_diff
|
||||
pixel_per_lon = bigimage_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def add_gps_location(self, lat, lon, shape, size, fill):
|
||||
"""
|
||||
Function adds a shape at lat x lon
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
if shape is "ellipsis":
|
||||
draw.ellipsis((x-size, y-size, x+size, y+size), fill)
|
||||
else:
|
||||
draw.rectangle([x-size, y-size, x+size, y+size], fill)
|
||||
self.update()
|
||||
|
||||
def center_display(self, lat, lon):
|
||||
"""
|
||||
Function centers the display image
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
self.update()
|
||||
|
||||
# def update_rover_map_location(self, lat, lon):
|
||||
# print "I did nothing"
|
||||
|
||||
# def draw_circle(self, lat, lon, radius, fill):
|
||||
# print "I did nothing"
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
|
||||
class OverlayImage(object):
|
||||
def __init__(self, latitude, longitude, northwest, southeast,
|
||||
big_width, big_height, width, height):
|
||||
self.northwest = northwest
|
||||
self.southeast = southeast
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.big_width = big_width
|
||||
self.big_height = big_height
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.big_image = None
|
||||
self.display_image = None
|
||||
self.indicator = None
|
||||
self.helper = MapHelper.MapHelper()
|
||||
|
||||
x, y = self._get_cartesian(latitude, longitude)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
|
||||
self.generate_image_files()
|
||||
self.write_once = True
|
||||
|
||||
def generate_image_files(self):
|
||||
"""
|
||||
Creates big_image and display image sizes
|
||||
|
||||
Returns NONE
|
||||
"""
|
||||
self.big_image = self.helper.new_image(self.big_width, self.big_height,
|
||||
True)
|
||||
self.display_image = self.helper.new_image(self.width, self.height,
|
||||
True)
|
||||
self.generate_dot_and_hat()
|
||||
self.indicator.save("location.png")
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
pixel_per_lat = self.big_height / viewport_lat_diff
|
||||
pixel_per_lon = self.big_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def update_new_location(self, latitude, longitude,
|
||||
compass, navigation_list, landmark_list):
|
||||
size = 5
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
for element in navigation_list:
|
||||
x, y = self._get_cartesian(float(element[2]), float(element[1]))
|
||||
draw.ellipse((x-size, y-size, x+size, y+size), fill="red")
|
||||
# for element in landmark_list:
|
||||
# x, y = self._get_cartesian(element[1], element[2])
|
||||
# draw.ellipsis((x-size, y-size, x+size, y+size), fill="blue")
|
||||
self._draw_rover(longitude, latitude, compass)
|
||||
self.update()
|
||||
|
||||
return self.display_image
|
||||
|
||||
def generate_dot_and_hat(self):
|
||||
self.indicator = self.helper.new_image(100, 100, True)
|
||||
draw = PIL.ImageDraw.Draw(self.indicator)
|
||||
draw.ellipse((50-12, 50-12, 50+12, 50+12), fill="red")
|
||||
draw.line((25, 40, 50, 12), fill="red", width=7)
|
||||
draw.line((50, 12, 75, 40), fill="red", width=7)
|
||||
|
||||
def _draw_rover(self, lat, lon, angle=0):
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
# print x,y
|
||||
# Center of the circle on the indicator is (12.5, 37.5)
|
||||
x = x - 50
|
||||
y = y - 50
|
||||
rotated = self.indicator.copy()
|
||||
rotated = rotated.rotate(angle, expand=True)
|
||||
rotated.save("rotated.png")
|
||||
self.big_image.paste(rotated, (x, y), rotated)
|
||||
if self.write_once:
|
||||
self.display_image.save("Something.png")
|
||||
self.write_once = False
|
||||
|
||||
def update(self):
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
'''
|
||||
Mapping.py: Objected Orientated Google Maps for Python
|
||||
ReWritten by Chris Pham
|
||||
|
||||
Copyright OSURC, orginal code from GooMPy by Alec Singer and Simon D. Levy
|
||||
|
||||
This code is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU Lesser General Public License as
|
||||
published by the Free Software Foundation, either version 3 of the
|
||||
License, or (at your option) any later version.
|
||||
This code is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
You should have received a copy of the GNU Lesser General Public License
|
||||
along with this code. If not, see <http://www.gnu.org/licenses/>.
|
||||
'''
|
||||
|
||||
#####################################
|
||||
# Imports
|
||||
#####################################
|
||||
# Python native imports
|
||||
import math
|
||||
import urllib2
|
||||
from io import StringIO, BytesIO
|
||||
import os
|
||||
import time
|
||||
import PIL.ImageDraw
|
||||
import signing
|
||||
import RoverMapHelper as MapHelper
|
||||
|
||||
#####################################
|
||||
# Constants
|
||||
#####################################
|
||||
_KEYS = []
|
||||
# Number of pixels in half the earth's circumference at zoom = 21
|
||||
_EARTHPIX = 268435456
|
||||
# Number of decimal places for rounding coordinates
|
||||
_DEGREE_PRECISION = 4
|
||||
# Larget tile we can grab without paying
|
||||
_TILESIZE = 640
|
||||
# Fastest rate at which we can download tiles without paying
|
||||
_GRABRATE = 4
|
||||
# Pixel Radius of Earth for calculations
|
||||
_PIXRAD = _EARTHPIX / math.pi
|
||||
_DISPLAYPIX = _EARTHPIX / 2000
|
||||
|
||||
file_pointer = open('key', 'r')
|
||||
for i in file_pointer:
|
||||
_KEYS.append(i.rstrip())
|
||||
file_pointer.close()
|
||||
|
||||
|
||||
class GMapsStitcher(object):
|
||||
def __init__(self, width, height,
|
||||
latitude, longitude, zoom,
|
||||
maptype, radius_meters=None, num_tiles=4, debug=False):
|
||||
self.helper = MapHelper.MapHelper()
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.start_latitude = latitude
|
||||
self.start_longitude = longitude
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.zoom = zoom
|
||||
self.maptype = maptype
|
||||
self.radius_meters = radius_meters
|
||||
self.num_tiles = num_tiles
|
||||
self.display_image = self.helper.new_image(width, height)
|
||||
self.debug = debug
|
||||
|
||||
# Get the big image here
|
||||
self._fetch()
|
||||
self.center_display(latitude, longitude)
|
||||
|
||||
def __str__(self):
|
||||
"""
|
||||
This string returns when used in a print statement
|
||||
Useful for debugging and to print current state
|
||||
|
||||
returns STRING
|
||||
"""
|
||||
string_builder = ""
|
||||
string_builder += ("Center of the displayed map: %4f, %4f\n" %
|
||||
(self.center_x, self.center_y))
|
||||
string_builder += ("Center of the big map: %4fx%4f\n" %
|
||||
(self.start_longitude, self.start_longitude))
|
||||
string_builder += ("Current latitude is: %4f, %4f\n" %
|
||||
(self.longitude, self.latitude))
|
||||
string_builder += ("The top-left of the box: %dx%d\n" %
|
||||
(self.left_x, self.upper_y))
|
||||
string_builder += ("Number of tiles genreated: %dx%d\n" %
|
||||
(self.num_tiles, self.num_tiles))
|
||||
string_builder += "Map Type: %s\n" % (self.maptype)
|
||||
string_builder += "Zoom Level: %s\n" % (self.zoom)
|
||||
string_builder += ("Dimensions of Big Image: %dx%d\n" %
|
||||
(self.big_image.size[0], self.big_image.size[1]))
|
||||
string_builder += ("Dimensions of Displayed Image: %dx%d\n" %
|
||||
(self.width, self.height))
|
||||
string_builder += ("LatLong of Northwest Corner: %4f, %4f\n" %
|
||||
(self.northwest))
|
||||
string_builder += ("LatLong of Southeast Corner: %4f, %4f\n" %
|
||||
(self.southeast))
|
||||
return string_builder
|
||||
|
||||
def _grab_tile(self, longitude, latitude, sleeptime=0):
|
||||
"""
|
||||
This will return the tile at location longitude x latitude.
|
||||
Includes a sleep time to allow for free use if there is no API key
|
||||
|
||||
returns PIL.IMAGE OBJECT
|
||||
"""
|
||||
# Make the url string for polling
|
||||
# GET request header gets appended to the string
|
||||
urlbase = 'https://maps.googleapis.com/maps/api/staticmap?'
|
||||
urlbase += 'center=%.4f,%.4f&zoom=%d&maptype=%s'
|
||||
urlbase += '&size=%dx%d&format=png&key=%s'
|
||||
|
||||
# Fill the formatting
|
||||
specs = (self.helper.fast_round(latitude, _DEGREE_PRECISION),
|
||||
self.helper.fast_round(longitude, _DEGREE_PRECISION),
|
||||
self.zoom, self.maptype, _TILESIZE, _TILESIZE, _KEYS[0])
|
||||
filename = 'Resources/Maps/' + ('%.4f_%.4f_%d_%s_%d_%d_%s' % specs)
|
||||
filename += '.png'
|
||||
|
||||
# Tile Image object
|
||||
tile_object = None
|
||||
|
||||
if os.path.isfile(filename):
|
||||
tile_object = PIL.Image.open(filename)
|
||||
|
||||
# If file on filesystem
|
||||
else:
|
||||
# make the url
|
||||
url = urlbase % specs
|
||||
url = signing.sign_url(url, _KEYS[1])
|
||||
result = urllib2.urlopen(urllib2.Request(url)).read()
|
||||
tile_object = PIL.Image.open(BytesIO(result))
|
||||
if not os.path.exists('Resources/Maps'):
|
||||
os.mkdir('Resources/Maps')
|
||||
tile_object.save(filename)
|
||||
# Added to prevent timeouts on Google Servers
|
||||
time.sleep(sleeptime)
|
||||
|
||||
return tile_object
|
||||
|
||||
def _pixels_to_lon(self, iterator, lon_pixels):
|
||||
"""
|
||||
This converts pixels to degrees to be used in
|
||||
fetching squares and generate correct squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines, no idea
|
||||
degrees = self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles / 2) * _TILESIZE, self.zoom)
|
||||
return math.degrees((lon_pixels + degrees - _EARTHPIX) / _PIXRAD)
|
||||
|
||||
def _pixels_to_lat(self, iterator, lat_pixels):
|
||||
"""
|
||||
This converts pixels to latitude using meridian projection
|
||||
to get the latitude to generate squares
|
||||
|
||||
returns FLOAT(degrees)
|
||||
"""
|
||||
# Magic Lines
|
||||
return math.degrees(math.pi / 2 - 2 * math.atan(math.exp(((lat_pixels +
|
||||
self.helper.pixels_to_degrees(
|
||||
(iterator - self.num_tiles /
|
||||
2) * _TILESIZE, self.zoom)) -
|
||||
_EARTHPIX) / _PIXRAD)))
|
||||
|
||||
def fetch_tiles(self):
|
||||
"""
|
||||
Function that handles fetching of files from init'd variables
|
||||
|
||||
returns PIL.IMAGE OBJECT, (WEST, NORTH), (EAST, SOUTH)
|
||||
|
||||
North/East/South/West are in FLOAT(degrees)
|
||||
"""
|
||||
# cap floats to precision amount
|
||||
self.latitude = self.helper.fast_round(self.latitude,
|
||||
_DEGREE_PRECISION)
|
||||
self.longitude = self.helper.fast_round(self.longitude,
|
||||
_DEGREE_PRECISION)
|
||||
|
||||
# number of tiles required to go from center
|
||||
# latitude to desired radius in meters
|
||||
if self.radius_meters is not None:
|
||||
self.num_tiles = (int(
|
||||
round(2 * self.helper.pixels_to_meters(
|
||||
self.latitude, self.zoom) /
|
||||
(_TILESIZE / 2. / self.radius_meters))))
|
||||
|
||||
lon_pixels = _EARTHPIX + self.longitude * math.radians(_PIXRAD)
|
||||
|
||||
sin_lat = math.sin(math.radians(self.latitude))
|
||||
lat_pixels = _EARTHPIX - _PIXRAD * math.log((1+sin_lat)/(1-sin_lat))/2
|
||||
self.big_size = self.num_tiles * _TILESIZE
|
||||
big_image = self.helper.new_image(self.big_size, self.big_size)
|
||||
|
||||
for j in range(self.num_tiles):
|
||||
lon = self._pixels_to_lon(j, lon_pixels)
|
||||
for k in range(self.num_tiles):
|
||||
lat = self._pixels_to_lat(k, lat_pixels)
|
||||
tile = self._grab_tile(lon, lat)
|
||||
big_image.paste(tile, (j * _TILESIZE, k * _TILESIZE))
|
||||
|
||||
west = self._pixels_to_lon(0, lon_pixels)
|
||||
east = self._pixels_to_lon(self.num_tiles - 1, lon_pixels)
|
||||
|
||||
north = self._pixels_to_lat(0, lat_pixels)
|
||||
south = self._pixels_to_lat(self.num_tiles - 1, lat_pixels)
|
||||
return big_image, (north, west), (south, east)
|
||||
|
||||
def move_pix(self, dx, dy):
|
||||
"""
|
||||
Function gets change in x and y (dx, dy)
|
||||
then displaces the displayed map that amount
|
||||
|
||||
NO RETURN
|
||||
"""
|
||||
self._constrain_x(dx)
|
||||
self._constrain_y(dy)
|
||||
self.update()
|
||||
|
||||
def _constrain_x(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.left_x - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[0] - self.width)):
|
||||
return self.left_x
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def _constrain_y(self, diff):
|
||||
"""
|
||||
Helper for move_pix
|
||||
"""
|
||||
new_value = self.upper_y - diff
|
||||
|
||||
if ((not new_value > 0) and
|
||||
(new_value < self.big_image.size[1] - self.height)):
|
||||
return self.upper_y
|
||||
else:
|
||||
return new_value
|
||||
|
||||
def update(self):
|
||||
"""
|
||||
Function remakes display image using top left corners
|
||||
"""
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
# self.display_image.resize((self.image_zoom, self.image_zoom))
|
||||
|
||||
def _fetch(self):
|
||||
"""
|
||||
Function generates big image
|
||||
"""
|
||||
self.big_image, self.northwest, self.southeast = self.fetch_tiles()
|
||||
|
||||
def move_latlon(self, lat, lon):
|
||||
"""
|
||||
Function to move the object/rover
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self._constrain_x(self.center_x-x)
|
||||
self._constrain_y(self.center_y-y)
|
||||
self.update()
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
bigimage_width = self.big_image.size[0]
|
||||
bigimage_height = self.big_image.size[1]
|
||||
|
||||
pixel_per_lat = bigimage_height / viewport_lat_diff
|
||||
pixel_per_lon = bigimage_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def add_gps_location(self, lat, lon, shape, size, fill):
|
||||
"""
|
||||
Function adds a shape at lat x lon
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
if shape is "ellipsis":
|
||||
draw.ellipsis((x-size, y-size, x+size, y+size), fill)
|
||||
else:
|
||||
draw.rectangle([x-size, y-size, x+size, y+size], fill)
|
||||
self.update()
|
||||
|
||||
def center_display(self, lat, lon):
|
||||
"""
|
||||
Function centers the display image
|
||||
"""
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
self.update()
|
||||
|
||||
# def update_rover_map_location(self, lat, lon):
|
||||
# print "I did nothing"
|
||||
|
||||
# def draw_circle(self, lat, lon, radius, fill):
|
||||
# print "I did nothing"
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
|
||||
class OverlayImage(object):
|
||||
def __init__(self, latitude, longitude, northwest, southeast,
|
||||
big_width, big_height, width, height):
|
||||
self.northwest = northwest
|
||||
self.southeast = southeast
|
||||
self.latitude = latitude
|
||||
self.longitude = longitude
|
||||
self.big_width = big_width
|
||||
self.big_height = big_height
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.big_image = None
|
||||
self.display_image = None
|
||||
self.indicator = None
|
||||
self.helper = MapHelper.MapHelper()
|
||||
|
||||
x, y = self._get_cartesian(latitude, longitude)
|
||||
self.center_x = x
|
||||
self.center_y = y
|
||||
|
||||
self.left_x = (self.center_x - (self.width/2))
|
||||
self.upper_y = (self.center_y - (self.height/2))
|
||||
|
||||
self.generate_image_files()
|
||||
self.write_once = True
|
||||
|
||||
def generate_image_files(self):
|
||||
"""
|
||||
Creates big_image and display image sizes
|
||||
|
||||
Returns NONE
|
||||
"""
|
||||
self.big_image = self.helper.new_image(self.big_width, self.big_height,
|
||||
True)
|
||||
self.display_image = self.helper.new_image(self.width, self.height,
|
||||
True)
|
||||
self.generate_dot_and_hat()
|
||||
self.indicator.save("location.png")
|
||||
|
||||
def _get_cartesian(self, lat, lon):
|
||||
"""
|
||||
Helper for getting the x, y given lat and lon
|
||||
|
||||
returns INT, INT (x, y)
|
||||
"""
|
||||
viewport_lat_nw, viewport_lon_nw = self.northwest
|
||||
viewport_lat_se, viewport_lon_se = self.southeast
|
||||
# print "Lat:", viewport_lat_nw, viewport_lat_se
|
||||
# print "Lon:", viewport_lon_nw, viewport_lon_se
|
||||
|
||||
viewport_lat_diff = viewport_lat_nw - viewport_lat_se
|
||||
viewport_lon_diff = viewport_lon_se - viewport_lon_nw
|
||||
|
||||
# print viewport_lon_diff, viewport_lat_diff
|
||||
|
||||
pixel_per_lat = self.big_height / viewport_lat_diff
|
||||
pixel_per_lon = self.big_width / viewport_lon_diff
|
||||
# print "Pixel per:", pixel_per_lat, pixel_per_lon
|
||||
|
||||
new_lat_gps_range_percentage = (viewport_lat_nw - lat)
|
||||
new_lon_gps_range_percentage = (lon - viewport_lon_nw)
|
||||
# print lon, viewport_lon_se
|
||||
|
||||
x = new_lon_gps_range_percentage * pixel_per_lon
|
||||
y = new_lat_gps_range_percentage * pixel_per_lat
|
||||
|
||||
return int(x), int(y)
|
||||
|
||||
def update_new_location(self, latitude, longitude,
|
||||
compass, navigation_list, landmark_list):
|
||||
size = 5
|
||||
draw = PIL.ImageDraw.Draw(self.big_image)
|
||||
for element in navigation_list:
|
||||
x, y = self._get_cartesian(float(element[2]), float(element[1]))
|
||||
draw.ellipse((x-size, y-size, x+size, y+size), fill="red")
|
||||
# for element in landmark_list:
|
||||
# x, y = self._get_cartesian(element[1], element[2])
|
||||
# draw.ellipsis((x-size, y-size, x+size, y+size), fill="blue")
|
||||
self.draw_rover(longitude, latitude, compass)
|
||||
self.update()
|
||||
|
||||
return self.display_image
|
||||
|
||||
def generate_dot_and_hat(self):
|
||||
self.indicator = self.helper.new_image(100, 100, True)
|
||||
draw = PIL.ImageDraw.Draw(self.indicator)
|
||||
draw.ellipse((50-12, 50-12, 50+12, 50+12), fill="red")
|
||||
draw.line((25, 40, 50, 12), fill="red", width=7)
|
||||
draw.line((50, 12, 75, 40), fill="red", width=7)
|
||||
|
||||
def draw_rover(self, lat, lon, angle=0):
|
||||
x, y = self._get_cartesian(lat, lon)
|
||||
# print x,y
|
||||
# Center of the circle on the indicator is (12.5, 37.5)
|
||||
x = x - 50
|
||||
y = y - 50
|
||||
rotated = self.indicator.copy()
|
||||
rotated = rotated.rotate(angle, expand=True)
|
||||
rotated.save("rotated.png")
|
||||
self.big_image.paste(rotated, (x, y), rotated)
|
||||
if self.write_once:
|
||||
self.display_image.save("Something.png")
|
||||
self.write_once = False
|
||||
|
||||
def update(self):
|
||||
self.display_image.paste(self.big_image, (-self.left_x, -self.upper_y))
|
||||
|
||||
def connect_signals_and_slots(self):
|
||||
pass
|
||||
|
||||
|
||||
@@ -18,6 +18,7 @@ import RoverMap
|
||||
#####################################
|
||||
# put some stuff here later so you can remember
|
||||
|
||||
GPS_TOPIC_NAME = "/rover_status/gps_status"
|
||||
|
||||
class RoverMapCoordinator(QtCore.QThread):
|
||||
pixmap_ready_signal = QtCore.pyqtSignal()
|
||||
@@ -33,6 +34,7 @@ class RoverMapCoordinator(QtCore.QThread):
|
||||
self.navigation_label = self.left_screen.navigation_waypoints_table_widget
|
||||
self.landmark_label = self.left_screen.landmark_waypoints_table_widget
|
||||
|
||||
self.gps_status = rospy.Subscriber(GPS_TOPIC_NAME, GPSInfo, self.edit_rover_location)
|
||||
|
||||
self.setings = QtCore.QSettings()
|
||||
|
||||
@@ -142,3 +144,9 @@ class RoverMapCoordinator(QtCore.QThread):
|
||||
navigation_list,
|
||||
landmark_list)
|
||||
# self.overlay_image.save("something.png")
|
||||
|
||||
def edit_rover_location(self, data):
|
||||
#Need to parse data for lat, long, and angle
|
||||
if data.GPS_connection_status:
|
||||
self.overlay_image_object.draw_rover(data.something, data.something, data.gps_heading)
|
||||
self.overlay_image_object.update()
|
||||
|
||||
Reference in New Issue
Block a user