def _start_populate(self, threshold =50):
"""
This function publishes the walls and unexplored maps to the 'self._wall' and 'self._not_explored_nodes'
publishers. This gives the map a basis to view what is occuring in realtime.
This is used as a startup function, and as a base paint function.
:param threshold: This is the value that is used to discern a wall vs. unknown space
:return: None
"""
# logging.info("Configuring for publishing")
grid = self._map
wall_cells = tools.makeGridCells('map',0.3,0.3)
not_explored = tools.makeGridCells('map',0.3,0.3)
self.wall_list =[]; self.not_explored_list = []
for i in range(0,self._height): #height should be set to hieght of grid
for j in range(0,self._width): #height should be set to hieght of grid
point = tools.makePoint(j,i,0)
if (self._map[i][j] > threshold ):
self.wall_list.append(point)
else:
self.not_explored_list.append(point)
wall_cells.cells = self.wall_list
not_explored.cells = self.not_explored_list
self._walls.publish(wall_cells)
rospy.sleep(0.1)
self._not_explored_nodes.publish(not_explored)
rospy.sleep(0.1)
def aStarSearch(self, start):
"""
This function generates a dictionary of paths where the shortest path can be found through
traversing from the goal back to the start.
This version of A* uses tools.distFormula as the heuristic. Could be changed out.
:param start: (x,y) touple of the location in the grid.
:return: Dictionary of <end point> : <starting Point> as key,value
"""
frontier = Queue.PriorityQueue()
frontier.put((0, start)) ## Put takes a touple of priority, value
came_from = {}
cost_so_far = {}
came_from[start] = None
cost_so_far[start] = 0
while not frontier.empty():
current_touple = frontier.get() ## Returns the (priority, (x,y)
_,current = current_touple ## sets current to (x,y)
x,y = current
if x == self.goalX and y == self.goalY:
break #We're there!
#unpack current
neighbors = self.getNeighbors(x,y)
# print neighbors
for next in neighbors: ## Get list of touples (points)
# print "AStar",next,current
cost_next = cost_so_far[current] + tools.distFormula(current, next)
if next not in cost_so_far or cost_so_far[next] > cost_next:
if next not in cost_so_far:
frontier.put((cost_next + tools.distFormula(next, self._goal_pos), next))## Put takes a touple of priority, value
nx,ny = next
self.explored_nodes_list.append(tools.makePoint(nx,ny,0))
self._repaint_map()
cost_so_far[next] = cost_next
came_from[next] = current
if frontier.empty():
print "It's empty!!!!!"
rospy.sleep(1)
return came_from
