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