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