def pickFrontier(self, frontierList, heuristic, locationTouple):
"""
:param frontierList: List of Frontiers , list of list of <(x,y) touple > in grid cell location on the map
:param heuristic: function that only requires a single frontier to make a decision.
:return:an (x,y) touple of the point to go to in grid cell location on the map
"""
cx, cy = locationTouple
targetFrontier = frontierList[0]
print "Trying to find the best frontier"
if len(frontierList) == 0: # If there are no frontiers
raise Exception("Passed in an empty frontier list!")
# Select the largest frontier
elif len(frontierList) != 1:
for elt in frontierList: # Assuming a larger Heuristic is better,
if heuristic(targetFrontier) < heuristic(
elt
): # Check if the next frontier is a better candidate for travel
targetFrontier = elt
print "Best frontier found, contains ", len(targetFrontier), " nodes"
# Find the closest point on the frontier
currentTarget = targetFrontier[0]
for elt in targetFrontier:
if tools.distFormula(elt, (cx, cy)) < tools.distFormula(currentTarget, (cx, cy)):
currentTarget = elt # Update which is the closest element in the target frontier.
print "Closest point found at: ", currentTarget
return currentTarget
def _aStarSearch(self, givenMap, start, goal, threshold=70):
"""
This function generates a dictionary of paths where the shortest path can be found through
traversing from the goal back to the start.
This version of A* uses tools.distFormula as the heuristic. Could be changed out.
:param start: (x,y) touple of the location in the grid.
:return: Dictionary of <end point> : <starting Point> as key,value
"""
frontier = Queue.PriorityQueue()
frontier.put((0, start)) # Put takes a touple of priority, value
goalX, goalY = goal
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
found_goal = False
while not frontier.empty():
current_touple = frontier.get() # Returns the (priority, (x,y)
_, current = current_touple # sets current to (x,y)
x, y = current
if x == goalX and y == goalY:
found_goal = True
break # We're there!
# unpack current
neighbors = tools.getNeighbors(x, y, givenMap, threshold=threshold)
# print neighbors
for next in neighbors: # Get list of touples (points)
# print "AStar",next,current
cost_next = cost_so_far[current] + tools.distFormula(current, next)
if next not in cost_so_far or cost_so_far[next] > cost_next:
if next not in cost_so_far:
frontier.put(
(cost_next + tools.distFormula(next, goal), next)
) # Put takes a touple of priority, value
nx, ny = next
cost_so_far[next] = cost_next
came_from[next] = current
if found_goal:
return came_from
else:
raise PathPlannerException("The path is not travelable")
