def enclosed(curr_pos, depth=0):
depth += 1
if (curr_pos[0] < 0 or curr_pos[0] >= constants.NUM_CHUNKS or curr_pos[1] < 0 or curr_pos[1] >= constants.NUM_CHUNKS
or (visited_map[curr_pos[0]][curr_pos[1]] == 1)
or constants.MapData(data_map[curr_pos[0]][curr_pos[1]]) == constants.MapData.WALL
or constants.MapData(data_map[curr_pos[0]][curr_pos[1]]) == constants.MapData.AVOID
or constants.MapData(data_map[curr_pos[0]][curr_pos[1]]) == constants.MapData.WALL_AVOID
or depth > 2950):
return 0
else:
visited_map[curr_pos[0]][curr_pos[1]] = 1
return 1 + enclosed([curr_pos[0]-1, curr_pos[1]], depth) + enclosed([curr_pos[0]+1, curr_pos[1]], depth) + enclosed([curr_pos[0], curr_pos[1]-1], depth) + enclosed([curr_pos[0], curr_pos[1]+1], depth)
def nextToWall(row, col, data_map):
'''
Return true if given position is next to wall in data_map
'''
for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
node_position = (row + new_position[0], col + new_position[1])
row = node_position[0]
col = node_position[1]
if (constants.MapData(data_map[row][col])
== constants.MapData.WALL) or (constants.MapData(
data_map[row][col]) == constants.MapData.AVOID):
return True
return False
def findWalls(self):
'''
Defines all pixels with <127 value to be a wall.
'''
temp = [[0 if ((constants.MapData(self.data_map[i][j]) == constants.MapData.WALL) or
(constants.MapData(self.data_map[i][j]) == constants.MapData.WALL_AVOID)) else
constants.MapData(self.data_map[i][j]) for j in range(constants.NUM_CHUNKS)]
for i in range(constants.NUM_CHUNKS)]
self.data_map = temp
for i in range(constants.NUM_CHUNKS):
for j in range(constants.NUM_CHUNKS):
if( self.compressed_map[i][j] < 127):
self.data_map[i][j] = constants.MapData.WALL
self.addAvoidsAround(i,j)
def addAvoidsAround(self,i,j):
for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
node_position = (i + new_position[0], j + new_position[1])
row = node_position[0]
col = node_position[1]
if ( (node_position[0] >=0) and (node_position[0] < constants.NUM_CHUNKS) and (node_position[1] >=0) and (node_position[1] < constants.NUM_CHUNKS)):
if (constants.MapData(self.data_map[row][col]) != constants.MapData.WALL):
self.data_map[row][col] = constants.MapData.WALL_AVOID
def printIncremental(self, robot_pos, dest, index):
'''
Overlay data_map on compressed_map in color, and save to a .png file.
'''
# print(self.data_map)
im = PIL.Image.new(mode="RGB", size=(
constants.NUM_CHUNKS, constants.NUM_CHUNKS))
pixels = im.load()
for i in range(im.size[0]):
for j in range(im.size[1]):
datum = constants.MapData(self.data_map[i][j])
if(datum != constants.MapData.NULL):
# paint data
if(datum == constants.MapData.WALL):
# indices reversed for image
pixels[j, i] = (0, 0, 255)
elif(datum == constants.MapData.PATH):
pixels[j, i] = (255, 255, 0)
elif(datum == constants.MapData.FILL):
pixels[j, i] = (255, 192, 203)
elif(datum == constants.MapData.AVOID):
pixels[j, i] = (255, 165, 0)
elif(datum == constants.MapData.WALL_AVOID):
pixels[j, i] = (204, 85, 0)
else:
# paint map
pixel = self.compressed_map[i][j]
pixels[j, i] = (pixel, pixel, pixel)
# paint robot_pos and dest
pixels[robot_pos[1], robot_pos[0]] = (255, 0, 0)
pixels[dest[1], dest[0]] = (0, 255, 0)
# draw scale in bottom right
draw = ImageDraw.Draw(im)
y1 = constants.NUM_CHUNKS * 0.9
y2 = constants.NUM_CHUNKS * 0.9
x2 = constants.NUM_CHUNKS * 0.9
x1 = x1 - (constants.MAP_SIZE / constants.MAP_SIZE_METERS)
draw.line([(x1, y1), (x2, y2)], fill = "none", width = 1)
draw.text((x1, y1), "1 meter", font=ImageFont.load_default())
path = './resources/overlay_map_{index}.png'
im.save(path.format(index = index))
'''
Save byte_map to a .pgm file in the /resources folder.
'''
path = './resources/byte_map_{index}.pgm'
pgm_save(path.format(index = index), self.byte_map,
(constants.MAP_SIZE, constants.MAP_SIZE))
def chooseDestination(self, robot_pos, badDestList):
'''
Performs a radially outward expanding search from current robot pos for
a square that fits the defined destination threshold for exploredness.
'''
for distance in range(constants.MIN_SEARCH, constants.MAX_SEARCH):
for row_offset in range(-distance, distance):
cur_row = robot_pos[0] + row_offset
if(cur_row < 0 or cur_row >= constants.NUM_CHUNKS):
continue
for col_offset in range(-distance, distance):
cur_col = robot_pos[1] + col_offset
if(cur_col < 0 or cur_col >= constants.NUM_CHUNKS):
continue
if((self.compressed_map[cur_row][cur_col] < constants.DEST_THRESHOLD) and
constants.MapData(self.data_map[cur_row][cur_col]) != constants.MapData.WALL and
constants.MapData(self.data_map[cur_row][cur_col]) != constants.MapData.AVOID and
constants.MapData(self.data_map[cur_row][cur_col]) != constants.MapData.WALL_AVOID and
(cur_row,cur_col) not in badDestList):
return cur_row, cur_col
return None
def astar(maze, start, end):
# create start/end nodes, open/closed lists
start_node = Node(None, start)
start_node.g = start_node.h = start_node.f = 0
end_node = Node(None, end)
end_node.g = end_node.h = end_node.f = 0
open = []
closed = []
open.append(start_node)
while len(open) > 0:
# get current node
current_node = open[0]
current_index = 0
for index, item in enumerate(open):
if item.f < current_node.f:
current_node = item
current_index = index
# Pop current off open list, add to closed list
open.pop(current_index)
closed.append(current_node)
# Found the goal
if (current_node.position[0]
== end_node.position[0]) and (current_node.position[1]
== end_node.position[1]):
path = []
current = current_node
while current is not None:
path.append(current.position)
current = current.parent
return path[::-1] # Return reversed path
# Generate children
children = []
# Adjacent squares
for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
node_position = (current_node.position[0] + new_position[0],
current_node.position[1] + new_position[1])
# Make sure in range
if node_position[0] > (
len(maze) -
1) or node_position[0] < 0 or node_position[1] > (
len(maze[len(maze) - 1]) - 1) or node_position[1] < 0:
continue
# Make sure not a wall or obstacle
if ((constants.MapData(maze[node_position[0]][node_position[1]])
== constants.MapData.WALL) or
(constants.MapData(maze[node_position[0]][node_position[1]])
== constants.MapData.AVOID) or
(constants.MapData(maze[node_position[0]][node_position[1]])
== constants.MapData.WALL_AVOID)):
continue
# Make sure it's not next to a wall
# if (nextToWall(node_position[0], node_position[1], maze)):
# continue
new_node = Node(current_node, node_position)
children.append(new_node)
# add children only if not in closed and open
for child in children:
# check if child is in closed list
add = True
for closed_child in closed:
if (child.position[0] == closed_child.position[0]) and (
child.position[1] == closed_child.position[1]):
add = False
child.g = current_node.g + 1
child.h = ((child.position[0] - end_node.position[0])**2) + (
(child.position[1] - end_node.position[1])**2)
child.f = child.g + child.h
# check if child is in the open list
for open_node in open:
if (child.position[0] == open_node.position[0]) and (
child.position[1] == open_node.position[1]):
# and child.g >= open_node.g
add = False
if (add):
open.append(child)