def get_char_freq(file_name):
"""
gets the sorted list of character frequencies
:param file_name: the file to parse through
:type file_name: str
:return: the huffman tree and char count
"""
char_frequency = {}
with open(file_name) as to_encode:
cur_char = to_encode.read(1)
while cur_char:
if char_frequency.has_key(cur_char):
char_frequency[cur_char] += 1
else:
char_frequency[cur_char] = 1
cur_char = to_encode.read(1)
# create the huffman list of nodes sorted by count
nodes = PriorityQueue()
# adds the initial nodes to the list
for node_char, node_freq in char_frequency.iteritems():
nodes.put(h_node(None, None, node_freq, node_char), node_freq)
# create the tree of nodes required to get the character codes
while nodes.count() > 1:
# get the two smallest nodes
h_node_a = nodes.get()
h_node_b = nodes.get()
freq_sum = h_node_a.frequency + h_node_b.frequency
nodes.put(h_node(h_node_a, h_node_b, freq_sum, None), freq_sum)
return nodes.get()
def search(self, start, end):
frontier = PriorityQueue()
frontier.put(0, start)
cost_so_far = dict()
cost_so_far[start] = 0
came_from = dict()
came_from[start] = None
while not frontier.is_empty():
pri, current = frontier.get()
if current == end:
self.paths[start].update(self.reconstruct_path(end, came_from))
break
neighbors = [
neighbor.end for neighbor in self.graph.neighbors(current)
]
for neighbor in neighbors:
new_cost = cost_so_far[current] + self.graph.cost(
current, neighbor)
if neighbor not in cost_so_far.keys(
) or new_cost < cost_so_far[neighbor]:
cost_so_far[neighbor] = new_cost
came_from[neighbor] = current
frontier.put(new_cost, neighbor)
def search(self, initial_state_data, manhattan_distance, max_nodes=0):
assert isinstance(initial_state_data, Search.NodeStateData)
assert isinstance(manhattan_distance, bool)
heuristic = "h2cost" if manhattan_distance else "h1cost"
current_node = self.__create_node(initial_state_data, heuristic)
frontier = PriorityQueue() # nodes we're looking at now
frontier.push(current_node, current_node.search_data.fcost)
explored = set() # nodes we've looked at and needn't again
while True:
if frontier.empty() or (0 < max_nodes <=
len(frontier) + len(explored)): # failure
return None
current_node = frontier.pop()
# print(current_node.state_data.get_tiles())
if current_node.state_data.goal_test: # success
return Search.build_solution(current_node.state_data)
if current_node not in explored: # mark the current node as explored if not already
explored.add(current_node)
# print("moved ", str(current_node.state_data.last_move), " to " + str(current_node.state_data.parent),
# str(current_node.search_data.fcost))
# expand unexplored neighbors, updating their path cost if a better one is found
for prioritized_neighbor_node in self.__prioritize_neighbors(
current_node, heuristic):
neighbor_node = prioritized_neighbor_node[1]
if (neighbor_node not in explored) and (
not frontier.contains(neighbor_node)):
frontier.push(neighbor_node, prioritized_neighbor_node[0])
elif frontier.contains(neighbor_node) and (
frontier.get(neighbor_node).search_data.fcost >
neighbor_node.search_data.fcost):
frontier.replace(neighbor_node)
def a_star_search(self, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = start
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current.position_x == goal.position_x and current.position_y == goal.position_y:
break
for next in self.neighbors(current):
next_cell = Cell(next[0], next[1])
if next_cell.position_x == goal.position_x and next_cell.position_y == goal.position_y:
next_cell = goal
new_cost = cost_so_far[current] + self.heuristic(current, next_cell)
if next_cell not in cost_so_far or new_cost < cost_so_far[next_cell]:
cost_so_far[next_cell] = new_cost
priority = new_cost + self.heuristic(goal, next_cell)
frontier.put(next_cell, priority)
came_from[next_cell] = current
return came_from # , cost_so_far
def dijkstra_search(graph, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
# different from first implementation in that it remembers
# WHERE we came from, not just that we visited it
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
# from front of the queue
current = frontier.get()
# print("Visiting ", current)
# main difference from implementation 2
if current == goal:
break
# get neighbors of current node
# add it to back of frontier queue if not yet visited
for next in graph.neighbors(current):
new_cost = cost_so_far[current] + graph.cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost
frontier.put(next, priority)
came_from[next] = current
return came_from, cost_so_far
def search(self, startHex, goalHex):
startNode = HexNode(startHex, None)
goalNode = HexNode(goalHex, None)
frontier = PriorityQueue()
frontier.put(startNode, 0)
cameFrom = {}
currCost = {}
cameFrom[startNode] = None
currCost[startNode] = 0
while not frontier.empty():
currNode = frontier.get()
if currNode.h == goalNode.h:
break
for nextNode in self.getNeighborNodes(currNode.h):
newCost = currCost[currNode] + self.cost(currNode, nextNode)
if nextNode not in currCost or newCost < currCost[nextNode]:
currCost[nextNode] = newCost
priority = newCost + HexMap.heuristic(goalNode, nextNode)
frontier.put(nextNode, priority)
cameFrom[nextNode] = currNode
return cameFrom, currCost
def dijkstra(start, C):
openSet = PriorityQueue()
openSet_check = {}
closedSet = {}
g = {}
for i in range(len(start)):
g[start[i]] = 0
for i in range(len(start)):
openSet.put(start[i], 0)
openSet_check[start[i]] = 1
while not openSet.empty():
currentState = openSet.get()
closedSet[currentState] = 1
for nextState in get_successors(currentState):
if nextState in closedSet:
continue
newCost = g[currentState] + get_cost(C, nextState)
if nextState not in openSet_check:
g[nextState] = newCost
openSet.put(nextState, g[nextState])
openSet_check[nextState] = 1
elif newCost >= g[nextState]:
continue
else:
g[nextState] = newCost
openSet.update(nextState, g[nextState])
openSet_check[nextState] = 1
temp = ''
#For printing the H as a grid:
'''for i in range(N):
for j in range(N):
if j==0:
temp +=str(g[(i,j)])
else:
temp +=','+str(g[(i,j)])
temp += "\n"
print temp'''
#For writing the H onto a file
f = open('heuristic.txt', 'w')
temp = ''
for i in range(N):
temp = ''
for j in range(N):
if j == 0:
temp += str(g[(i, j)])
else:
temp += ',' + str(g[(i, j)])
if i == 0:
f.write(temp)
else:
f.write('\n' + temp)
f.close()
return False
def a_star_search(self, start, goal):
self.graph2D.reset()
#Push start into frontier
frontier = PriorityQueue()
frontier.put(start, 0)
# Create a dictionary that contains previous position of current position
parent = {}
# Create a dictionary that contains cost_so_far[current_position] = sum up cost from start to current_position
cost_so_far = {}
# Initilization dictionary
parent[start] = None
parent[goal] = None
cost_so_far[start] = 0
while not frontier.empty():
# Get position from front positions (previous position)
current = frontier.get()
# If you are standing at goal to stop
if current == goal:
break
# Get adjacent vertices
for next in self.graph2D.get_neighbors(current):
#Update new_cost
new_cost = cost_so_far[current] + 1
# Checking is in cost_so_far or new_cost is smaller than old_cost
if next not in cost_so_far or new_cost < cost_so_far[next]:
#Update new cost
cost_so_far[next] = new_cost
priority = new_cost + self.heuristic(goal, next)
#Update frontier and parent
frontier.put(next, priority)
parent[next] = current
return self.backtrace(parent, start, goal)
def a_star_search(graph, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current == goal:
break
for next in graph.neighbors(current):
new_cost = cost_so_far[current] + graph.cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + heuristic(
goal, next
) # reduce priority for positions further away from goal
frontier.put(next, priority)
came_from[next] = current
return came_from, cost_so_far
def a_star_search(self, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = start
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current.position_x == goal.position_x and current.position_y == goal.position_y:
break
for next in self.neighbors(current):
next_cell = Cell(next[0], next[1])
if next_cell.position_x == goal.position_x and next_cell.position_y == goal.position_y:
next_cell = goal
new_cost = cost_so_far[current] + self.heuristic(
current, next_cell)
if next_cell not in cost_so_far or new_cost < cost_so_far[
next_cell]:
cost_so_far[next_cell] = new_cost
priority = new_cost + self.heuristic(goal, next_cell)
frontier.put(next_cell, priority)
came_from[next_cell] = current
return came_from # , cost_so_far
def a_star(self):
# Genera camino usando el algoritmo A*
frontier = PriorityQueue()
frontier.put(self.start, 0)
came_from = {}
cost_so_far = {}
came_from[self.start] = None
cost_so_far[self.start] = 0
# comienza la busqueda
while not frontier.empty():
current = frontier.get()
# termina si llega a la meta
if current == self.goal:
break
# busca en sus vecinos
for next in self.graph.neighbors(current):
new_cost = cost_so_far[current] + self.graph.cost(
current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self.cost(self.posiciones[self.goal],
self.posiciones[next])
frontier.put(next, priority)
came_from[next] = current
# ordena el plan
current = self.goal
path = [current]
while current != self.start:
current = came_from[current]
path.append(current)
# invierte el camino para empezar al inicio
path.reverse()
self.plan = path
def a_star_search(self, start, goal):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start.get_key()] = None
cost_so_far[start.get_key()] = 0
while not frontier.empty():
current = frontier.get()
if current == goal:
break
for point in self.graph.neighbors(current):
move_cost = self.graph.cost(current, point)
new_cost = cost_so_far[current.get_key()] + move_cost
if point is None:
continue
if point.get_key(
) not in cost_so_far or new_cost < cost_so_far[
point.get_key()]:
cost_so_far[point.get_key()] = new_cost
priority = new_cost + self.heuristic(goal, point)
frontier.put(point, priority)
came_from[point.get_key()] = current
#return came_from, cost_so_far
return came_from
def astar(maze, start, goal):
pq = PriorityQueue()
pq.put(start, 0)
predecessor = {start: None}
g_value = {start: 0}
while not pq.is_empty():
current_cell = pq.get()
if current_cell == goal:
return get_path(predecessor, start, goal)
for directions in ["up", "right", "down", "left"]:
row_offset, col_offset = offsets[directions]
neighbor = (current_cell[0] + row_offset,
current_cell[1] + col_offset)
if is_legal_pos(maze, neighbor) and neighbor not in g_value:
g_value[neighbor] = g_value[current_cell] + 1
f_value = g_value[neighbor] + heuristic(neighbor, goal)
pq.put(neighbor, f_value)
predecessor[neighbor] = current_cell
return None
def Astar(start, goal, C, im):
"""Input: Start state, Goal State, Cost map and original map
Output: Bool value False after completion of the algorithm
This function performs the A-Star algorithm to find the shortest path to the goal
"""
openSet = PriorityQueue()
openSet.put(start, 0)
openSet_check = {}
openSet_check[start] = 1
closedSet = {}
plan = {}
g = {}
g[start] = 0
f = {}
j = 0
f[start] = get_heuristic(start, goal)
p = 0
while not openSet.empty():
currentState, fpop = openSet.get()
if currentState == goal:
print "Path found!"
path = reconstruct_path(plan, currentState)
path.reverse()
get_totalCost(path, C)
visualize(path, C, im)
#for i in range(len(path)):
# print path[i]
break
closedSet[currentState] = 1
for nextState in get_successors(currentState):
j = j + 1
if nextState in closedSet:
continue
newCost = g[currentState] + get_cost(C, nextState)
if nextState not in openSet_check:
plan[nextState] = currentState
g[nextState] = newCost
f[nextState] = g[(nextState)] + get_heuristic(
currentState, nextState)
openSet.put(nextState, f[nextState])
openSet_check[nextState] = 1
continue
elif newCost >= g[nextState]:
openSet_check[nextState] = 1
continue
plan[nextState] = currentState
g[nextState] = newCost
f[nextState] = g[(nextState)] + get_heuristic(
currentState, nextState)
openSet.put(nextState, f[nextState])
openSet_check[nextState] = 1
return False
def algorithm_Prim(self, start_position):
priority_queue = PriorityQueue(self.size)
self.visited = [False] * (self.size + 1)
self.visited[start_position] = True
self.insert_to_priority_queue_from(start_position, priority_queue)
while self.visited.count(True) < len(self.nodes):
target = priority_queue.get()
self.visited_history.append((target[1], target[0]))
position = target[0]
self.visited[position] = True
self.insert_to_priority_queue_from(position, priority_queue)
self.__show_graph__()
def Astar(start, goal, C):
openSet = PriorityQueue()
openSet.put(start, 0)
openSet_check = {}
openSet_check[start] = 1
closedSet = {}
plan = {}
g = {}
g[start] = 0
f = {}
j = 0
f[start] = get_heuristic(start)
p = 0
while not openSet.empty():
currentState, fpop = openSet.get()
if currentState in goal:
path = reconstruct_path(plan, currentState)
path.reverse()
get_totalCost(path, C)
visualize(path, C)
for i in range(len(path)):
print path[i]
break
closedSet[currentState] = 1
for nextState in get_successors(currentState):
j = j + 1
if nextState in closedSet:
continue
newCost = g[currentState] + get_cost(C, nextState)
if nextState not in openSet_check:
plan[nextState] = currentState
g[nextState] = newCost
f[nextState] = g[(nextState)] + get_heuristic(nextState)
openSet.put(nextState, f[nextState])
openSet_check[nextState] = 1
continue
elif newCost >= g[nextState]:
openSet_check[nextState] = 1
continue
plan[nextState] = currentState
g[nextState] = newCost
f[nextState] = g[(nextState)] + get_heuristic(nextState)
openSet.put(nextState, f[nextState])
openSet_check[nextState] = 1
return False
def dijkstra(start, C):
"""Input: Start state and Cost map
Output: The cost of each cell as a dictionary
This function performs the Dijkstra algorithm to find the cost of each cell in the map
"""
openSet = PriorityQueue()
openSet_check = {}
closedSet = {}
g = {}
for i in range(len(start)):
g[start[i]] = 0
openSet.put(start[i], 0)
openSet_check[start[i]] = 1
while not openSet.empty():
currentState = openSet.get()
closedSet[currentState] = 1
for nextState in get_successors(currentState):
if nextState in closedSet:
continue
newCost = g[currentState[0]] + get_cost(nextState)
if nextState not in openSet_check:
g[nextState] = newCost
openSet.put(nextState, g[nextState])
openSet_check[nextState] = 1
continue
elif newCost >= g[nextState]:
continue
g[nextState] = newCost
openSet.put(nextState, g[nextState])
openSet_check[nextState] = 1
# This writes the cost values onto a file which is accessed by the A-star algorithm
# for the planning part
f = open('heuristic.txt', 'w')
temp = ''
for i in range(M):
temp = ''
for j in range(N):
if j == 0:
temp += str(g[(i, j)])
else:
temp += ',' + str(g[(i, j)])
if i == 0:
f.write(temp)
else:
f.write('\n' + temp)
f.close()
return g
def Dijkstra(G, v0):
inf = float("+inf")
dist = [inf] * len(G)
prev = [None] * len(G)
dist[v0] = 0
Q = PriorityQueue()
for i in range(len(dist)):
Q.put(i, dist[i])
while not Q.empty():
u,d = Q.get()
for w in range(len(G)):
if G[u][w] and w in Q and dist[u] + G[u][w] < dist[w]:
dist[w] = dist[u] + G[u][w]
prev[w] = u
Q.update(w, dist[w])
return dist
def dijkstra(start,C):
openSet = PriorityQueue()
openCheck = {}
closedSet = {}
g = {}
for i in range(len(start)):
g[start[i]] = 0
for i in range(len(start)):
openSet.put(start[i],0)
openCheck[start[i]] = 1
while not openSet.empty():
currentState = openSet.get()
closedSet[currentState] = 1
for nextState in getSuccessors(currentState):
if nextState in closedSet:
continue
newCost = g[currentState] + cost(C,nextState)
if nextState not in openCheck:
g[nextState] = newCost
openSet.put(nextState,g[nextState])
openCheck[nextState] = 1
elif newCost >= g[nextState]:
continue
else:
g[nextState] = newCost
openSet.update(nextState,g[nextState])
openCheck[nextState] = 1
hmap = open('heuristic.txt','w')
temp=''
for i in range(N):
temp=''
for j in range(N):
if j==0:
temp +=str(g[(i,j)])
else:
temp +=','+str(g[(i,j)])
if i==0:
hmap.write(temp)
else:
hmap.write('\n'+temp)
hmap.close()
return False
def findPath(self, gridMap):
print("A*.findpath()")
startCell = gridMap.getCellOfType(CellConstants.START_CELL)
goal = gridMap.getCellOfType(CellConstants.FINISH_CELL)
openSet = PriorityQueue()
openSet.put(startCell, 0)
gScore = {} #cost so far
gScore[startCell] = 0.0
#
fScore = {}
fScore[startCell] = self.cbDist(
startCell.position, goal.position,
1.0) #calculate huristic (h()) and add to g() to get f()
cameFrom = {}
cameFrom[startCell] = None
while not openSet.empty():
current = openSet.get()
if current == goal:
print("Goal reached")
waypoints = self.reconstructPath(cameFrom, current)
return waypoints
#Get neighboring cells
t = gridMap.getNeighbors(current)
for next in t:
tentative_gScore = gScore[current] + next.cost
if next not in gScore or tentative_gScore < gScore.setdefault(
next, float('inf')):
gScore[next] = tentative_gScore
fScore[next] = gScore[next] + self.cbDist(
next.position, goal.position, 1.0)
openSet.put(next, fScore[next])
cameFrom[next] = current
print("Path not Found")
return None
def a_star_search(graph, start, finish):
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current == finish:
break
for next in graph.neighbors(current):
new_cost = cost_so_far[current] + graph.cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + heuristic(finish, next)
frontier.put(next, priority)
came_from[next] = current
return came_from, cost_so_far
def shortest_path_with_moving_polygons(self, start, goal, pick_up_points):
self.graph2D.reset()
frontier = PriorityQueue()
frontier.put(start, 0)
parent = {}
cost_so_far = {}
parent[start] = None
parent[goal] = None
cost_so_far[start] = 0
trace = []
self.history.append(self.graph2D.polygons)
while not frontier.empty():
current = frontier.get()
trace.append(current)
if current == goal:
break
forbidden_points = [start, goal]
for point in pick_up_points:
forbidden_points.append(point)
self.graph2D.move_polygons(forbidden_points)
self.history.append(self.graph2D.polygons)
for next in self.graph2D.get_neighbors(current):
new_cost = cost_so_far[current] + 1
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self.heuristic(goal, next)
frontier.put(next, priority)
parent[next] = current
return self.backtrace(parent, start, goal), trace
def dijkstra(self, G, start, end=None):
D = {} # dictionary of final distances
P = {} # dictionary of predecessors
Q = PriorityQueue() # estimated distances of non-final vertices
Q[start] = 0 # add zero cost
for v in Q:
D[v] = Q[v]
if v == end:
break
for w in G.getVertex(v).adjacencies:
vwLength = D.get(v) + G.vertexAdjacencies(v).get(w).getcost()
if w in D:
if vwLength < D.get(w):
raise ValueError("Dijkstra: found better path to already-final vertex")
elif w not in Q or vwLength < Q.get(w):
Q[w] = vwLength
P[w] = v
return D, P
def dijkstra(graph: Graph,
s,
g,
starter_tickets=[],
start_cost=0,
previous_path=[],
reversable_nodes=[],
is_recurrency_dijkstra=False):
start = graph.getNodeById(s)
goal = graph.getNodeById(g)
frontier = PriorityQueue()
frontier.put(start, 0)
came_from = {start.id: None}
start_tickets = set(starter_tickets)
if start.group is not None:
start_tickets.add(start.group)
tickets_earned = {start.id: start_tickets}
cost_so_far = {start.id: start_cost}
reverse_states = []
while not frontier.empty():
current = frontier.get()
if current == goal:
break
if current.id in reversable_nodes:
if came_from[
current.
id] is not None and current.group not in tickets_earned[
came_from[current.id]] and current.group != 0:
reverse_states.append(
(reconstruct_path(came_from, s, current.id),
cost_so_far[current.id], tickets_earned[current.id]))
elif came_from[
current.
id] is None and not is_recurrency_dijkstra and current.group != 0:
reverse_states.append(([current.id], cost_so_far[current.id],
tickets_earned[current.id]))
for i in graph.getNeighbors(current.id).values():
next_node = graph.getNodeById(i.id)
new_tickets = set(tickets_earned[current.id])
new_tickets.add(next_node.group)
new_cost = graph.getCost(
current, next_node,
tickets_earned[current.id]) + cost_so_far[current.id]
if next_node.id not in cost_so_far or new_cost < cost_so_far[
next_node.id]:
cost_so_far[next_node.id] = new_cost
priority = new_cost
frontier.put(next_node, priority)
came_from[next_node.id] = current.id
tickets_earned[next_node.id] = new_tickets
best_path = previous_path + reconstruct_path(came_from, start.id, goal.id)
best_cost = cost_so_far[goal.id]
best_tickets = tickets_earned[goal.id]
for state in reverse_states: # state = previous_path, previous_cost, previous_tickets
p, c, t = dijkstra(graph=graph,
s=state[0][-1],
g=g,
starter_tickets=state[2],
start_cost=state[1],
previous_path=state[0],
reversable_nodes=reversable_nodes,
is_recurrency_dijkstra=True)
if c < best_cost:
best_path = p
best_cost = c
best_tickets = t
best_path = clean_path(
best_path
) # clean path from 'duplicates' e.g [1, 0, 0, 2] -> [1,0,2]. Duplicates occur due to recurrency dijkstra
return best_path, best_cost, best_tickets
class PathFinder(object):
def __init__(self, bzrc, tank_index):
self.bzrc = bzrc
self.points = []
self.edges = []
self.visibility_graph = None
self.frontier = []
self.visited = []
self.path = []
self.search_snapshots = []
self.create_visibility_graph(tank_index)
def get_path(self):
return self.get_a_star_path()
########################
### VISIBILITY GRAPH ###
########################
def create_visibility_graph(self, tank_index):
print "Generating Visibility Graph for Tank", tank_index
tank = self.bzrc.get_mytanks()[tank_index]
self.points = []
# append tank position to points, this MUST be the first point in this list (we're relying on that for
# our search algorithms below)
self.points.append((tank.x, tank.y))
# append goal flag position to points
flags = self.bzrc.get_flags()
for flag in flags:
if tank.flag != '-' and flag.color in tank.callsign: # if the tank has the flag, go home
self.points.append((flag.x, flag.y)) # TODO: this line only works if the other team hasn't already taken our flag and run with it. We should probably use our base instead...
break
elif flag.color not in tank.callsign: # if the tank has no flag, go for one
self.points.append((flag.x, flag.y))
break
# append obstacle corners to points, and obstacle edges to our list of edges
for obstacle in self.bzrc.get_obstacles():
i = 0
for point in obstacle:
self.points.append(point)
self.edges.append([point, obstacle[(i+1) % len(obstacle)]])
i += 1
# initialize the visibility graph to all -1's
length = len(self.points)
self.visibility_graph = [[-1 for _ in range(length)] for _ in range(length)]
# figure out the visibility between each pair of points
for col in range(length):
for row in range(length):
if self.visibility_graph[row][col] == -1: # we haven't considered this pair of points yet
if self.is_visible(self.points[col], self.points[row]):
# "if you are visible to me, than I am visible to you" mentality
self.visibility_graph[row][col] = 1
self.visibility_graph[col][row] = 1
else:
# "if you are not visible to me, than I am not visible to you" mentality
self.visibility_graph[row][col] = 0
self.visibility_graph[col][row] = 0
# remove any edges that are on the very edge of the map (and therefore couldn't be traversed by the tank)
self.remove_visibility_on_world_edges()
def update_visibility_graph(self, tank_index):
tank = self.bzrc.get_mytanks()[tank_index]
self.points[0] = (tank.x, tank.y) # update the tank position
# update goal flag position in points
flags = self.bzrc.get_flags()
for flag in flags:
if tank.flag != '-' and flag.color in tank.callsign: # if the tank has the flag, go home
self.points[1] = (flag.x, flag.y) # TODO: this line only works if the other team hasn't already taken our flag and run with it. We should probably use our base instead...
break
elif tank.flag == '-' and flag.color not in tank.callsign: # if the tank has no flag, go for one
self.points[1] = (flag.x, flag.y)
break
# update the first two rows and columns in our visibility graph (these are the only points that have changed)
# initialize the visibility graph to all -1's
length = len(self.points)
# figure out the visibility between each pair of points
for col in range(length):
for row in range(length):
if col > 1 and row > 1:
continue
if self.is_visible(self.points[col], self.points[row]):
# "if you are visible to me, than I am visible to you" mentality
self.visibility_graph[row][col] = 1
self.visibility_graph[col][row] = 1
else:
# "if you are not visible to me, than I am not visible to you" mentality
self.visibility_graph[row][col] = 0
self.visibility_graph[col][row] = 0
def is_visible(self, point1, point2):
# check if they are the same point...a point is not visible to itself
if point1 == point2:
return False
# check if they share an edge
p1_found = False
for edge in self.edges:
if point1 in edge:
p1_found = True
if point2 in edge:
return True
# check if they are both in the same obstacle (but obviously don't share an edge because of the case above)
if p1_found:
for obstacle in self.bzrc.get_obstacles():
if point1 in obstacle and point2 in obstacle:
return False
# check if there are any intersecting edges between these two points
temp_edge = [point1, point2]
for edge in self.edges:
if self.line_segments_intersect(temp_edge, edge):
return False
# No edges intersected, these two points are visible to each other
return True
# Determines if two line segments intersect by checking that the two ends of one segment are on different sides
# of the other segment, AND vise-versa (both conditions must be true for the line segments to intersect).
# See http://stackoverflow.com/questions/7069420/check-if-two-line-segments-are-colliding-only-check-if-they-are-intersecting-n
# for further explanation & math.
def line_segments_intersect(self, edge1, edge2):
# if both points of edge1 are on the same side of edge2, or one of edge1's points is on edge2
# then we already know the lines aren't crossing over each other.
cp1 = self.cross_product(edge1[0], edge2)
cp2 = self.cross_product(edge1[1], edge2)
if (cp1 > 0 and cp2 > 0) or (cp1 < 0 and cp2 < 0) or cp1 == 0 or cp2 == 0:
return False
# if both points of edge2 are on the same side of edge1, or one of edge2's endpoints is on edge1
# then we know the lines aren't crossing over each other
cp1 = self.cross_product(edge2[0], edge1)
cp2 = self.cross_product(edge2[1], edge1)
if (cp1 > 0 and cp2 > 0) or (cp1 < 0 and cp2 < 0) or cp1 == 0 or cp2 == 0:
return False
return True
# Returns the cross product between the given line segment and the vector from that line segment to the given point
def cross_product(self, point, segment):
seg_start = segment[0]
seg_end = segment[1]
x = 0
y = 1
return (seg_end[x]-seg_start[x]) * (point[y]-seg_end[y]) - (seg_end[y]-seg_start[y]) * (point[x]-seg_end[x])
def remove_visibility_on_world_edges(self):
length = len(self.points)
for index1 in range(length):
point1 = self.points[index1]
# check left edge
if point1[0] == -400: # the x value is on the left edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[0] == -400: # both of these points' x values are on the left edge of the map
self.visibility_graph[index1][index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check right edge
elif point1[0] == 400: # the x value is on the right edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[0] == 400: # both of these points' x values are on the edge of the map
self.visibility_graph[index1][index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check top edge
elif point1[1] == 400: # the y value is on the top edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[1] == 400: # both of these points' y values are on the top edge of the map
self.visibility_graph[index1][index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check bottom edge
elif point1[1] == -400: # the y value is on the bottom edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[1] == -400: # both of these points' y values are on the bottom edge of the map
self.visibility_graph[index1][index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
def print_visibility_graph(self):
print "Visibility Graph:"
for row in self.visibility_graph:
print row
##########################
### DEPTH FIRST SEARCH ###
##########################
def get_depth_first_search_path(self):
print "Running Depth First Search to Get Path"
# clear everything and add the tank's start position to the frontier
self.clear_history()
self.frontier.append(self.points[0])
if not self.r_dfs(self.frontier[0]):
print >> sys.stderr, 'DFS failed to find the goal'
return self.path
def r_dfs(self, vertex):
self.search_snapshots.append(Snapshot(list(self.visited), list(self.frontier)))
self.frontier.remove(vertex)
self.visited.append(vertex)
# recursive base case, found the goal
if self.is_goal(vertex):
self.path.append(vertex)
return True
# find out which point we're dealing with
try:
index = self.points.index(vertex)
except:
print >> sys.stderr, 'Vertex not found in points'
return False
# prepare qualified new neighbors to be added to frontier
row = self.visibility_graph[index]
new_neighbors = []
index = 0
for item in row:
neighbor = self.points[index]
if item == 1 and neighbor not in self.visited:
new_neighbors.append(neighbor)
index += 1
# recursive base case, no new neighbors
if len(new_neighbors) == 0:
return False
# it's a beautiful day in the neighborhood
for neighbor in new_neighbors:
self.frontier.insert(0, neighbor)
if self.r_dfs(neighbor):
self.path.insert(0, vertex)
return True
# none of our children in this path returned true
return False
############################
### BREADTH FIRST SEARCH ###
############################
def get_breadth_first_search_path(self):
print "Running Breadth First Search to Get Path"
# clear everything and add the beginning node (with the tank's start position) to the frontier
self.clear_history()
self.frontier.append(BFSNode(self.points[0], None))
self.breadth_first_search()
return self.path
def breadth_first_search(self):
while self.frontier:
if self.frontier[0].my_point in self.visited:
# we've already visited this node, don't need to look at it again
self.frontier.pop(0)
continue
self.search_snapshots.append(Snapshot(list(self.visited), list(self.frontier), uses_bfs_nodes=True))
current_node = self.frontier.pop(0)
vertex = current_node.my_point
self.visited.append(vertex)
# find out which (x,y) point we're dealing with
try:
index = self.points.index(vertex)
except:
print >> sys.stderr, 'Vertex not found in points'
return
# find the neighbors of this point
row = self.visibility_graph[index]
for i in range(0, len(row)):
neighbor = self.points[i]
if row[i] == 1 and neighbor not in self.visited:
if self.is_goal(neighbor):
# If this child is the goal node, we're done!
last_node = BFSNode(neighbor, current_node)
self.frontier.append(last_node)
self.search_snapshots.append(Snapshot(list(self.visited), list(self.frontier), uses_bfs_nodes=True))
self.reconstruct_path_from_last_node(last_node)
return
else:
# Add this child to the end of the queue
self.frontier.append(BFSNode(neighbor, current_node))
# If we get to this point, then the frontier became empty before we found the goal
print "BFS failed to find the goal"
def reconstruct_path_from_last_node(self, last_node):
self.path = []
node = last_node
while node:
self.path.insert(0, node.my_point)
node = node.parent
############################
### A-STAR ALGORITHM ###
############################
def get_a_star_path(self):
print "Running A* to Get Path"
self.a_star_search()
return self.path
def a_star_search(self):
self.clear_history()
self.frontier = PriorityQueue()
self.frontier.put(AStarNode(self.points[0], None, 0), 0)
last_node = None
# iterate until the frontier is empty
while not self.frontier.empty():
snapshot = Snapshot(list(self.visited), self.frontier.getNodes(), uses_a_star_nodes=True)
self.search_snapshots.append(snapshot)
current = self.frontier.get()
if current.my_point in self.visited:
# Never mind, we didn't want that snapshot :)
self.search_snapshots.remove(snapshot)
continue
self.visited.append(current.my_point)
# end immediately if the goal is found
if self.is_goal(current.my_point):
last_node = current
break
# find out which point we're dealing with
try:
index = self.points.index(current.my_point)
except:
print >> sys.stderr, 'Vertex not found in points'
return False
# prepare qualified new neighbors to be added to frontier
row = self.visibility_graph[index]
index = 0
for item in row:
neighbor = self.points[index]
if item == 1 and neighbor not in self.visited:
distance = current.cost
distance += self.distance(neighbor, current.my_point) # distance so far
est_distance_remaining = distance + self.distance(neighbor, self.points[1]) # est. distance to go
self.frontier.put(AStarNode(neighbor, current, distance), est_distance_remaining)
index += 1
# unwind path back to start
while not last_node == None:
self.path.insert(0, last_node.my_point)
last_node = last_node.parent
################################
### SEARCH ALGORITHM HELPERS ###
################################
def clear_history(self):
self.visited = []
self.frontier = []
self.path = []
self.search_snapshots = []
def is_goal(self, point):
return point == self.points[1] # the goal point is always the second one in our list of points
def distance(self, point1, point2):
return math.sqrt((point1[0]-point2[0])**2 + (point1[1] - point2[1]) ** 2)
class DungeonMaster(object):
def __init__(self, clock, dungeon, locale_id):
self.clock = clock
self.dungeon = dungeon
self.path_finder = PathFinder(dungeon)
self.locale_id = locale_id
self.queue = PriorityQueue()
self.factions = {}
self.playing = True
self.dungeon_expire = self.clock.max_locale_time
self.char_id = 'DM'
self.placement_locations = {
'ne': [],
'e': [],
'se': [],
'nw': [],
'w': [],
'sw': [],
}
self.init_placement_locations()
self.clock.set_local_time(locale_id=locale_id, locale_time=0)
def init_placement_locations(self):
center = self.dungeon.get_hex(x=0, y=0, z=0)
ring_1 = self.dungeon.ring(center, self.dungeon.map_radius - 1)
ring_2 = self.dungeon.ring(center, self.dungeon.map_radius)
rings = ring_1 + ring_2
high = self.dungeon.map_radius - 1
low = 1
for point in rings:
x = abs(point.x)
y = abs(point.y)
z = abs(point.z)
if x >= high and y > low and y < high:
if point.x < 0:
self.placement_locations['w'].append(point)
else:
self.placement_locations['e'].append(point)
elif y >= high and x > low and x < high:
if point.y < 0:
self.placement_locations['se'].append(point)
else:
self.placement_locations['nw'].append(point)
elif z >= high and x > low and x < high:
if point.z < 0:
self.placement_locations['ne'].append(point)
else:
self.placement_locations['sw'].append(point)
def run_dungeon(self):
while self.is_playing():
self.run()
def run(self):
char = self.next_char()
self.increment_time()
while self.get_time() < char.gcd:
self.increment_time()
self.activate_char(char)
def is_playing(self):
if self.get_time() > self.dungeon_expire:
self.playing = False
elif len(self.factions) <= 1:
self.playing = False
else:
self.playing = True
return self.playing
def add_char(self, member, faction='dm', edge='dm', insert_time=0):
if faction == 'dm':
faction = self
if faction.faction_id not in self.factions.keys():
self.factions[faction.faction_id] = []
if insert_time == 0:
insert_time = self.get_time() + member.get_stat('initiative')
self.factions[faction.faction_id].append(member)
self.queue.put(member, insert_time)
member.dm = self
faction.place_char(member, self.get_placement_locations(edge))
def remove_char(self, member):
faction_id = member.faction.faction_id
self.factions[faction_id].remove(member)
if len(self.factions[faction_id]) == 0:
self.factions.pop(faction_id, None)
def next_char(self):
return self.queue.get()
def get_time(self):
return self.clock.get_locale_time(self.locale_id)
def increment_time(self):
self.clock.increment_locale_time(self.locale_id)
def activate_char(self, char):
priority = char.activate()
if char.health > 0:
self.queue.put(char, priority)
def get_placement_locations(self, edge):
sides = ['ne', 'e', 'se', 'sw', 'w', 'nw']
if edge in sides:
return self.placement_locations[edge]
def get_adjacent_enemies(self, requestor):
hexes = self.dungeon.neighbors(requestor.dungeon_hex)
return self.get_enemies(requestor, hexes)
def get_nearby_enemies(self, requestor, radius=2):
hexes = self.dungeon.spiral(requestor.dungeon_hex, radius)
return self.get_enemies(requestor, hexes)
def get_enemies(self, requestor, hexes):
enemies = []
for dungeon_hex in hexes:
if dungeon_hex is None:
next
elif dungeon_hex.character is None:
next
elif dungeon_hex.character.faction != requestor.faction:
enemies.append(dungeon_hex.character)
return enemies
def get_nearest_enemy(self, requestor):
enemies = []
min_dist = requestor.sight_range
closest = None
for faction in self.factions.keys():
if faction != requestor.faction.faction_id:
for member in self.factions[faction]:
enemies.append(member)
for enemy in enemies:
current = self.distance(requestor, enemy)
if current < min_dist:
min_dist = current
closest = enemy
requestor.target_enemy = enemy
requestor.distance_to_enemy = min_dist
return closest
def distance(self, actor, target):
return self.dungeon.distance(actor.dungeon_hex, target.dungeon_hex)
def move_char_along_path(self, actor, source, dest, distance):
path = self.path_finder.find_path(
start=source,
goal=dest,
)
if (len(path) - 1) < distance:
distance = (len(path) - 1)
actor.move(path[distance])
class PathFinder(object):
def __init__(self, bzrc, tank_index):
self.bzrc = bzrc
self.points = []
self.edges = []
self.visibility_graph = None
self.frontier = []
self.visited = []
self.path = []
self.search_snapshots = []
self.create_visibility_graph(tank_index)
def get_path(self):
return self.get_a_star_path()
########################
### VISIBILITY GRAPH ###
########################
def create_visibility_graph(self, tank_index):
print "Generating Visibility Graph for Tank", tank_index
tank = self.bzrc.get_mytanks()[tank_index]
self.points = []
# append tank position to points, this MUST be the first point in this list (we're relying on that for
# our search algorithms below)
self.points.append((tank.x, tank.y))
# append goal flag position to points
flags = self.bzrc.get_flags()
for flag in flags:
if tank.flag != '-' and flag.color in tank.callsign: # if the tank has the flag, go home
self.points.append(
(flag.x, flag.y)
) # TODO: this line only works if the other team hasn't already taken our flag and run with it. We should probably use our base instead...
break
elif flag.color not in tank.callsign: # if the tank has no flag, go for one
self.points.append((flag.x, flag.y))
break
# append obstacle corners to points, and obstacle edges to our list of edges
for obstacle in self.bzrc.get_obstacles():
i = 0
for point in obstacle:
self.points.append(point)
self.edges.append([point, obstacle[(i + 1) % len(obstacle)]])
i += 1
# initialize the visibility graph to all -1's
length = len(self.points)
self.visibility_graph = [[-1 for _ in range(length)]
for _ in range(length)]
# figure out the visibility between each pair of points
for col in range(length):
for row in range(length):
if self.visibility_graph[row][
col] == -1: # we haven't considered this pair of points yet
if self.is_visible(self.points[col], self.points[row]):
# "if you are visible to me, than I am visible to you" mentality
self.visibility_graph[row][col] = 1
self.visibility_graph[col][row] = 1
else:
# "if you are not visible to me, than I am not visible to you" mentality
self.visibility_graph[row][col] = 0
self.visibility_graph[col][row] = 0
# remove any edges that are on the very edge of the map (and therefore couldn't be traversed by the tank)
self.remove_visibility_on_world_edges()
def update_visibility_graph(self, tank_index):
tank = self.bzrc.get_mytanks()[tank_index]
self.points[0] = (tank.x, tank.y) # update the tank position
# update goal flag position in points
flags = self.bzrc.get_flags()
for flag in flags:
if tank.flag != '-' and flag.color in tank.callsign: # if the tank has the flag, go home
self.points[1] = (
flag.x, flag.y
) # TODO: this line only works if the other team hasn't already taken our flag and run with it. We should probably use our base instead...
break
elif tank.flag == '-' and flag.color not in tank.callsign: # if the tank has no flag, go for one
self.points[1] = (flag.x, flag.y)
break
# update the first two rows and columns in our visibility graph (these are the only points that have changed)
# initialize the visibility graph to all -1's
length = len(self.points)
# figure out the visibility between each pair of points
for col in range(length):
for row in range(length):
if col > 1 and row > 1:
continue
if self.is_visible(self.points[col], self.points[row]):
# "if you are visible to me, than I am visible to you" mentality
self.visibility_graph[row][col] = 1
self.visibility_graph[col][row] = 1
else:
# "if you are not visible to me, than I am not visible to you" mentality
self.visibility_graph[row][col] = 0
self.visibility_graph[col][row] = 0
def is_visible(self, point1, point2):
# check if they are the same point...a point is not visible to itself
if point1 == point2:
return False
# check if they share an edge
p1_found = False
for edge in self.edges:
if point1 in edge:
p1_found = True
if point2 in edge:
return True
# check if they are both in the same obstacle (but obviously don't share an edge because of the case above)
if p1_found:
for obstacle in self.bzrc.get_obstacles():
if point1 in obstacle and point2 in obstacle:
return False
# check if there are any intersecting edges between these two points
temp_edge = [point1, point2]
for edge in self.edges:
if self.line_segments_intersect(temp_edge, edge):
return False
# No edges intersected, these two points are visible to each other
return True
# Determines if two line segments intersect by checking that the two ends of one segment are on different sides
# of the other segment, AND vise-versa (both conditions must be true for the line segments to intersect).
# See http://stackoverflow.com/questions/7069420/check-if-two-line-segments-are-colliding-only-check-if-they-are-intersecting-n
# for further explanation & math.
def line_segments_intersect(self, edge1, edge2):
# if both points of edge1 are on the same side of edge2, or one of edge1's points is on edge2
# then we already know the lines aren't crossing over each other.
cp1 = self.cross_product(edge1[0], edge2)
cp2 = self.cross_product(edge1[1], edge2)
if (cp1 > 0 and cp2 > 0) or (cp1 < 0
and cp2 < 0) or cp1 == 0 or cp2 == 0:
return False
# if both points of edge2 are on the same side of edge1, or one of edge2's endpoints is on edge1
# then we know the lines aren't crossing over each other
cp1 = self.cross_product(edge2[0], edge1)
cp2 = self.cross_product(edge2[1], edge1)
if (cp1 > 0 and cp2 > 0) or (cp1 < 0
and cp2 < 0) or cp1 == 0 or cp2 == 0:
return False
return True
# Returns the cross product between the given line segment and the vector from that line segment to the given point
def cross_product(self, point, segment):
seg_start = segment[0]
seg_end = segment[1]
x = 0
y = 1
return (seg_end[x] - seg_start[x]) * (point[y] - seg_end[y]) - (
seg_end[y] - seg_start[y]) * (point[x] - seg_end[x])
def remove_visibility_on_world_edges(self):
length = len(self.points)
for index1 in range(length):
point1 = self.points[index1]
# check left edge
if point1[0] == -400: # the x value is on the left edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[
0] == -400: # both of these points' x values are on the left edge of the map
self.visibility_graph[index1][
index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check right edge
elif point1[
0] == 400: # the x value is on the right edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[
0] == 400: # both of these points' x values are on the edge of the map
self.visibility_graph[index1][
index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check top edge
elif point1[1] == 400: # the y value is on the top edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[
1] == 400: # both of these points' y values are on the top edge of the map
self.visibility_graph[index1][
index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
# check bottom edge
elif point1[
1] == -400: # the y value is on the bottom edge of the map
for index2 in range(length):
point2 = self.points[index2]
if self.visibility_graph[index1][index2] == 1 and point2[
1] == -400: # both of these points' y values are on the bottom edge of the map
self.visibility_graph[index1][
index2] = 0 # make them invisible to each other
self.visibility_graph[index2][index1] = 0
def print_visibility_graph(self):
print "Visibility Graph:"
for row in self.visibility_graph:
print row
##########################
### DEPTH FIRST SEARCH ###
##########################
def get_depth_first_search_path(self):
print "Running Depth First Search to Get Path"
# clear everything and add the tank's start position to the frontier
self.clear_history()
self.frontier.append(self.points[0])
if not self.r_dfs(self.frontier[0]):
print >> sys.stderr, 'DFS failed to find the goal'
return self.path
def r_dfs(self, vertex):
self.search_snapshots.append(
Snapshot(list(self.visited), list(self.frontier)))
self.frontier.remove(vertex)
self.visited.append(vertex)
# recursive base case, found the goal
if self.is_goal(vertex):
self.path.append(vertex)
return True
# find out which point we're dealing with
try:
index = self.points.index(vertex)
except:
print >> sys.stderr, 'Vertex not found in points'
return False
# prepare qualified new neighbors to be added to frontier
row = self.visibility_graph[index]
new_neighbors = []
index = 0
for item in row:
neighbor = self.points[index]
if item == 1 and neighbor not in self.visited:
new_neighbors.append(neighbor)
index += 1
# recursive base case, no new neighbors
if len(new_neighbors) == 0:
return False
# it's a beautiful day in the neighborhood
for neighbor in new_neighbors:
self.frontier.insert(0, neighbor)
if self.r_dfs(neighbor):
self.path.insert(0, vertex)
return True
# none of our children in this path returned true
return False
############################
### BREADTH FIRST SEARCH ###
############################
def get_breadth_first_search_path(self):
print "Running Breadth First Search to Get Path"
# clear everything and add the beginning node (with the tank's start position) to the frontier
self.clear_history()
self.frontier.append(BFSNode(self.points[0], None))
self.breadth_first_search()
return self.path
def breadth_first_search(self):
while self.frontier:
if self.frontier[0].my_point in self.visited:
# we've already visited this node, don't need to look at it again
self.frontier.pop(0)
continue
self.search_snapshots.append(
Snapshot(list(self.visited),
list(self.frontier),
uses_bfs_nodes=True))
current_node = self.frontier.pop(0)
vertex = current_node.my_point
self.visited.append(vertex)
# find out which (x,y) point we're dealing with
try:
index = self.points.index(vertex)
except:
print >> sys.stderr, 'Vertex not found in points'
return
# find the neighbors of this point
row = self.visibility_graph[index]
for i in range(0, len(row)):
neighbor = self.points[i]
if row[i] == 1 and neighbor not in self.visited:
if self.is_goal(neighbor):
# If this child is the goal node, we're done!
last_node = BFSNode(neighbor, current_node)
self.frontier.append(last_node)
self.search_snapshots.append(
Snapshot(list(self.visited),
list(self.frontier),
uses_bfs_nodes=True))
self.reconstruct_path_from_last_node(last_node)
return
else:
# Add this child to the end of the queue
self.frontier.append(BFSNode(neighbor, current_node))
# If we get to this point, then the frontier became empty before we found the goal
print "BFS failed to find the goal"
def reconstruct_path_from_last_node(self, last_node):
self.path = []
node = last_node
while node:
self.path.insert(0, node.my_point)
node = node.parent
############################
### A-STAR ALGORITHM ###
############################
def get_a_star_path(self):
print "Running A* to Get Path"
self.a_star_search()
return self.path
def a_star_search(self):
self.clear_history()
self.frontier = PriorityQueue()
self.frontier.put(AStarNode(self.points[0], None, 0), 0)
last_node = None
# iterate until the frontier is empty
while not self.frontier.empty():
snapshot = Snapshot(list(self.visited),
self.frontier.getNodes(),
uses_a_star_nodes=True)
self.search_snapshots.append(snapshot)
current = self.frontier.get()
if current.my_point in self.visited:
# Never mind, we didn't want that snapshot :)
self.search_snapshots.remove(snapshot)
continue
self.visited.append(current.my_point)
# end immediately if the goal is found
if self.is_goal(current.my_point):
last_node = current
break
# find out which point we're dealing with
try:
index = self.points.index(current.my_point)
except:
print >> sys.stderr, 'Vertex not found in points'
return False
# prepare qualified new neighbors to be added to frontier
row = self.visibility_graph[index]
index = 0
for item in row:
neighbor = self.points[index]
if item == 1 and neighbor not in self.visited:
distance = current.cost
distance += self.distance(
neighbor, current.my_point) # distance so far
est_distance_remaining = distance + self.distance(
neighbor, self.points[1]) # est. distance to go
self.frontier.put(AStarNode(neighbor, current, distance),
est_distance_remaining)
index += 1
# unwind path back to start
while not last_node == None:
self.path.insert(0, last_node.my_point)
last_node = last_node.parent
################################
### SEARCH ALGORITHM HELPERS ###
################################
def clear_history(self):
self.visited = []
self.frontier = []
self.path = []
self.search_snapshots = []
def is_goal(self, point):
return point == self.points[
1] # the goal point is always the second one in our list of points
def distance(self, point1, point2):
return math.sqrt((point1[0] - point2[0])**2 +
(point1[1] - point2[1])**2)
def A_star(self):
"""
Returns the path from the start position to the goal position using the
A-star (A*) algorithm. Returns <None> if no solution is found.
"""
# Heuristic distance between two positions
def heuristic(a, b):
"""
Calculates the Manhattan distance between two positions.
"""
x1, y1 = a
x2, y2 = b
dist = abs(x1 - x2) + abs(y1 - y2)
return dist
# Initialize the priority queue
pq = PriorityQueue(queue_type='min')
# Values for the start point
g = 0
h = heuristic(self.goal, self.start)
f = g + h
# Add the start point to the priority queue.
pq.put(f, self.start)
g_values = {self.start: g}
previous = {self.start: None}
self.added = 1
# Loop until the priority queue is empty
self.visited = 0
while (not pq.is_empty()):
# Get the highest priority position from the priority queue
f, current = pq.get()
self.visited += 1
# Stop if it is the goal and return the path
if (current == self.goal):
path = self.get_path(previous)
return path
# Define the order in the directions
idx = self.order_dir()
# Add to the priority queue the neighbours of the current position
for direction in idx:
# Offset values
row_offset, col_offset = self.offset[direction]
# Neighbour position
row_neigh = current[0] + row_offset
col_neigh = current[1] + col_offset
neighbour = (row_neigh, col_neigh)
# If neighbour position is valid and not in the dictionary
if (self.layout[row_neigh][col_neigh] != '#'
and self.layout[row_neigh][col_neigh] != '*'
and neighbour not in previous):
# Values (the g-value of all neighbour positions differ
# from the g-value of the current position by 1)
g = g_values[current] + 1
h = heuristic(self.goal, neighbour)
f = g + h
# Add it to the priority queue
pq.put(f, neighbour)
g_values[neighbour] = g
previous[neighbour] = current
self.added += 1
return None
class AStarPlanner(object):
def __init__(self, planning_env):
self.planning_env = planning_env
self.nodes = dict()
self.PQ = PriorityQueue()
def Plan(self, start_config, goal_config):
print start_config
print goal_config
waypoints = {}
cost_at_way = {}
plan = []
start = self.planning_env.discrete_env.configuration_to_nodeID(
start_config)
goal = self.planning_env.discrete_env.configuration_to_nodeID(
goal_config)
print "start:", start
print "goal:", goal
x = []
y = []
count = 0
waypoints = {}
cost_at_way[start] = 0
waypoints[start] = 0
self.PQ.put(start, 0)
weight = 1
while not self.PQ.empty():
current = self.PQ.get()
count = count + 1
#print "current",current
if (current == goal):
print "goal reached"
thing = copy.copy(goal)
path = numpy.array([
self.planning_env.discrete_env.nodeID_to_configuration(
thing)
])
npath = numpy.array([thing])
thing = waypoints[thing]
while thing != 0:
path = numpy.vstack((numpy.array(
self.planning_env.discrete_env.nodeID_to_configuration(
thing)), path))
npath = numpy.vstack((numpy.array(thing), npath))
thing = waypoints[thing]
print "nodes expanded:", count
print "npath:", path
print "self.planning_env.count", self.planning_env.count
print "Final cost", cost_at_way[goal]
if self.planning_env.space_dim == 2:
self.planning_env.plotthegraph(path)
return path
#print self.planning_env.get_successors(current)
for next in self.planning_env.get_successors(current):
#print "next:",next
#print "waypoints[current]",waypoints[current]
new_cost = cost_at_way[
current] + self.planning_env.compute_distance(
current, next) + 5 * self.planning_env.modeswitch(
current, next, waypoints[current])
if next not in cost_at_way or new_cost < cost_at_way[next]:
#if next not in cost_at_way:
#print "new cost:",new_cost
#else:
#print "updated cost:",new_cost
cost_at_way[next] = new_cost
priority = new_cost + weight * self.planning_env.get_heuristic(
next, goal)
self.PQ.put(next, priority)
waypoints[next] = current
#print "waypoint:",waypoints
print "Path Not Found"
return 0
def Plan(self, start_config, goal_config):
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
## INITIALIZATIONS
#print "start_config",start_config
#print "goal_config",goal_config
start_time = time.time()
startID = self.planning_env.find_ID(start_config)
goalID = self.planning_env.find_ID(goal_config)
print "IDs", startID, goalID
PQ = PriorityQueue()
g = {}
closedlist = []
PQ.put(startID, 0)
truecost = {}
g[startID] = 0
truecost[startID] = True
parent = {}
parent[startID] = 0
# Starting of planner
while not PQ.empty():
current = PQ.get()
#print "current",current
if current == goalID:
print "goal reached"
thing = copy.copy(goalID)
path = numpy.array([self.planning_env.IDtoconfig(thing)])
npath = numpy.array([thing])
thing = parent[thing]
while thing != 0:
path = numpy.vstack(
(numpy.array(self.planning_env.IDtoconfig(thing)),
path))
npath = numpy.vstack((numpy.array(thing), npath))
thing = parent[thing]
path = numpy.vstack(
(numpy.array(self.planning_env.IDtoconfig(thing)), path))
npath = numpy.vstack((numpy.array(thing), npath))
#print "nodes expanded:",count
print "npath:", path
#print "self.planning_env.count",self.planning_env.count
print "Number of edge evalualtion", self.planning_env.count
print "FInal TIme ", time.time() - start_time
print "Final cost", g[goalID]
#if self.planning_env.space_dim==2:
# self.planning_env.plotthegraph(path)
return path
break
if current in closedlist:
continue
elif truecost[current]: # tells if its in collision or not
closedlist.append(current)
for neighbor in self.planning_env.get_successors(current):
if neighbor not in closedlist:
#print "neighbor",neighbor
tempcost = g[
current] + self.planning_env.compute_distance(
current, neighbor)
if neighbor not in g or tempcost < g[neighbor]:
g[neighbor] = tempcost
priority = tempcost + self.planning_env.get_heuristic(
neighbor, goalID)
PQ.put(neighbor, priority)
truecost[neighbor] = False
parent[neighbor] = current
else:
temp = self.planning_env.gettruecost(parent[current], current)
#print "temp,node",temp,current
if temp < float('inf'):
truecost[current] = True
g_here = g[parent[current]] + temp
#self.planning_env.PlotEdge(neighbour,new_guy)
if current not in g or g_here <= g[current]:
#print "coming here"
g[current] = g_here
priority = g_here + self.planning_env.get_heuristic(
neighbor, goalID)
PQ.put(current, priority)
print "Goal not found"
#startID= self.planning_env.configuration_to_nodeID(start_config)
plan = []
plan.append(start_config)
plan.append(goal_config)
return plan
class DStar_planner(object):
def __init__(self, start, goal, known_map, r): #The non-pset version
# def __init__(self, start, goal, known_map, r, _uvu, _cspu): #PSET verison
self.s_start = KeyedVertex(start)
self.s_last = self.s_start
self.s_goal = KeyedVertex(goal)
self.k_m = 0
self.U = PriorityQueue() # Koenig-22
self.map_known = known_map
self.robRange = r
self.encounteredVertices = []
#further initialization
self.s_goal.rhs = 0
self.s_goal.key = self.s_goal.CalculateKey(self.s_start,
self.map_known, self.k_m)
self.U.put(self.s_goal)
self.encounteredVertices.append(self.s_goal)
#This one is my addition, to help track initialized vertices
self.encounteredVertices.append(self.s_start)
#configurable pset functions
# self.uvu = _uvu
# self.cspu = _cspu
def pathExists(self):
if (self.s_start.g == inf):
print(
"No path exists by virtue of the start node having infinite g cost."
)
return False
else:
return True
# For each of the update functions, check to see if we're tracking one already
def updateStart(self, pos):
self.s_start = next(
(s for s in self.encounteredVertices if (s.pos == pos)),
KeyedVertex(pos))
# This goal refers to overall current target, not next waypoint
def updateGoal(self, pos):
self.s_goal = next(
(s for s in self.encounteredVertices if (s.pos == pos)), False)
if (
not self.s_goal
): #<Burn it to the ground> This means the above got false back, and the whole thing needs to be reset for the next pathfinding goal
self.encounteredVertices = [] #abandon all the collected values
self.s_start = KeyedVertex(
self.s_start.pos) #creating a new copy, forget about the old
self.s_goal = KeyedVertex(pos) # as originally fed in
self.s_last = self.s_start
self.k_m = 0
self.U = PriorityQueue()
# cont'd
self.s_goal.rhs = 0
self.s_goal.key = self.s_goal.CalculateKey(self.s_start,
self.map_known,
self.k_m)
self.U.put(self.s_goal)
self.encounteredVertices.append(self.s_goal)
#This one is my addition, to help track initialized vertices
self.encounteredVertices.append(self.s_start)
def checkAndUpdate(self, path, planStep):
# Starts at line 28; prior lines taken care of via main & stepping
# print("C&U -> Plan step: {}".format(self.s_start)) #Plan step should always be 0 with this config
changedEdges = []
edgeFrom = self.s_start #s
for pos in path[1:]: # Koenig-28
edgeTo = next(
(s for s in self.encounteredVertices if (s.pos == pos)),
False) # get corresponding vertex
if (self.map_known.c(
edgeFrom,
edgeTo) == inf): # Koenig-29 obstruction appeared
changedEdges.append(edgeFrom) #s
edgeFrom = edgeTo
if (len(changedEdges) > 0):
# print("Replanning due to changed edges..."),
# self.k_m = self.k_m + self.s_last.h(self.s_start,self.map_known)
prvKm = self.k_m
prvH = self.s_last.h(self.s_start, self.map_known)
self.k_m = prvKm + prvH
self.s_last = copy.deepcopy(
self.s_start
) #so it doesn't link w/ and change when s_start changes. could just record pos, but then the nifty h function would need to be reworked
for s in changedEdges:
# print("Changed edges:{}".format(s))
self.UpdateVertex(s)
path = self.computeShortestPath()
# path = retrieveShortestPath(map_known, agent1.getPos()) # Koenig-26a (overkill, only need first element, but we want to record the whole map)
if (not path):
print("Path not retrieved successfully. Final (known) state:")
self.map_known.display_terminal()
# input("Exits on enter...")
raise Exception("Path not found. Self destruct!")
# input("Exits on enter...")
return True, path
else:
return False, path
# # print("No new obstructions.")
def computeShortestPath(self):
# return self.cspu(self.map_known, self) #relic of PSET
while( (self.U.key_peek() < self.s_start.CalculateKey(self.s_start,self.map_known,self.k_m)) \
or (self.s_start.rhs != self.s_start.g) ): # Koenig-10")
k_old = self.U.key_peek() # Koenig-11
u = self.U.get() # Koenig-12
if (k_old < u.CalculateKey(self.s_start, self.map_known,
self.k_m)): # Koenig-13
u.key = u.CalculateKey(self.s_start, self.map_known,
self.k_m) # Koenig-14 (start)
self.U.put(u) # Koenig-14 (end)
elif (u.g > u.rhs): # Koenig-15
u.g = u.rhs # Koenig-16
# Koenig-17 (start)
for s in self.potentialPredeccessorPositions(u):
self.UpdateVertex(s) # Koenig-17 (end)
else: # Koenig-18
u.g = inf # Koenig-19
# Koenig-20 (start)
for s in self.potentialPredeccessorPositions(u):
self.UpdateVertex(s)
self.UpdateVertex(u) # Koenig-20 (end)
return self.retrieveShortestPath()
# get list of potential movements
# r dictates range (per dynamics, etc)
# TODO: Should we consider dynamics to determine where you can actually get - immediately left /right may be in "range" of a ground robot, but it can't strafe
# inits and adds to encountered list if not previously on it
# Note: Do not consider obsticles here, that's should be in calculating the cost to go
def potentialSuccessorPositions(
self, slf
): #Up down left right? requires some dynamics knowledge in future
intRange = int(
self.robRange) #can round down for what range to iterate over
xDim, yDim = self.map_known.dimensions()
xRange = range(
max(slf.pos[0] - intRange - 1, 0),
min(slf.pos[0] + intRange + 1,
xDim - 1)) #center around s.position, and limit to Map bounds
#For y, visibly up is numerically down (origin is top-left) ; split up so can break on dist easier
yRangeDown = range(slf.pos[1], min(
slf.pos[1] + intRange + 1,
yDim - 1)) #center around s.position, and limit to Map bounds
yRangeUp = range(
(slf.pos[1] - 1), max(slf.pos[1] - intRange - 1, 0),
-1) #center around s.position, and limit to Map bounds
successors = []
for x in xRange:
for y in yRangeDown:
if (slf.pos == (x, y)):
continue
if (MAII_map.distance(slf.pos, (x, y)) > self.robRange):
break
else:
s = next((s for s in self.encounteredVertices
if (s.pos == (x, y))), False)
if not s:
s = KeyedVertex((x, y))
self.encounteredVertices.append(s)
successors.append(
s) #get from list of existing; if doesn't exist, init
for y in yRangeUp:
if (MAII_map.distance(slf.pos, (x, y)) > self.robRange):
break
else:
s = next((s for s in self.encounteredVertices
if (s.pos == (x, y))), False)
if not s:
s = KeyedVertex((x, y))
self.encounteredVertices.append(s)
successors.append(s)
return successors
# For now, disregarding dynamics, successors=
# See above commentary on self.potentialSuccessorPositions
def potentialPredeccessorPositions(self, s):
return self.potentialSuccessorPositions(s)
def UpdateVertex(self, u):
# self.uvu(self.map_known, self, u) # PSET relic
if not (u == self.s_goal): # Koenig-7 (start)
successors = self.potentialSuccessorPositions(u)
min_cost = inf
for s in successors: #over successors
# self.map_known.setObs((1,3),False)
cg = s.g + self.map_known.c(u, s)
if (cg < min_cost):
min_cost = cg
u.rhs = min_cost # Koenig-7 (end)
self.U.remove(u) # Koenig-8
u.key = u.CalculateKey(self.s_start, self.map_known,
self.k_m) # Koenig-9 (start)
if not (u.g == u.rhs):
self.U.put(u)
# Go from start to fin by following vertexes which minimize c(s,s')+g(s')
def retrieveShortestPath(self): #, known_map, cur):
path = []
curPos = next(
(s
for s in self.encounteredVertices if (s.pos == self.s_start.pos)),
False)
path.append(curPos.pos)
nextPos = curPos
iteration = 0
while not (curPos.pos == self.s_goal.pos): # Koenig-7 (start)
succs = self.potentialSuccessorPositions(curPos)
min_cost = inf
for s_suc in succs: #over successors
cg = s_suc.g + self.map_known.c(curPos, s_suc)
if (cg < min_cost):
min_cost = cg
nextPos = s_suc
if (min_cost == inf):
return False
elif (iteration == 50):
return False
curPos = nextPos
path.append(curPos.pos)
iteration += 1
return path
def a_star_search(start, goal, D_cost_so_far):
frontier = PriorityQueue()
frontier.put(start)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current = frontier.get()
if current == goal:
break
for next in current.successors():
new_cost = cost_so_far[current] + 1
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
next.priority = new_cost + next.heuristic(D_cost_so_far)
frontier.put(next)
came_from[next] = current
# find path
current = goal
path = []
while current != start:
path.append(current.pieces)
current = came_from[current]
path.append(start.pieces)
path.reverse()
# print result
last = path[0]
cur_piece = 0
last_piece = 0
actionNo = 0
for cur in path:
# print exit if length of path change
if len(cur) < len(last):
for piece_i in range(0, len(last)):
if last[piece_i] not in cur:
last_piece = last[piece_i]
print('EXIT from {0}.'.format(tuple(last_piece)))
actionNo += 1
else:
for piece_i in range(0, len(cur)):
if cur[piece_i] not in last:
cur_piece = cur[piece_i]
for piece_i in range(0, len(last)):
if last[piece_i] not in cur:
last_piece = last[piece_i]
# print move if hexa distance is 1, jump if hexa distance is 2
if type(cur_piece) != type(0):
if hexe_distance(last_piece, cur_piece) > 1:
print('JUMP from {0} to {1}.'.format(
tuple(last_piece), tuple(cur_piece)))
actionNo += 1
else:
print('MOVE from {0} to {1}.'.format(
tuple(last_piece), tuple(cur_piece)))
actionNo += 1
last = cur
def Plan(self, start_config, goal_config):
if self.visualize and hasattr(self.planning_env, 'InitializePlot'):
self.planning_env.InitializePlot(goal_config)
# Starting of planner
start_time=time.time()
startID= self.planning_env.find_ID(start_config)
goalID= self.planning_env.find_ID(goal_config)
print "IDs",startID,goalID
PQ= PriorityQueue()
g={}
PQ.put(startID,0)
g[startID]=0
parent={}
parent[startID]=-1
x=0
memory=[]
# Starting of planner
while not PQ.empty():
current= PQ.get()
if current==goalID:
x=x+1
print "goal reached ",x
thing=copy.copy(goalID)
path= numpy.array([self.planning_env.IDtoconfig(thing)])
npath= numpy.array([thing])
thing= parent[thing]
while thing!=-1:
path= numpy.vstack((numpy.array(self.planning_env.IDtoconfig(thing)),path))
npath= numpy.vstack((numpy.array(thing),npath))
thing = parent[thing]
thing=copy.copy(goalID)
collision= False
while thing!= startID:
temp= self.planning_env.gettruecost(parent[thing],thing)
if temp==float('inf'):
memory.append([parent[thing],thing])
collision =True
#print collision
thing = parent[thing]
#path= numpy.vstack((numpy.array(self.planning_env.IDtoconfig(thing)),path))
#npath= numpy.vstack((numpy.array(thing),npath))
#print "npath:",npath
#print "memory",memory
#c=input("stop")
if collision==True:
PQ= PriorityQueue()
g={}
PQ.put(startID,0)
g[startID]=0
parent={}
parent[startID]=-1
continue
print "Number of edge evalualtion",self.planning_env.count
print "FInal TIme ", time.time()-start_time
print "npath:",path
#print "self.planning_env.count",self.planning_env.count
print "Final cost",g[goalID]
#if self.planning_env.space_dim==2:
# self.planning_env.plotthegraph(path)
return path
break
for neighbor in self.planning_env.get_successors(current):
if [current,neighbor] not in memory:
if [neighbor,current] not in memory:
tempcost= g[current] + self.planning_env.compute_distance(current,neighbor)
if neighbor not in g or tempcost < g[neighbor]:
g[neighbor]=tempcost
priority= tempcost+ self.planning_env.get_heuristic(neighbor,goalID)
PQ.put(neighbor,priority)
parent[neighbor]= current
print "Goal not found"
class Game:
""" Defines a running instance of a Murder Mystery.
Handles the initialization of the various game states.
"""
def __init__(self, abilities, players):
self.gm = players[0]
self.errorAbility = ErrorAbility()
self.eventQueue = PriorityQueue()
self.parser = Parser()
self.inbox = Inbox()
self.outbox = Outbox()
self.players = {}
for player in players[1:]:
self.players[player.getName().lower()] = player
self.abilities = {}
for ability in abilities:
self.abilities[ability.getName().lower()] = ability
def getGameMaster(self):
return self.gm
def addEvent(self, event):
""" Schedules an event to be run.
"""
self.eventQueue.put(event)
def addEvents(self, events):
""" Schedules a list of events to be run.
"""
if(events):
for event in events:
self.eventQueue.put(event)
def removeEvent(self, event):
""" Removes a specific event from the priority queue
"""
self.eventQueue.remove(event)
def getOutbox(self):
return self.outbox
def isValidAbility(self, name):
return name.lower() in self.abilities
def getAbility(self, name):
return self.abilities[name.lower()]
def getAbilityNames(self):
return self.abilities.keys()
def isValidPlayer(self, name):
return name.lower() in self.players
def getPlayer(self, name):
return self.players[name.lower()]
def getPlayerNames(self):
return self.players.keys()
def removePlayer(self, name):
if( self.isValidPlayer(name) ):
del self.players[name.lower()]
def run(self):
""" Run the game, and Don't stop.
Ever.
"""
print "Starting the main loop!"
while( True ):
self.step()
time.sleep(5)
def step(self):
""" Perform one step of the game logic.
You need to call this repeatedly to make the game run.
"""
#process incoming messages
newMessages = self.inbox.poll()
commands = self.parser.parse(self.abilities.values(), newMessages, self.errorAbility)
for (sender,ability,args) in commands:
print "Handling '"+ability.getName()+"' for '"+sender+"'"
for player in self.players.values():
if( player.getContact() == sender ):
print "\tRunning the ability!"
self.addEvents(ability.getEventsFor(self, player, args))
break
#Process the queue of events
while( not self.eventQueue.empty() ):
event = self.eventQueue.get()
if( event.when() < datetime.now() ):
print "Performing an event"
self.addEvents(event.perform(self))
else:
#Doesn't support peeking, so shove it back in the queue if it
#shouldn't happen yet
self.eventQueue.put(event)
break
def a_star(self, start, goal):
"""
A*
This is where the A* algorithum belongs
:param start: tuple of start pose
:param goal: tuple of goal pose
:return: dict of tuples
"""
# Transform start and end
start = map_helper.world_to_index2d(start, self.map)
goal = map_helper.world_to_index2d(goal, self.map)
# Priority queue for frontier
frontier = PriorityQueue()
frontier.put(start, 0)
# To find path at end
came_from = {}
# Track cost to reach each point
cost_so_far = {}
# Initially start
came_from[start] = None
cost_so_far[start] = 0
# Loop till finding a path
while not frontier.empty():
# Pop best cell from frontier
current = frontier.get()
rospy.logdebug("Current Node %s " % (current, ))
# Done!
if current == goal:
break
# Add neighbors to frontier
for next in map_helper.get_neighbors(current, self.map):
rospy.logdebug("Next node %s" % (next, ))
# Find cost
new_cost = cost_so_far[current] + self.move_cost(current, next)
if next not in cost_so_far or new_cost < cost_so_far[next]:
cost_so_far[next] = new_cost
priority = new_cost + self.euclidean_heuristic(goal, next)
frontier.put(next, priority)
came_from[next] = current
# Display frontier cells
frontier_list = frontier.get_items()
frontier_map = []
frontier_to_display = []
for point in frontier_list:
frontier_map.append(map_helper.world_to_index2d(point, self.map))
frontier_to_display.append(
map_helper.index2d_to_world(point, self.map))
# Print debug
rospy.logdebug("Frontier list: %s " % frontier_list)
# Generate path
path = [map_helper.index2d_to_world(goal, self.map)]
last_node = goal
while came_from[last_node] is not None:
next_node = came_from[last_node]
path.insert(0, map_helper.index2d_to_world(next_node, self.map))
last_node = next_node
new_path = self.optimize_path(path)
rospy.logdebug("Path %s " % new_path)
self.paint_wavefront(frontier_to_display)
self.paint_obstacles(new_path)
self.paint_cells(frontier_list, new_path)
self.points = new_path