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