caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-09 02:31:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

change self only function in Overlay Class, and start on rover gps location

Browse Source
This commit is contained in:
Christopher Pham
2018-05-03 16:30:24 -07:00
parent 7ff1dd8d66
commit e4318a9242
2 changed files with 456 additions and 448 deletions
Download Patch File Download Diff File Expand all files Collapse all files

896
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMap.py
View File

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

8
software/ros_packages/ground_station/src/Framework/MapSystems/RoverMapCoordinator.py
View File

@@ -18,6 +18,7 @@ import RoverMap
##################################### #####################################
# put some stuff here later so you can remember # put some stuff here later so you can remember
GPS_TOPIC_NAME = "/rover_status/gps_status"
class RoverMapCoordinator(QtCore.QThread): class RoverMapCoordinator(QtCore.QThread):
pixmap_ready_signal = QtCore.pyqtSignal() pixmap_ready_signal = QtCore.pyqtSignal()
@@ -33,6 +34,7 @@ class RoverMapCoordinator(QtCore.QThread):
self.navigation_label = self.left_screen.navigation_waypoints_table_widget self.navigation_label = self.left_screen.navigation_waypoints_table_widget
self.landmark_label = self.left_screen.landmark_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() self.setings = QtCore.QSettings()
@@ -142,3 +144,9 @@ class RoverMapCoordinator(QtCore.QThread):
navigation_list, navigation_list,
landmark_list) landmark_list)
# self.overlay_image.save("something.png") # 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()