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 _repaint_map(self, baseMap=False, path=False): """ This repaints the map based off the path from A* and the explored regions. This is also run if the map is updated through 'self._updateMap(self, msg)' :return: """ if baseMap: self._start_populate() self._path.publish(tools.makeGridCells('/map',0.3,0.3,self.path_list)) if path: self._path.publish(tools.makeGridCells('/map',0.3,0.3,self.path_list)) for i in xrange(10): self._explored_nodes.publish(tools.makeGridCells('/map',0.3,0.3,self.explored_nodes_list)) self._waypoints.publish(tools.makeGridCells('/map',0.3,0.3,self.waypoint_list))