def test_insert(self):
# Verify the value which is smaller than the root of min heap
# is new root after it is inserted
test_input = [
[2],
[3, 4],
[3, 5, 4],
[16, 14, 7, 2, 8, 1],
[12, 11, 6, 8, 10, 2, 3, 1],
[8, 3, 1, 5, 6, 4, 2]
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.build_heap()
new_min = min(test_input[i])-1
pq.insert(new_min)
self.assertEqual(pq.find_top(), new_min)
# Verify the value which is larger than the root of min heap
# is the leaf node after it is inserted
test_input = [
[2],
[3, 4],
[3, 5, 4],
[16, 14, 7, 2, 8, 1],
[12, 11, 6, 8, 10, 2, 3, 1],
[8, 3, 1, 5, 6, 4, 2]
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.build_heap()
new_max = max(test_input[i])+1
pq.insert(new_max)
self.assertEqual(pq.queue[pq.queue_size-1], new_max)
def test_PriorityQueue_parameters(self):
with self.assertRaises(Exception):
priority_queue.PriorityQueue(supplied_iterable=423515342282)
''' Just call do_something(), and if an exception gets raised,
the test automatically fails.
This is also the reason why assertDoesNotRaise() does not exist.
Reference:
http://stackoverflow.com/questions/6181555/
pass-a-unit-test-if-an-exception-isnt-thrown '''
priority_queue.PriorityQueue()
with self.assertRaises(Exception):
priority_queue.PriorityQueue(
supplied_iterable="Test string 12()3)\45")
def setUp(self):
self.empty_priority_queue = priority_queue.PriorityQueue()
# Test the supplied_iterable kwarg:
def craft_iterable():
# 10, 20, 30, 40, 50, 60, 70, 80
for each_number in range(10, 81, 10):
import random
random_value = random.randint(400, 500)
yield priority_queue.Node(random_value, each_number)
self.populated_priority_queue = priority_queue.PriorityQueue(
supplied_iterable=craft_iterable())
def __init__(self):
self.nodes, self.connections, self.level, self.index = [], [], 0, 0
self.last_node = None
# iterator stuff
self.current_node = None
self.queue = priority_queue.PriorityQueue()
self.seen = []
self.directed = -1
def astar_search(waternet_graph, start_vertex, goal_vertex):
current_node = AStarNode(start_vertex, None, None, 0)
# Check if the start is the goal state
if is_goal_state(current_node, goal_vertex):
return construct_route(current_node)
# Priority queue initialised with node and empty explored list
unexplored_nodes = priority_queue.PriorityQueue() #priority doesn't matter, right?
unexplored_nodes.push(current_node, 0)
explored_nodes = []
# Perform AStar search until the unexplored nodes is empty.
while not unexplored_nodes.isEmpty():
# Pop the current_node of the unexplored node list.
current_node = unexplored_nodes.pop()
explored_nodes.append(current_node)
# Check if this current node is the goal node
if is_goal_state(current_node, goal_vertex):
return construct_route(current_node)
# Get the neighbours of the current_node
for current_neighbour, edge_id in waternet_graph.neighbours(current_node.vertex):
#value = graph_helper.euclidean_distance(node.state, current_neighbour)*1000
current_edge = waternet_graph.edge(edge_id)
if isinstance(edge_id, basestring):
edge_id = int(edge_id.split("_")[0])
current_neighbour = AStarNode(current_neighbour, edge_id, current_node, (current_node.route_cost + current_node.vertex.euclidean_distance(current_neighbour) * current_edge.cost_function())) #initialise node with actual distance, successor pathcost + entire path cost
# Check if the neighbour is in the explored explored nodes
if is_explored_node(current_neighbour, explored_nodes):
continue
# Check if the neighbour already exists in the explored nodes list.
is_unexplored_neighbour, current_unexplored_route_cost = is_unexplored_node(current_neighbour, goal_vertex, unexplored_nodes)
# + amount(child.state, child.parent.state)
distance = current_neighbour.route_cost
# Check if it is not an unexplored node
if not is_unexplored_neighbour:
unexplored_nodes.push(current_neighbour, distance)
elif current_neighbour.route_cost < current_unexplored_route_cost:
# Replace in frontier heap only if the
for unexplored_node in unexplored_nodes.heap:
# Only check if the state of the unexplored node is equal to the current neighbour state.
if(unexplored_node[2].vertex == current_neighbour.vertex):
unexplored_nodes.heap.remove(unexplored_node)
unexplored_nodes.push(current_neighbour, distance)
break
return False
def __init__(self):
self.setup_options_and_constraints()
self.queue = priority_queue.PriorityQueue()
self.selector = motif_state_selector.default_selector()
self.scorer = motif_state_search_scorer.MTSS_Astar()
self.solutions = []
self.lookup = None
self.test_node = None
self.no_more_solutions = 0
def generic_a_star(self, moves, start, end, max_nodes=MAX_VALID_NODES):
"""
Performs an a* search on the map with the given set of moves
queue implementation stores the state which consists of
current position and estimated cost to end point. Based on
the estimated cost the queue is sorted. The store is indexed
on position and stores actual cost of reaching the point
along with parent point.
Note: end should be a valid point on the map for path to be found
Args:
moves: list of allowed moves at each point
start: x and y coordinates of start point
end: x and y coordinates of end point
max_nodes: max number of valid nodes to process
Returns:
store: contains the all the states encountered with links to parent states
it can be used to generate the path and the next step
"""
def total_cost(cur_pos, next_pos, end):
return self.movement_cost(cur_pos, next_pos) + self.heuristic_cost(
next_pos, end)
queue = priority_queue.PriorityQueue()
queue.push((self.heuristic_cost(start, end), start))
store = {start: (0, start)}
while not queue.is_empty():
(_, cur_pos) = queue.pop()
(cur_score, _) = store[cur_pos]
if cur_pos == end or max_nodes <= 0:
return store # return store on reaching end or when exhausted nodes
next_moves = [cur_pos + move for move in moves]
valid_moves = [
move for move in next_moves if self.is_valid_move(move)
]
max_nodes -= len(valid_moves)
possible_state = [(total_cost(cur_pos, move, end), move)
for move in valid_moves]
valid_state = [
(next_score, next_pos)
for (next_score, next_pos) in possible_state
if next_score <= cur_score + self.heuristic_cost(cur_pos, end)
]
update_state = [
state for state in valid_state if state[1] not in store
]
for (next_score, next_pos) in update_state:
store[next_pos] = (cur_score +
self.movement_cost(cur_pos, next_pos),
cur_pos)
queue.push((next_score, next_pos))
return {}
def testQueue(self):
queue_a = priority_queue.PriorityQueue()
queue_a.push(11)
queue_a.push(2)
queue_a.push(13)
first_item_removed = queue_a.pop()
queue_a.push(29)
self.assertEqual(first_item_removed, 13)
self.assertFalse(queue_a.is_empty())
self.assertEqual(queue_a.pop(), 29)
self.assertEqual(queue_a.pop(), 11)
self.assertEqual(queue_a.pop(), 2)
def test_delete(self):
# Verify each node of the result keeps proper position
# by checking heap property
test_input = [
[1, 2, 3, 4, 5, 6, 7],
[3, 4, 5],
[4, 9, 8, 11, 12, 13, 15],
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.delete(1)
check_priority_order(pq)
def test_heapify(self):
# Verify each node of the result keeps proper position
# by checking heap property
test_input = [
[2],
[5, 11, 8],
[7, 5, 6, 11, 8, 9, 10],
[8, 3, 1, 5, 6, 4, 2],
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.heapify(0)
check_priority_order(pq)
def search_shortest_dist(graph, start, end):
# Check if the graph is valid. If it is not, return error string.
if graph is None: return 'graph is null: Function Failed'
# Declare a queue and enqueue the starting node.
q = priority_queue.PriorityQueue()
q.put(start, 0)
curr_cost = {}
curr_cost[start] = 0
# Declare a string to count factory stops and int to count miles traversed
# Set it to blank space.
solution = ''
miles_traveled = 0
nodes_expanded = 0
# While the q is not empty, get the next node on the queue
# and check for the goal. If at the goal, return the
# string of stops and the nodes expanded. Else,
# expand the node to the queue.
prev = None
while not q.empty():
cur = q.get()
if prev == cur:
prev = cur
cur = q.get()
nodes_expanded = nodes_expanded + 1
end[1].add_comp(cur)
end[2].add_comp(cur)
end[3].add_comp(cur)
end[4].add_comp(cur)
end[5].add_comp(cur)
if prev is not None and prev != cur:
miles_traveled = miles_traveled + graph.cost(prev, cur)
if cur != start:
solution = solution + cur
#the widgets are complete, return
if end[1].done and end[2].done and end[3].done and end[4].done and end[
5].done:
return (solution, miles_traveled, nodes_expanded)
#add the neighbors to the queue in order of their cost plus current cost
for next in graph.neighbors(cur):
update_cost = curr_cost[cur] + graph.cost(cur, next)
#if the nighbor isn't in the queue or the new cost is less than it's original cost
if next not in curr_cost or update_cost < curr_cost[next]:
curr_cost[next] = update_cost
prior = update_cost + dist_hist(graph.weights[cur], end, next, 5)
q.put(next, prior)
prev = cur
# Return the failed because solution was not found
solution = 'FAILED'
miles_traveled = -1
return (solution, miles_traveled, nodes_expanded)
def search_smallest_stops(graph, start, end):
# Check if the graph is valid. If it is not, return error string.
if graph is None: return 'graph is null: Function Failed'
# Declare a queue and enqueue the starting node.
q = priority_queue.PriorityQueue()
q.put(start, 0)
curr_cost = {}
curr_cost[start] = 0
# Declare a string to count factory stops
# Set it to blank space.
solution = ''
nodes_expanded = 0
# While the q is not empty, get the next node on the queue
# and check for the goal. If at the goal, copy the successful
# path to the array of mazedata and then return 1. Else,
# expand the node to the queue.
prev = None
while not q.empty():
cur = q.get()
if prev == cur:
prev = cur
cur = q.get()
nodes_expanded = nodes_expanded + 1
end[1].add_comp(cur)
end[2].add_comp(cur)
end[3].add_comp(cur)
end[4].add_comp(cur)
end[5].add_comp(cur)
if cur != start:
solution = solution + cur
if end[1].done and end[2].done and end[3].done and end[4].done and end[
5].done:
return (solution, nodes_expanded)
for next in graph.neighbors(cur):
update_cost = curr_cost[cur] + graph.cost(cur, next)
if next not in curr_cost or update_cost < curr_cost[next]:
curr_cost[next] = update_cost
prior = curr_cost[next] + stop_hist(end, next, 5)
q.put(next, prior)
prev = cur
# Return the failed because solution was not found
solution = 'FAILED: ' + solution
return (solution, nodes_expanded)
def __iter__(self):
if len(self.nodes) != 0:
for n in self.nodes:
avail_pos = n.available_children_pos()
if len(avail_pos) != 0:
self.current_node = n
break
if self.current_node is None:
self.current_node = self.nodes[0]
else:
self.current_node = None
self.seen = [self.current_node]
self.queue = priority_queue.PriorityQueue()
self.directed = -1
return self
def test_build_heap(self):
# Verify each node of the result keeps proper position
# by checking heap property
test_input = [
[2],
[3, 4],
[3, 5, 4],
[16, 14, 7, 2, 8, 1],
[12, 11, 6, 8, 10, 2, 3, 1],
[8, 3, 1, 5, 6, 4, 2]
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.build_heap()
check_priority_order(pq)
def test_find_top(self):
# Verify the result of find_top method in min heap is
# same as the minimum of the array
test_input = [
[2],
[3, 4],
[3, 5, 4],
[16, 14, 7, 2, 8, 1],
[12, 11, 6, 8, 10, 2, 3, 1],
[8, 3, 1, 5, 6, 4, 2]
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.build_heap()
self.assertEqual(pq.find_top(), min(test_input[i]))
def __init__(self, circuit):
"""Creates a simulation that will run on a pre-built circuit.
The Circuit instance does not need to be completely built before it is
given to the class constructor. However, it does need to be complete
before the run method is called.
Args:
circuit: The circuit whose state transitions will be simulated.
"""
self.circuit = circuit
self.in_transitions = []
self.queue = pq.PriorityQueue()
self.probes = []
self.probe_all_undo_log = []
def test_extract_top(self):
# Verify the result of find_top method in min heap is
# same as the minimum of the array and it's properly removed
test_input = [
[2],
[3, 4],
[3, 5, 4],
[16, 14, 7, 2, 8, 1],
[12, 11, 6, 8, 10, 2, 3, 1],
[8, 3, 1, 5, 6, 4, 2]
]
for i in range(len(test_input)):
pq = priority_queue.PriorityQueue(test_input[i])
pq.build_heap()
min_value = min(test_input[i])
self.assertEqual(pq.extract_top(), min_value)
assert min_value not in pq.queue
def solve(self, problem):
done = set()
start = problem.startState()
goal_state = None
cache = {start:(0, start)}
pq = priority_queue.PriorityQueue()
pq.push(0, start)
while not pq.is_empty():
cost_to, s = pq.pop()
# once we pop a state skip it
if s in done:
continue
done.add(s)
# goal then must have traversed shortest path
if problem.isGoal(s):
self.cost, _ = cache[s]
goal_state = s
break
# add each ns to pq if cost lower now
for (a,ns,c) in problem.succAndCost(s):
total = cost_to + c
if ns not in cache:
cache[ns] = (total, s)
pq.push(total, ns)
elif cache[ns][0] > total:
cache[ns] = (total, s)
pq.push(total, ns)
# collect actions with backpointers
self.actions = [goal_state]
s = goal_state
while s != start:
ps = cache[s][1]
self.actions.append(ps)
s = ps
self.actions.reverse()
def short_path(graph, start, end):
queue = priority_queue.PriorityQueue(
lambda x, y: x['distance'] < y['distance'])
queue.enqueue({"name": start, "distance": 0, 'index': 0, 'path': ['A']})
seen = {}
while len(queue):
value = queue.dequeue()
seen[value['name']] = True
if value['name'] == end:
value.pop('name')
return value
for node in graph[value['name']]:
if node in seen:
continue
queue.enqueue({
'name':
node,
'distance':
value['distance'] + graph[value['name']][node],
'path':
value['path'] + [node]
})
return None
def __init__(self):
self.scheduling_queue = priority_queue.PriorityQueue()
self.priority_level = {"high": 3, "mid": 2, "low": 1}
self.flag_set = ["--once", "--daily", "--weekly", "--monthly"]
#!/usr/bin/env python
""" A first template for a PID controller """
import sys
sys.path.insert(0,'..')
import priority_queue as pq
### Initializing queue
# key and cost in range(0,5)
print('Initializing queue...')
queue = pq.PriorityQueue()
for vals in range(0,5):
queue.insert(vals,vals)
# Print queue
for elems in queue.queue:
print('%3d %3d'%(elems.key,elems.cost))
### Pop element
print('\nExtracting min key...')
min_val,idx = queue.min_extract()
print('Min key: {}, Min cost: {}'.format(min_val,idx))
# Print queue
for elems in queue.queue:
print('%3d %3d'%(elems.key,elems.cost))
### Check if is element
print('\nChecking if 5 is a key: {}'.format(queue.is_member(5)))
print('Checking if 3 is a key: {}'.format(queue.is_member(3)))
### Insert element
def test_compare(self):
pq = priority_queue.PriorityQueue([3, 4, 2])
self.assertEqual(pq.compare(pq.queue[0], pq.queue[1]), True)
self.assertEqual(pq.compare(pq.queue[1], pq.queue[2]), False)
def test_swap(self):
pq = priority_queue.PriorityQueue([3, 4])
pq.swap(0,1)
self.assertEqual(pq.queue, [4, 3])
def setUp(self):
self.pq = priority_queue.PriorityQueue(
['T', 'N', 'R', 'P', 'H', 'O', 'A', 'E', 'I', 'G'])
#os.system('cls')
print('\n')
print(str(round(((time.clock() - StartTime) / 60.0), 2)),
'minutes elapsed')
print(str(PMIDReadCount), 'results fetched,',
str(round(((PMIDReadCount / float(NumOfPMIDs)) * 100.0), 2)),
'% done')
print('URL Fetch rate', str(URLFetchCount - dFetchRate), 'per',
str(UpdateTimeInSec), 'sec,', str(URLFetchCount),
'total URLs fetched')
print('Total Proxies:', str(len(ProxyQueue.ItemList)))
ProxyQueue = pq.PriorityQueue()
ListCount = 0 # Python list, each item is string 'facebook_count|twitter_count'
skip_index = 0 # number of inputs PMIDs to ignore
ListPMID = [] # Python list, each entry is a PMID as a string
PMIDReadCount = skip_index # Number of PMIDs that have their count done so far
URLFetchCount = 0 # Number of successful URLs requests so far
NumOfPMIDs = 0 # Number of PMIDs
StartTime = 0 # start time of our program on clock
dFetchRate = 0 # delta fetch rate, to be used to trigger refresh of program performance stats after a specified time interval
NameInputFile = 'pubmed_test.txt'
NameOutputFile = NameInputFile[0:-4] + '_stats.' + NameInputFile[-3:]
BaseURLPubmed = 'https://www.ncbi.nlm.nih.gov/pubmed/'
def solve(state):
""" solves the 8-torus problem in the least amount of moves possible """
# run A* search algorithm
goal_state = [1, 2, 3, 4, 5, 6, 7, 8,
0] # this is the desired state we wish to reach
pq = priority_queue.PriorityQueue()
# stores the given state as a dictionary and stores it in our priority queue
state_dict = {
'state': state,
'h': calculate_heuristic(state),
'parent': None,
'g': 0,
'f': calculate_heuristic(state)
}
pq.enqueue(state_dict)
found = False
closed = [] # stores all 'closed' nodes
goal_dict = None # will store the dictionary of the desired goal state
# finds the goal state through the A* algorithm
while not pq.is_empty():
# pops the min-cost element from the queue, adds it to closed, and stores the state
curr_dict = pq.pop()
curr_state = curr_dict['state']
add_to_closed(closed, curr_dict)
# checks if goal_state is reached
if curr_state == goal_state:
goal_dict = curr_dict # stores the goal dictionary
break
# generates and enqueues the successor states as dictionaries
succ_states = generate_succ(curr_state)
for succ in succ_states:
enqueue = True
succ_dict = {
'state': succ,
'h': calculate_heuristic(succ),
'parent': curr_dict['state'],
'g': curr_dict['g'] + 1,
'f': calculate_heuristic(succ) + curr_dict['g'] + 1
}
for closed_dict in closed:
if (closed_dict['state'] == succ_dict['state']):
if (closed_dict['f'] < succ_dict['f']):
enqueue = False
if enqueue:
pq.enqueue(succ_dict)
prev = goal_dict # this is our goal that we have reached
moves = [prev] # stores the list of moves requires to solve the puzzle
# performs the second half of the A* algorithm by going through the closed list to produce a a list of moves
# required to reach the goal state
num_moves = int(goal_dict['f'])
for i in range(num_moves):
for curr in closed:
if curr['state'] == prev[
'parent']: # finds the parent state within the of the current state
prev = curr
# sets the previous state to the parent (current) state
moves.append(
curr) # adds the current (parent) to the moves list
break
# reverses the list of moves into the correct order
moves = reversed(moves)
# prints out the desired results
for x in moves:
print str(x['state']) + " h= " + str(x['h']) + " moves: " + str(x['g'])
print("Max queue length: " + str(pq.max_len))
self.name = name
self.age = age
def __eq__(self, other):
return self.age == other.age
def __lt__(self, other):
return self.age < other.age
def __repr__(self):
return '(' + str(self.name) + '; ' + str(self.age) + ')'
print("int MaxHeap:")
lst = []
q_int = q.PriorityQueue(lst, key=max, seq_type=int)
print("Checking is empty before assignment:", q_int.is_empty())
q_int.push(1)
q_int.push(2)
q_int.push(3)
q_int.push(4)
q_int.push(0)
q_int.push(100)
print("Checking is empty after assignment:", q_int.is_empty())
print("Heap size is", q_int.heap_size())
print(q_int, "is heap")
peeked = q_int.peek()
print(peeked, "is peeked element")
def a_star(self, mapdata, start, goal):
### REQUIRED CREDIT
rospy.loginfo("Executing A* from (%d,%d) to (%d,%d)" % (start[0], start[1], goal[0], goal[1]))
afrontier = priority_queue.PriorityQueue()
afrontier.put(start, 0)
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
path = []
the_frontier = GridCells()
the_frontier.header.frame_id = "map"
the_frontier.cell_width = mapdata.info.resolution
the_frontier.cell_height = mapdata.info.resolution
the_frontier.cells = []
while not afrontier.empty():
current = afrontier.get()
#rospy.loginfo("at point" + str(current))
if current == goal:
rospy.loginfo("found the goal")
break
current_x = current[0]
current_y = current[1]
for this_next in PathPlanner.neighbors_of_8(mapdata, current_x, current_y):
new_cost = cost_so_far[current] + PathPlanner.euclidean_distance(current_x, current_y, this_next[0], this_next[1])
if this_next not in cost_so_far or new_cost < cost_so_far[this_next]:
cost_so_far[this_next] = new_cost
priority = new_cost + PathPlanner.euclidean_distance(this_next[0], this_next[1], goal[0], goal[1])
afrontier.put(this_next, priority)
came_from[this_next] = current
msg_Point = Point()
world_point = PathPlanner.grid_to_world(mapdata, this_next[0], this_next[1])
msg_Point.x = world_point.x
msg_Point.y = world_point.y
msg_Point.z = 0
the_frontier.cells.append(msg_Point)
self.frontier.publish(the_frontier)
rospy.sleep(0.025)
the_path = GridCells()
the_path.header.frame_id = "map"
the_path.cell_width = mapdata.info.resolution
the_path.cell_height = mapdata.info.resolution
the_path.cells = []
rospy.loginfo("shit's goin down")
while 1 == 1:
msg_Point = Point()
world_point = PathPlanner.grid_to_world(mapdata, current[0], current[1])
msg_Point.x = world_point.x
msg_Point.y = world_point.y
msg_Point.z = 0
the_path.cells.append(msg_Point)
path.append(current)
previous_node = came_from[current]
if previous_node == None:
break
current = previous_node
rospy.loginfo(current)
self.path.publish(the_path)
return path
def a_star(self, mapdata, start, goal):
"""
Calculates the astar path.
Publishes the list of cells that were added to the path.
:param mapdata [OccupancyGrid] The map data.
:param start [int8,int8] The x,y grid location of the starting location
:param goal [int8,int8] The x,y grid location of the goal location
:return [[int8]] The C-Space as a list of path values.
"""
rospy.loginfo("Executing A* from (%d,%d) to (%d,%d)" %
(start[0], start[1], goal[0], goal[1]))
frontier_cells = []
unoptimized_path = []
visited_cells = []
#initializes dictionaries and lists
frontier = priority_queue.PriorityQueue()
frontier.put(start, 0)
came_from = {}
came_from[start] = None
cost_so_far = {}
cost_so_far[start] = 0
#only when not empty
while not frontier.empty():
current = frontier.get()
#have we reached?
if current == goal:
break
#current cell is turned into a Point class and added to the visited_cells list
visited_cell = Point()
world_pos_astar = PathPlanner.grid_to_world(
mapdata, current[0], current[1])
visited_cell.x = world_pos_astar[0]
visited_cell.y = world_pos_astar[1]
visited_cell.z = 0
visited_cells.append(visited_cell)
#finds neighbor list in order to start A* algorithm
list_of_8_neighbors = PathPlanner.neighbors_of_8(
mapdata, current[0], current[1])
#iterates through all 8 neighbors to determine cost at each node (f(n))
for next in list_of_8_neighbors:
next_x = next[0]
next_y = next[1]
#h(n) cost from neighbor. Heuristic is euclidiean distance
new_cost = cost_so_far[
current] + PathPlanner.euclidean_distance(
next_x, next_y, current[0], current[1])
#checks to see how the cost of the next neighbor compares to the visited_cell
if next not in cost_so_far or new_cost < cost_so_far[next]:
#updates cost_so_far[] dictionary to include new neighbor
cost_so_far[next] = new_cost
priority = new_cost + PathPlanner.euclidean_distance(
next_x, next_y, goal[0], goal[1])
frontier.put(next, priority)
#generates list of frontier_cell Points to use in rviz
frontier_cell = Point()
world_pos_astar = PathPlanner.grid_to_world(
mapdata, next[0], next[1])
frontier_cell.x = world_pos_astar[0]
frontier_cell.y = world_pos_astar[1]
frontier_cell.z = 0
frontier_cells.append(frontier_cell)
came_from[next] = current
#publishes frontier in rviz
frontier_cells_message = GridCells()
frontier_cells_message.header.frame_id = 'map'
frontier_cells_message.cell_width = mapdata.info.resolution
frontier_cells_message.cell_height = mapdata.info.resolution
frontier_cells_message.cells = frontier_cells
## Return the C-space
self.frontier.publish(frontier_cells_message)
#generates list of visited cells and publishes to rviz
visited_cells_message = GridCells()
visited_cells_message.header.frame_id = 'map'
visited_cells_message.cell_width = mapdata.info.resolution
visited_cells_message.cell_height = mapdata.info.resolution
visited_cells_message.cells = visited_cells
## Return the C-space
self.expanded_cells.publish(visited_cells_message)
node = goal
#Iterates through came_from[] dictionary backwards to find A* path
while node != start:
node = came_from[node]
unoptimized_path_cell = Point()
world_pos_unoptimized_path = PathPlanner.grid_to_world(
mapdata, node[0], node[1])
unoptimized_path_cell.x = world_pos_unoptimized_path[0]
unoptimized_path_cell.y = world_pos_unoptimized_path[1]
unoptimized_path_cell.z = 0
unoptimized_path.append(unoptimized_path_cell)
# message is published and displayed in rviz
unoptimized_path_message = GridCells()
unoptimized_path_message.header.frame_id = 'map'
unoptimized_path_message.cell_width = mapdata.info.resolution
unoptimized_path_message.cell_height = mapdata.info.resolution
unoptimized_path_message.cells = unoptimized_path
## Return the C-space
return unoptimized_path
