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
