class Astar:
def __init__(self, control):
self.control = control
self.gps = Algorithms(self.control)
self.opened = []
heapq.heapify(self.opened)
self.closed = set()
self.cells = []
self.walls = []
self.grid_height = 9 # size of the y-axis
self.grid_width = 9 # size of the x-axis
def create_walls(self):
"""
walls = ((0, 5), (1, 0), (1, 1), (1, 5), (2, 3), # This is a test array should be filled from sensor data
(3, 1), (3, 2), (3, 5), (4, 1), (4, 4), (5, 1))
this should be filled with database requests for all information about the grid area.
then any obstacles should be added the the walls array
"""
self.walls = [
Wall(0, 5, 5),
Wall(1, 0, 5),
Wall(1, 1, 5),
Wall(1, 5, 5),
Wall(2, 3, 5),
Wall(3, 1, 5),
Wall(3, 2, 5),
Wall(3, 5, 5),
Wall(4, 1, 5),
Wall(4, 4, 5),
Wall(5, 1, 5),
]
return self.walls
# need to give the grids there lat and long positions.
def init_grid(self, startlocation, endlocation):
walls = self.create_walls()
# mkae a variable so it relates to the size of the
bottom_left_location_lat = math.floor(self.grid_height / 2) * 0.00001
bottom_left_location_lon = math.floor(self.grid_width / 2) * 0.00001
lat = startlocation[0] - bottom_left_location_lat # one more than the actual distance as it gets inc'ed first
lon = startlocation[1] - bottom_left_location_lon # one more than the actual distance as it gets inc'ed first
for x in range(self.grid_width):
lon += 0.00001
for y in range(self.grid_height):
wall = self.find_wall(x, y)
if isinstance(wall, Wall):
reachable = wall.accessibility
else:
reachable = 1
lat += 0.00001
self.cells.append(Cell(x, y, reachable, lat, lon))
self.start = self.get_cell(startlocation[0], startlocation[1])
self.end = self.get_cell(endlocation[0], endlocation[1])
def get_heuristic(self, cell):
"""
compute the heuristic value H for a cell
distance between this cell anf the end cell multiplied by 10
:param cell:
:return heuristic value:
"""
return 10 * (abs(cell.x - self.end.x) + abs(cell.y - self.end.y))
def find_wall(self, x, y):
for wall in self.walls:
if wall.x == x and wall.y == y:
return wall
return False
def get_cell(self, x, y):
"""
returns a cell by using the x and y coordinates to find it in the array cells
:param x:
:param y:
:return:
"""
return self.cells[x * self.grid_height + y]
def get_adjacent_cells(self, cell):
"""
Returns adjacent cells to a cell. Clockwise starting
from the one on the right.
@param cell get adjacent cells for this cell
@returns adjacent cells list
"""
cells = []
if cell.x < self.grid_width - 1:
cells.append(self.get_cell(cell.x + 1, cell.y))
if cell.y > 0:
cells.append(self.get_cell(cell.x, cell.y - 1))
if cell.x > 0:
cells.append(self.get_cell(cell.x - 1, cell.y))
if cell.y < self.grid_height - 1:
cells.append(self.get_cell(cell.x, cell.y + 1))
return cells
def display_path(self):
cell = self.end
while cell.parent is not self.start:
cell = cell.parent
print "path: cell: %d,%d" % (cell.x, cell.y)
def update_cell(self, adj, cell):
"""
Update adjacent cell
@param adj adjacent cell to current cell
@param cell current cell being processed
"""
adj.g = (cell.g + 10) + (cell.reachable * 10) # better but needs to be tested
adj.h = self.get_heuristic(adj)
adj.parent = cell
adj.f = adj.h + adj.g
def get_path(self):
cell = self.end
path = []
while cell.parent is not self.start:
path.append(cell)
cell = cell.parent
print "path: cell: %d, %d, %d. %d" % (cell.x, cell.y, cell.lat, cell.lon)
return path
def solve(self):
"""Solve maze, find path to ending cell.
@returns path or None if not found.
"""
# add starting cell to open heap queue
heapq.heappush(self.opened, (self.start.f, self.start))
while len(self.opened):
# pop cell from heap queue
f, cell = heapq.heappop(self.opened)
# add cell to closed list so we don't process it twice
self.closed.add(cell)
# if ending cell, return found path
if cell is self.end:
return self.get_path()
# get adjacent cells for cell
adj_cells = self.get_adjacent_cells(cell)
for adj_cell in adj_cells:
if adj_cell.reachable < 4 and adj_cell not in self.closed:
if (adj_cell.f, adj_cell) in self.opened:
# if adj cell in open list, check if current path is
# better than the one previously found
# for this adj cell.
if adj_cell.g > cell.g + 10:
self.update_cell(adj_cell, cell)
else:
self.update_cell(adj_cell, cell)
# add adj cell to open list
heapq.heappush(self.opened, (adj_cell.f, adj_cell))
def get_grid_data(self, destination):
curlocation = self.gps.getLocation()
distance = self.gps.getDistance(curlocation, destination)
bearing = self.gps.getHeading(curlocation, destination)
grid_height = abs(self.grid_height)
grid_width = abs(self.grid_width)
calc_width = 0
calc_height = 0
centre_width = math.floor(self.grid_width / 2)
centre_height = math.floor(self.grid_height / 2)
startlocation = [centre_width, centre_height]
if distance < centre_height:
if 0 < bearing < 180:
if bearing < 90:
calc_width = centre_width + math.floor(distance * math.sin(bearing))
calc_height = centre_height + math.floor(distance * math.cos(bearing))
else:
calc_width = centre_width + math.floor(distance * math.sin(180 - bearing))
calc_height = centre_height - math.floor(distance * math.cos(180 - bearing))
else:
if bearing > -90:
calc_width = centre_width - math.floor(distance * math.cos(math.fabs(bearing)))
calc_height = centre_height + math.floor(distance * math.cos(math.fabs(bearing)))
else:
calc_width = centre_width - math.floor(distance * math.cos(math.fabs((-180) - bearing)))
calc_height = centre_height - math.floor(distance * math.cos(math.fabs((-180) - bearing)))
else:
if 0 < bearing < 180:
if 0 < bearing < 90:
if bearing < 45:
calc_width = centre_width + math.floor(centre_height * math.tan(math.fabs(bearing)))
calc_height = grid_height
else:
calc_width = grid_width
calc_height = centre_height + math.floor(centre_width / math.tan(math.fabs(bearing)))
else:
if bearing < 135:
calc_width = grid_width
calc_height = centre_height - math.floor(centre_width * math.tan(math.fabs(bearing - 90)))
else:
calc_width = centre_width + math.floor(centre_height / math.tan(math.fabs(bearing - 90)))
calc_height = 0
else:
if -90 < bearing < 0:
if bearing > -45:
calc_width = centre_width - math.floor(centre_height * math.tan(math.fabs(bearing)))
calc_height = grid_height
else:
calc_width = 0
calc_height = centre_height + math.floor(centre_width / math.tan(math.fabs(bearing)))
else:
if bearing > -135:
calc_width = 0
calc_height = centre_height - math.floor(centre_width * math.tan(math.fabs(bearing + 90)))
else:
calc_width = centre_width - math.floor(centre_height / math.tan(math.fabs(bearing + 90)))
calc_height = 0
endlocation = [calc_width, calc_height]
return [curlocation, distance, bearing, grid_width, grid_height, startlocation, endlocation]
@database_grid_update
def run_astar(self, destination):
# 0 - curlocation, 1 - distance, 2 - bearing, 3 - grid_width,
# 4 - grid_height, 5 - startlocation, 6 - endlocation
destination = format(destination, ".5f")
grid_values = self.get_grid_data(destination)
curlocation = grid_values[0]
distance = grid_values[1]
bearing = grid_values[2]
self.grid_width = grid_values[3]
self.grid_height = grid_values[4]
startlocation = grid_values[5]
endlocation = grid_values[6]
startlocation = format(startlocation, ".5f")
endlocation = format(endlocation, ".5f")
# this is where data is read from the database to fill the grid with non reachable locations
self.create_walls()
# build the grid using including the walls created by the database data
self.init_grid(startlocation, endlocation)
# find a path through the current grid
self.solve()
path = self.get_path()
points = []
# build points
for i in range(0, len(path), 1):
points.append([path[i].lat, path[i].lon])
self.gps.followPath(points, 50)
# get the new current location after following the path
curlocation = format(self.gps.getLocation(), ".5f")
# check if the destination has been reached.
if curlocation != destination:
print "Current location is : {0}.\nRe-running A-Star to get to Destination : {1}".format(
str(curlocation), str(destination)
)
self.run_astar(destination)
print "A-Star complete, with current location : {0}.\nThe given destination was : {1}".format(
str(curlocation), str(destination)
)
return
latitude.append(sys.argv[i])
longitude.append(sys.argv[i + 1])
points.append([latitude[j], longitude[j]])
j += 1
speedpercent = int(sys.argv[sys.argv.__len__() - 1])
print 'Speed: ' + str(speedpercent)
print 'lat: ' + str(latitude)
print 'long: ' + str(longitude)
print 'Points: ' + str(points)
if algo is '0':
g.pointToPoint([latitude[0], longitude[0]], speedpercent)
elif algo is '1':
g.followPath(points, speedpercent)
except Exception as e:
print e
'''
print 'Well that didn\'t quite go to plan!!!'
print 'Did you run this script correctly?'
print 'Hmmm maybe I can help you out with that, being a supreme being and all!'
print 'Try using this format:'
print 'python test_gps_algorithms.py algo lat1 long1 lat2 long2 ... check speedpercent'
print '\n'
print 'The algo argument is used to select which algorithm is being tested:'
print '0 - pointToPoint, 1 - followPath'
print 'The number of lat and long values is up to you but they must have the same number of values and'
print 'If you select pointToPoint then the code will only consider the first values'
print 'The check argument is the number of meters between each check on if tiberius is going to the right place'
print 'And finally speedpercent is the speed percentage of tiberius, hint its a percentage so between 0 and 100 :)'