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)
def read_map(msg):
global goal_pub
mapResolution = msg.info.resolution
mapWidth = msg.info.width
inputArray = msg.data
result = GridCells();
n = 0
#print "expanding obstacles"
print "searching for new frontier cells"
numExpansions = 1
freeSpaceValue = 50
for n in range(len(inputArray)):
#look at unknown cells
if inputArray[n] == -1:
#determine L bound
if n%mapWidth < numExpansions:
L = n%mapWidth
else:
L = numExpansions
#determine Upper bound
if n/mapWidth < numExpansions:
D = trunc(n/mapWidth)
else:
D = numExpansions
#determine R bound
if mapWidth - n%mapWidth - 1 < numExpansions:
R = mapWidth - n%mapWidth - 1
else:
R = numExpansions
#determine depth
if len(inputArray) - n <= numExpansions * mapWidth:
U = trunc((len(inputArray) - n - 1)/mapWidth)
else:
U = numExpansions
n2 = 0
n3 = 0
stop = 0
for n2 in range(int(U+D+1)):
for n3 in range(int(L+R+1)):
tempLoc = int(n + mapWidth * (U - n2) + (n3 - L))
#check if this unknown cell is next to free space
if inputArray[tempLoc] != -1 and inputArray[tempLoc] < freeSpaceValue:
result.cells.append(Point(n%mapWidth, int(n/mapWidth), 0))
stop = 1
break
if stop == 1:
break
#convert to position
for i in result.cells:
i.x = (i.x+0.5) * mapResolution + msg.info.origin.position.x
i.y = (i.y+0.5) * mapResolution + msg.info.origin.position.y
#now publish the list of frontier cells
result.header = msg.header
result.cell_height = mapResolution
result.cell_width = mapResolution
goal_pub.publish(result)
