class PathFinder(object): ''' PathFinder will search the map for a valid path to the goal. Args: Map = ROS map file to be traversed start = Initial location goal = Desired location Returns: Path = An array of coordinates representing a path as a series of waypoints. Resolution is, as of yet, unspecified. ''' map = None closed = None open = None start = None goal = None def __init__(start, goal, resolution, map): self.map = map self.start = start self.goal = goal self.closed = GridCells() self.closed.cell_height = resolution self.closed.cell_width = resolution self.closed.header.frame_id = "map" self.open = GridCells() self.open.cell_height = resolution self.open.cell_width = resolution self.open.header.frame_id = "map" def astar(self): closedset = [] #nodes already evaluated openset = [self.start] #nodes to be evaluated - contains start came_from = self.map #navigated nodes start.gscore = 0 start.heuristic = calcHeur(self.start, self.goal) + start.gscore lastNode = start while len(self.openset) > 0: distance = 100000 for node in self.open: if node.fscore < distance: current = node distance = node.fscore if node.isSame(self.goal): distance = 0 current = node if current.isSame(self.goal): return make_path(current) self.open.remove(current) self.closed.append(current) currentDirection = direction(current, last) upPoint = Point(current.x, current.y+resolution, 0) leftPoint = Point(current.x - resolution, current.y,0) rightPoint = Point(current.x+resolution, current.y, 0) downPoint = Point(current.x, current.y-resolution, 0) upNode = Node(upPoint, current.score + resolution, current) leftNode = Node(leftPoint, current.score + resolution, current) rightNode = Node(rightPoint, current.score + resolution, current) downNode = Node(downPoint, current.score + resolution, current) if currentDirection is Direction.up: upNode.heuristic -= 2*resolution elif currentDirection is Direction.right: rightNode.heuristic -= 2*resolution elif currentDirection is Direction.left: leftNode.heuristic -= 2*resolution elif currentDirection is Direction.down: downNode.heuristic -= 2*resolution possibleneighbors = [upNode, rightNode, leftNode, downNode] validneighbors = [] #remove neighbors that aren't in c-space for neighbor in possibleneighbors: for tiles in botWorld: if neighbor.isSame(item): validneighbors.append(neighbor) #remove neighbors that are already in the closedset for neighbor in validneighbors: for closedTiles in closedset: if neighbor.isSame(item): validneighbors.remove(neighbor) for neighbor in validneighbors: gscore = current.score + resolution fscore = gscore + neighbor.heuristic for object in openset: if neighbor.isSame(object): if neighbor.gscore < object.gscore: openset.remove(object) else: validneighbors.remove(neighbor) openset = validneighbors + openset closed = [] for node in closedset: closed.append(node.point) ClosedGC.cells = closed open = [] for node in openset: open.append(node.point) OpenGC.cells = open openPub.publish(OpenGC) closedPub.publish(ClosedGC) last = current def direction(current, last): ''' returns the last dirstion of motion for travel. Used to determine the value of the heuristic ''' diffx = current.x-last.x diffy = current.y-last.y if diffy > 0: return Direction.up if diffy < 0: return Direction.down if diffx > 0: return Direction.right if diffx < 0: return Direction.left else: return Direction.invalid