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))
