def displayProgress(e_array, f_array): global explored_pub global frontier_pub global map cells = GridCells() cells.cell_height = map.info.resolution cells.cell_width = map.info.resolution cells.header.frame_id = "map" #publish explored cells array = [] for i in e_array: x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y) array.append(Point(x,y,0)) cells.cells = array explored_pub.publish(cells) #publish frontier cells array = [] for i in f_array: x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y) array.append(Point(x,y,0)) cells.cells = array frontier_pub.publish(cells)
def displayPath(p_array): global path_pub global map global arrayTravelled cells = GridCells() cells.cell_height = map.info.resolution cells.cell_width = map.info.resolution cells.header.frame_id = "map" #publish path cells array = [] for i in p_array: x = (i.x + map.info.width+0.5)*map.info.resolution + map.info.origin.position.x y = -((i.y+0.5)*map.info.resolution - map.info.origin.position.y) array.append(Point(x,y,0)) cells.cells = array path_pub.publish(cells)