def findSafePoint(self, point, givenMap):
x, y = point
neighborList = tools.getNeighbors(x, y, givenMap, 102)
for elt in neighborList:
# print "I'm here man... "
tx, ty = elt
if givenMap[ty][tx] == 100 or givenMap[ty][tx] == -1:
nn = tools.getNeighbors(tx, ty, givenMap, 102)
for i in nn:
if not (i in neighborList):
neighborList.append(i)
else:
return tx, ty
raise FrontierException("Explored the whole map and found no safe spots. Get rekt")
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")
def getNeighborsFrontier(self, x, y, givenMap):
"""
Returns all the neighbors of a given node that are frontier nodes
:param x:
:param y:
:return:
"""
goodNeighbors = tools.getNeighbors(x, y, givenMap)
edgeNeighbors = []
for n in goodNeighbors:
nodeX, nodeY = n
if self.isNodeFrontier(nodeX, nodeY, givenMap):
edgeNeighbors.append(n)
return edgeNeighbors
def isNodeFrontier(self, x, y, givenMap):
"""testSquares
Returns true if the given coordinate specific a node that:
Is known reachable space
Is adjacent to unknown space
:param x:
:param y:
:return:
"""
if -1 < givenMap[y][x] < 50:
neighbors = tools.getNeighbors(x, y, givenMap)
for n in neighbors:
x, y = n
if givenMap[y][x] == -1:
return True
return False
