def test_bad_heuristic():
# For valid heuristics, the cost to move must be at least 1.
weights = (1. + 5. * np.random.random((10, 10))).astype(np.float32)
# An element smaller than 1 should raise a ValueError.
bad_cost = np.random.random() / 2.
weights[4, 4] = bad_cost
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 0), (9, 9))
assert '.f' % bad_cost in exc.value.args[0]
def test_no_solution():
# Vertical wall.
weights = np.ones((5, 5), dtype=np.float32)
weights[:, 2] = np.inf
path = pyastar.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True)
assert not path
# Horizontal wall.
weights = np.ones((5, 5), dtype=np.float32)
weights[2, :] = np.inf
path = pyastar.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True)
assert not path
def test_narrow():
# Column weights.
weights = np.ones((5, 1), dtype=np.float32)
path = pyastar.astar_path(weights, (0, 0), (4, 0))
expected = np.array([[0, 0], [1, 0], [2, 0], [3, 0], [4, 0]])
assert np.all(path == expected)
# Row weights.
weights = np.ones((1, 5), dtype=np.float32)
path = pyastar.astar_path(weights, (0, 0), (0, 4))
expected = np.array([[0, 0], [0, 1], [0, 2], [0, 3], [0, 4]])
assert np.all(path == expected)
def test_match_reverse():
# Might fail if there are multiple paths, but this should be rare.
h, w = 25, 25
weights = (1. + 5. * np.random.random((h, w))).astype(np.float32)
fwd = pyastar.astar_path(weights, (0, 0), (h - 1, w - 1))
rev = pyastar.astar_path(weights, (h - 1, w - 1), (0, 0))
assert np.all(fwd[::-1] == rev)
fwd = pyastar.astar_path(weights, (0, 0), (h - 1, w - 1),
allow_diagonal=True)
rev = pyastar.astar_path(weights, (h - 1, w - 1), (0, 0),
allow_diagonal=True)
assert np.all(fwd[::-1] == rev)
def test_small():
weights = np.array([[1, 3, 3, 3, 3], [2, 1, 3, 3, 3], [2, 2, 1, 3, 3],
[2, 2, 2, 1, 3], [2, 2, 2, 2, 1]],
dtype=np.float32)
# Run down the diagonal.
path = pyastar.astar_path(weights, (0, 0), (4, 4), allow_diagonal=True)
expected = np.array([[0, 0], [1, 1], [2, 2], [3, 3], [4, 4]])
assert np.all(path == expected)
# Down, right, down, right, etc.
path = pyastar.astar_path(weights, (0, 0), (4, 4), allow_diagonal=False)
expected = np.array([[0, 0], [1, 0], [1, 1], [2, 1], [2, 2], [3, 2],
[3, 3], [4, 3], [4, 4]])
assert np.all(path == expected)
def main():
# joystick = pygame.joystick.Joystick(0)
# joystick.init()
clock = pygame.time.Clock()
pygame.display.set_caption('World Visualizer')
screen = pygame.display.set_mode([DIMENSION_IN, DIMENSION_IN])
done = False
rtenable = False
selected = OBSTACLES[0] if OBSTACLES else None
while not done:
print('Start frame', time.time())
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
keys = pygame.key.get_pressed()
if keys[pygame.K_w]:
selected.y -= 4
if keys[pygame.K_a]:
selected.x -= 4
if keys[pygame.K_s]:
selected.y += 4
if keys[pygame.K_d]:
selected.x += 4
if keys[pygame.K_f]:
for obstacle in OBSTACLES:
if obstacle != selected:
selected = obstacle
break
if keys[pygame.K_e]:
rtenable = False
elif keys[pygame.K_r]:
rtenable = True
matrix = np.full(fill_value=1,
shape=(DIMENSION_IN, DIMENSION_IN),
dtype=np.float32)
screen.fill(WHITE)
draw_frame(screen, matrix, selected)
if rtenable:
path = pyastar.astar_path(matrix,
(DIMENSION_IN // 2, DIMENSION_IN // 2),
(DIMENSION_IN - 1, DIMENSION_IN // 2))
for x, y in path:
pygame.draw.circle(screen, RED, [int(x), int(y)], int(2), 0)
pygame.display.flip()
print('End frame', time.time())
clock.tick(60)
def planning(self, map, start, goal): # OccupancyGrid Pose Pose
map_array = self.map_to_array(map)
start = self.map_to_grid_cord(map.info, start.position.x,
start.position.y)
goal = self.map_to_grid_cord(map.info, goal.position.x,
goal.position.y)
weights = np.asarray(map_array, dtype=np.float32)
tmp_path = pyastar.astar_path(weights,
start,
goal,
allow_diagonal=True)
'''
for y in range(0, len(map_array)):
for x in range(0, len(map_array[y])):
if map_array[y][x] >=50:
print "X",
else:
print "-",
print("")
for y in range(0, len(map_array)):
for x in range(0, len(map_array[y])):
if x==start[0] and y == start[1]:
print "S",
elif x==goal[0] and y == goal[1]:
print "G",
elif [y, x] in tmp_path:
print "P",
elif weights[y][x] >=50:
print "X",
else:
print "-",
print("")
print("================")
'''
# The start and goal coordinates are in matrix coordinates (i, j).
path = []
for i in tmp_path:
tmp = (i[0] * map.info.resolution + map.info.origin.position.x,
i[1] * map.info.resolution + map.info.origin.position.y)
path.append(tmp)
return path
def main():
args = parse_args()
maze = imageio.imread(args.input)
if maze is None:
print(f"No file found: {args.input}")
return
else:
print(f"Loaded maze of shape {maze.shape} from {args.input}")
grid = maze.astype(np.float32)
grid[grid == 0] = np.inf
grid[grid == 255] = 1
assert grid.min() == 1, "cost of moving must be at least 1"
# start is the first white block in the top row
start_j, = np.where(grid[0, :] == 1)
start = np.array([0, start_j[0]])
# end is the first white block in the final column
end_i, = np.where(grid[:, -1] == 1)
end = np.array([end_i[0], grid.shape[0] - 1])
t0 = time.time()
# set allow_diagonal=True to enable 8-connectivity
path = pyastar.astar_path(grid, start, end, allow_diagonal=False)
dur = time.time() - t0
if path.shape[0] > 0:
print(f"Found path of length {path.shape[0]} in {dur:.6f}s")
maze = np.stack((maze, maze, maze), axis=2)
maze[path[:, 0], path[:, 1]] = (255, 0, 0)
print(f"Plotting path to {args.output}")
imageio.imwrite(args.output, maze)
else:
print("No path found")
print("Done")
def main():
maze = cv2.imread(MAZE_FPATH)
if maze is None:
print('no file found: %s' % (MAZE_FPATH))
return
else:
print('loaded maze of shape %r' % (maze.shape[0:2], ))
grid = cv2.cvtColor(maze, cv2.COLOR_BGR2GRAY).astype(np.float32)
grid[grid == 0] = np.inf
grid[grid == 255] = 1
assert grid.min() == 1, 'cost of moving must be at least 1'
# start is the first white block in the top row
start_j, = np.where(grid[0, :] == 1)
start = np.array([0, start_j[0]])
# end is the first white block in the final column
end_i, = np.where(grid[:, -1] == 1)
end = np.array([end_i[0], grid.shape[0] - 1])
t0 = time()
# set allow_diagonal=True to enable 8-connectivity
path = pyastar.astar_path(grid, start, end, allow_diagonal=False)
dur = time() - t0
if path.shape[0] > 0:
print('found path of length %d in %.6fs' % (path.shape[0], dur))
maze[path[:, 0], path[:, 1]] = (0, 0, 255)
print('plotting path to %s' % (OUTP_FPATH))
cv2.imwrite(OUTP_FPATH, maze)
else:
print('no path found')
print('done')
def test_invalid_start_and_goal():
weights = (1. + 5. * np.random.random((10, 10))).astype(np.float32)
# Test bad start indices.
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (-1, 0), (9, 9))
assert '-1' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (10, 0), (9, 9))
assert '10' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, -1), (9, 9))
assert '-1' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 10), (9, 9))
assert '10' in exc.value.args[0]
# Test bad goal indices.
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 0), (-1, 9))
assert '-1' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 0), (10, 9))
assert '10' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 0), (0, -1))
assert '-1' in exc.value.args[0]
with pytest.raises(ValueError) as exc:
pyastar.astar_path(weights, (0, 0), (0, 10))
assert '10' in exc.value.args[0]
def test_bad_weights_dtype():
weights = np.array([[1, 2, 3], [1, 2, 3], [1, 2, 3]], dtype=np.float64)
with pytest.raises(AssertionError) as exc:
pyastar.astar_path(weights, (0, 0), (2, 2))
assert "float64" in exc.value.args[0]
def main(food):
# maze = cv2.imread(MAZE_FPATH)
# if maze is None:
# print('no file found: %s' % (MAZE_FPATH))
# return
# else:
# print('loaded maze of shape %r' % (maze.shape[0:2],))
# grid = cv2.cvtColor(maze, cv2.COLOR_BGR2GRAY).astype(np.float32)
grid_size = (32, 35)
grid = np.ones(grid_size, dtype=np.float32)
maze = np.ones((grid_size[0], grid_size[1], 3), dtype=np.float32)
for row_rng, col_rng in shelves:
row_lower = row_rng[0]
row_upper = row_rng[1]
col_lower = col_rng[0]
col_upper = col_rng[1]
if row_upper == row_lower:
for j in range(col_lower, col_upper):
grid[row_upper, j] = np.inf
maze[row_upper, j] = (0, 0, 0)
elif col_upper == col_lower:
for i in range(row_lower, row_upper):
grid[i, col_upper] = np.inf
maze[i, col_upper] = (0, 0, 0)
else:
for i in range(row_lower, row_upper):
for j in range(col_lower, col_upper):
grid[i, j] = np.inf
maze[i, j] = (0, 0, 0)
assert grid.min() == 1, 'cost of moving must be at least 1'
output = {'shelves': maze.tolist()
, 'size': grid_size}
# start is the first white block in the top row
start_j, = np.where(grid[-1, :] == 1)
start = tuple(np.array([0, start_j[0]]))
# end is the first white block in the final column
# end_i, = np.where(grid[:, -1] == 1)
# end = np.array([end_i[0], grid.shape[0] - 1])
ends = []
if isinstance(food, str):
ends.append(locs[food])
elif isinstance(food, list):
for item in food:
item_name = item.lower()
if item_name not in list(locs.keys()):
raise ValueError('Unsupported food item, supported foods are %s'
% ', '.join(locs.keys()))
else:
ends.append(locs[item])
def find_closest(point, ends):
shortest_dist = float('inf')
for end in ends:
dist = np.linalg.norm(np.array(end) - np.array(point))
if dist == 0:
continue
else:
if dist < shortest_dist:
shortest_dist = dist
closest_point = end
return closest_point
order = {}
ends_scrap = [start, *ends.copy()]
end = start
for i in range(len(ends_scrap)-1):
closest = find_closest(end, ends_scrap)
ends_scrap.remove(end)
order[end] = closest
end = closest
# set allow_diagonal=True to enable 8-connectivity
path_color = (1, 1, 0)
for end in ends:
# food_item = [key for key, value in locs.items() if value == end][0]
path = pyastar.astar_path(grid, start, end, allow_diagonal=False)
start = end
output['path'] = path.tolist()
if path.shape[0] > 0:
maze[path[:, 0], path[:, 1]] = maze[path[:, 0], path[:, 1]] - path_color
# print('plotting path to %s' % (food_item))
else:
pass
# print('no path found')
path_color = (random.uniform(0.1, 0.5),
random.uniform(0.1, 0.3),
random.uniform(0.1, 0.5)
)
maze_out = np.repeat(maze, 20, axis=0)
maze_out = np.repeat(maze_out, 20, axis=1)
maze_out = np.flipud(maze_out)
for i in range(grid_size[0]):
for j in range(grid_size[1]):
maze[i, j] = maze[i, j] * 255
output['map'] = maze.tolist()
# img = Image.fromarray(maze.astype('uint8'))
# output['image'].show()
return output
def main():
global mouseX
global mouseY
#maze = cv2.imread(MAZE_FPATH)
raw_img = cv2.imread(RAW_FPATH)
raw_img_copy = cv2.imread(RAW_FPATH)
maze = cv2.blur(raw_img,(10,10))
# maze = cv2.resize(maze_huge, (1000, 1000))
#cv2.imwrite("modified.png", maze)
if maze is None:
print('no file found: %s' % (RAW_FPATH))
return
else:
print('loaded maze of shape %r' % (maze.shape[0:2],))
grid = cv2.cvtColor(maze, cv2.COLOR_BGR2GRAY).astype(np.float32)
grid[grid == 0] = 1
grid[grid == 255] = 1
assert grid.min() == 1, 'cost of moving must be at least 1'
print (grid)
# start is the first white block in the top row
start_j, = np.where(grid[0, :] == 1)
print (start_j)
start = np.array([0, start_j[len(start_j)/2]])
print ("start at : ", start)
# end is the first white block in the final column
end_i, = np.where(grid[grid.shape[0]-1,:] == 1)
print (end_i)
end = np.array([grid.shape[0] - 1, end_i[len(end_i)/2]])
print("end at : ", end)
t0 = time()
path = pyastar.astar_path(grid, start, end)
print path[50 : 60]
dur = time() - t0
if path.shape[0] > 0:
print('found path of length %d in %.6fs' % (path.shape[0], dur))
while(1):
cv2.namedWindow('RESULT')
cv2.setMouseCallback('RESULT',draw_circle)
sleep(0.1)
print(mouseY, mouseX)
cv2.imshow("RESULT", raw_img)
if (cv2.waitKey(20)==113):
start = np.array([mouseY, mouseX])
path = pyastar.astar_path(grid, start, end)
for i in range(grid.shape[0]):
for j in range(grid.shape[1]):
if(raw_img[i,j][2] == 255):
raw_img[i,j] = (255,255,255)
raw_img[path[:, 0], path[:, 1]] = (0, 0, 255)
raw_img[path[:, 0]-1, path[:, 1]] = (0, 0, 255)
raw_img[path[:, 0], path[:, 1]-1] = (0, 0, 255)
#raw_img = raw_img_copy[:]
#break
print('plotting path to %s' % (OUTP_FPATH))
cv2.imwrite(OUTP_FPATH, raw_img)
#while(1):
else:
print('no path found')
print('done')
def get_short_term_action(self,inputs):
actions = []
temp_map = self.gt_map.astype(np.float32)
temp_map[temp_map == 2] = np.inf
for i in range(self.agent_num):
goal = [inputs[i][1], inputs[i][0]]
agent_pos = [self.agent_pos[i][1], self.agent_pos[i][0]]
agent_dir = self.agent_dir[i]
path = pyastar.astar_path(temp_map, agent_pos, goal, allow_diagonal=False)
if len(path) == 1:
actions.append(1)
continue
relative_pos = np.array(path[1]) - np.array(agent_pos)
# first quadrant
if relative_pos[0] < 0 and relative_pos[1] > 0:
if agent_dir == 0 or agent_dir == 3:
actions.append(2) # forward
continue
if agent_dir == 1:
actions.append(0) # turn left
continue
if agent_dir == 2:
actions.append(1) # turn right
continue
# second quadrant
if relative_pos[0] > 0 and relative_pos[1] > 0:
if agent_dir == 0 or agent_dir == 1:
actions.append(2) # forward
continue
if agent_dir == 2:
actions.append(0) # turn left
continue
if agent_dir == 3:
actions.append(1) # turn right
continue
# third quadrant
if relative_pos[0] > 0 and relative_pos[1] < 0:
if agent_dir == 1 or agent_dir == 2:
actions.append(2) # forward
continue
if agent_dir == 3:
actions.append(0) # turn left
continue
if agent_dir == 0:
actions.append(1) # turn right
continue
# fourth quadrant
if relative_pos[0] < 0 and relative_pos[1] < 0:
if agent_dir == 2 or agent_dir == 3:
actions.append(2) # forward
continue
if agent_dir == 0:
actions.append(0) # turn left
continue
if agent_dir == 1:
actions.append(1) # turn right
continue
if relative_pos[0] == 0 and relative_pos[1] ==0:
# turn around
actions.append(1)
continue
if relative_pos[0] == 0 and relative_pos[1] > 0:
if agent_dir == 0:
actions.append(2)
continue
if agent_dir == 1:
actions.append(0)
continue
else:
actions.append(1)
continue
if relative_pos[0] == 0 and relative_pos[1] < 0:
if agent_dir == 2:
actions.append(2)
continue
if agent_dir == 1:
actions.append(1)
continue
else:
actions.append(0)
continue
if relative_pos[0] > 0 and relative_pos[1] == 0:
if agent_dir == 1:
actions.append(2)
continue
if agent_dir == 0:
actions.append(1)
continue
else:
actions.append(0)
continue
if relative_pos[0] < 0 and relative_pos[1] == 0:
if agent_dir == 3:
actions.append(2)
continue
if agent_dir == 0:
actions.append(0)
continue
else:
actions.append(1)
continue
'''
for i in range(self.agent_num):
goal = inputs[i]
agent_pos = self.agent_pos[i]
agent_dir = self.agent_dir[i]
relative_pos = np.array(goal) - np.array(agent_pos)
# first quadrant
if relative_pos[0] >= 0 and relative_pos[1] <= 0:
if agent_dir == 0 or agent_dir == 3:
actions.append(2) # forward
continue
if agent_dir == 1:
actions.append(0) # turn left
continue
if agent_dir == 2:
actions.append(1) # turn right
continue
# second quadrant
if relative_pos[0] >= 0 and relative_pos[1] >= 0:
if agent_dir == 0 or agent_dir == 1:
actions.append(2) # forward
continue
if agent_dir == 2:
actions.append(0) # turn left
continue
if agent_dir == 3:
actions.append(1) # turn right
continue
# third quadrant
if relative_pos[0] <= 0 and relative_pos[1] >= 0:
if agent_dir == 1 or agent_dir == 2:
actions.append(2) # forward
continue
if agent_dir == 3:
actions.append(0) # turn left
continue
if agent_dir == 0:
actions.append(1) # turn right
continue
# fourth quadrant
if relative_pos[0] <= 0 and relative_pos[1] <= 0:
if agent_dir == 2 or agent_dir == 3:
actions.append(2) # forward
continue
if agent_dir == 0:
actions.append(0) # turn left
continue
if agent_dir == 1:
actions.append(1) # turn right
continue
'''
return actions
def path_finder(img, food, aisles):
depts = []
for item in food:
depts.append(ncr_get(item))
pix = np.array(img)
grid_size = pix.shape[:2]
grid = np.ones(grid_size, dtype=np.float32)
maze = np.ones((grid_size[0], grid_size[1], 3), dtype=np.float32)
for i in range(grid_size[0]):
for j in range(grid_size[1]):
if sum(pix[i][j]) != 0:
grid[i][j] = np.inf
maze[i][j] = (0, 0, 0)
assert grid.min() == 1, 'cost of moving must be at least 1'
output = {'shelves': maze.tolist(), 'size': grid_size}
# start is the first white block in the top row
start_j, = np.where(grid[-1, :] == 1)
start = tuple(np.array([0, start_j[0]]))
# end is the first white block in the final column
ends = []
# if isinstance(food, str):
# ends.append(aisles[food])
# elif isinstance(food, list):
for item in depts:
# item_name = item.lower()
# if item_name not in list(aisles.keys()):
# raise ValueError('Unsupported food item, supported foods are %s'
# % ', '.join(aisles.keys()))
# else:
try:
ends.append((grid_size[0] - aisles[item]["vertical_position"],
grid_size[1] - aisles[item]["horizontal_position"]))
except KeyError:
continue
def find_closest(point, ends):
shortest_dist = float('inf')
for end in ends:
dist = np.linalg.norm(np.array(end) - np.array(point))
if dist == 0:
continue
else:
if dist < shortest_dist:
shortest_dist = dist
closest_point = end
return closest_point
order = {}
ends_scrap = [start, *ends.copy()]
end = start
for i in range(len(ends_scrap) - 1):
closest = find_closest(end, ends_scrap)
ends_scrap.remove(end)
order[end] = closest
end = closest
# set allow_diagonal=True to enable 8-connectivity
path_color = (1, 1, 0)
for end in ends:
# food_item = [key for key, value in locs.items() if value == end][0]
path = pyastar.astar_path(grid, start, end, allow_diagonal=False)
start = end
output['path'] = path.tolist()
if path.shape[0] > 0:
maze[path[:, 0],
path[:, 1]] = maze[path[:, 0], path[:, 1]] - path_color
# print('plotting path to %s' % (food_item))
else:
pass
# print('no path found')
path_color = (random.uniform(0.1, 0.5), random.uniform(0.1, 0.3),
random.uniform(0.1, 0.5))
maze_out = np.repeat(maze, 20, axis=0)
maze_out = np.repeat(maze_out, 20, axis=1)
maze_out = np.flipud(maze_out)
for i in range(grid_size[0]):
for j in range(grid_size[1]):
maze[i, j] = maze[i, j] * 255
output['map'] = maze.tolist()
# img = Image.fromarray(maze.astype('uint8'))
# img.show()
return output
import numpy as np
import pyastar
# The start and goal coordinates are in matrix coordinates (i, j).
start = (0, 0)
goal = (4, 4)
# The minimum cost must be 1 for the heuristic to be valid.
weights = np.array([[1, 3, 3, 3, 3], [2, 1, 3, 3, 3], [2, 2, 1, 3, 3],
[2, 2, 2, 1, 3], [2, 2, 2, 2, 1]],
dtype=np.float32)
print("Cost matrix:")
print(weights)
path = pyastar.astar_path(weights, start, goal, allow_diagonal=True)
# The path is returned as a numpy array of (i, j) coordinates.
print(f"Shortest path from {start} to {goal} found:")
print(path)
print("Reduced path")
print(pyastar.reduce_path(path))
