def compute_route(self, node_source, source_ori, node_target, target_ori):
self._previous_node = node_source
a_star = AStar()
a_star.init_grid(
self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source, source_ori, node_target,
target_ori), node_source, node_target)
route = a_star.solve()
# JuSt a Corner Case
# Clean this to avoid having to use this function
if route is None:
a_star = AStar()
a_star.init_grid(
self._map.get_graph_resolution()[0],
self._map.get_graph_resolution()[1],
self._map.get_walls_directed(node_source,
source_ori,
node_target,
target_ori,
both_walls=False), node_source,
node_target)
route = a_star.solve()
self._route = route
return route
async def step_6(robot1, robot2, currentPos1, currentPos2):
# Navigation procedure as in Step 2.
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (6, 4), 'to', (4, 0))
print('')
path = a.initLegoWorld((6, 4), (4, 0))
for i in range(len(path)):
await a.move(path[i])
await a.face("south")
currentPos2 = a.getPos()
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (8, 4), 'to', (4, 2))
print('')
path = a.initLegoWorld((8, 4), (4, 2))
for i in range(len(path)):
await a.move(path[i])
currentPos1 = a.getPos()
await robot1.drive_straight(distance_mm(150),
speed_mmps(50)).wait_for_completed()
await robot1.turn_in_place(degrees(70)).wait_for_completed()
return True, currentPos1, currentPos2
def create(self, method_name): if method_name == "dijkstra": return Dijkstra() elif method_name == "astar": return AStar() else: return AStar()
async def step_4(robot1, robot2, currentPos1, currentPos2, destPos):
# Navigation procedure as in Step 2.
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (4, 6), 'to', (8, 4))
print('')
path = a.initLegoWorld((4, 6), (8, 4))
for i in range(len(path)):
await a.move(path[i])
await a.face("north")
currentPos1 = a.getPos()
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (2, 6), 'to', (6, 4))
print('')
dest = (destPos[0], destPos[1])
path = a.initLegoWorld((2, 6), dest)
for i in range(len(path)):
await a.move(path[i])
await a.face("east")
currentPos1 = a.getPos()
return True, currentPos1, currentPos2
async def step_8(robot1, robot2, currentPos1, currentPos2):
# Undo the movements made by the robots when trying to 'look natural' in Step 6.
await robot1.turn_in_place(degrees(-70)).wait_for_completed()
await robot1.drive_straight(distance_mm(-150),
speed_mmps(50)).wait_for_completed()
# Navigation procedure as in Step 2.
# ROBOT 1
a = AStar(robot1, currentPos1)
print('Navigating from:', (4, 2), 'to', (0, 2))
print('')
path = a.initLegoWorld((4, 2), (0, 2))
for i in range(len(path)):
await a.move(path[i])
currentPos1 = a.getPos()
# ROBOT 2
a = AStar(robot2, currentPos2)
print('Navigating from:', (4, 0), 'to', (0, 6))
print('')
path = a.initLegoWorld((4, 0), (0, 6))
for i in range(len(path)):
await a.move(path[i])
await a.face("south")
currentPos2 = a.getPos()
return True, currentPos1, currentPos2
def __call__(self, matrix, snake, candy_pos):
if self.path:
return self.get_direction(snake[0], self.path.pop(0))
head2candy, _ = AStar(matrix, snake[0], candy_pos, walls="qzpmublrto")
if head2candy:
sim_matrix, sim_tail = self.simulate(matrix, snake, head2candy)
candy2tail, _ = AStar(sim_matrix, candy_pos, sim_tail, walls="qzpmublro")
if candy2tail:
self.path = head2candy[2:]
return self.get_direction(snake[0], head2candy[1])
head2tail, _ = AStar(matrix, snake[0], snake[-1], walls="qzpmublro")
return self.get_direction(snake[0], head2tail[1])
def __init__(self, name, speed=0.8):
rospy.init_node(name)
self.name = name
self.speed = speed
self.caught = False
self.my_pose = GroundtruthPose(name)
self.cop_poses = [
GroundtruthPose(c) for c in ['cop_0', 'cop_1', 'cop_2']
]
self.rate_limit = rospy.Rate(100) # 200ms
self.res = 0.2
self.occupancy_grid = OccupancyGrid(self.res, 0.5)
self.path_finder = AStar('euclidean',
self.occupancy_grid,
resolution=self.res)
self.map = list(self.path_finder.occupancy_grid.map)
self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
Twist,
queue_size=5)
rospy.Subscriber('/caught', String, self.catch_listener)
# file for pose history
self.pose_history = []
self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
with open(self.pose_history_fn, 'w'):
pass
# wait for ground truth poses node to init
while not self.my_pose.ready or not all(c.ready
for c in self.cop_poses):
self.rate_limit.sleep()
continue
self.target_position, self.path = self.get_random_target_position_and_path(
)
def main():
# create map
n,m = 256,256
grid = np.zeros((n,m), dtype=np.uint8)
mapper = CVMapper('map', grid, 2)
mapper()
grid /= 255 # back to 1
init, goal = mapper._init, mapper._goal
print 'Initial Point : {}'.format(init)
print 'Final Point : {}'.format(goal)
# h = EDist(goal)
h = GDist(goal, o=grid, scale=5.)
astar = AStar(grid, init, goal, h=h)
_, path = astar()
if path:
viz = grid.astype(np.float32)
viz = cv2.cvtColor(viz, cv2.COLOR_GRAY2BGR)
for (p0, p1) in zip(path[:-1], path[1:]):
y0,x0 = p0
y1,x1 = p1
#cv2.line( (y0,x0), (y1,x1), (128)
cv2.line(viz, (x0,y0), (x1,y1), (0,0,1), 1)
cv2.imshow('viz', viz)
while True:
if cv2.waitKey(0) == 27:
break
else:
print 'Path Not Found'
def test_map_with_40_nodes_start_5_end_34(self):
map_40 = Map(Map40.intersections, Map40.roads)
start_node_id = 5
dest_node_id = 34
astar = AStar(map_40)
self.assertEqual(astar.search(start_node_id, dest_node_id),
[5, 16, 37, 12, 34])
def test_map_with_40_nodes_start_8_end_24(self):
map_40 = Map(Map40.intersections, Map40.roads)
start_node_id = 8
dest_node_id = 24
astar = AStar(map_40, start_node_id, dest_node_id)
self.assertEqual(astar.search(start_node_id, dest_node_id),
[8, 14, 16, 37, 12, 17, 10, 24])
def send_pose_sp(self):
if self.occupancy and self.has_robot_location and self.nav_sp:
state_min = (-int(round(self.plan_horizon)),
-int(round(self.plan_horizon)))
state_max = (int(round(self.plan_horizon)),
int(round(self.plan_horizon)))
# Round initial, goal positions to grid resolution
x_init = round_pt_to_grid(self.robot_translation[:2],
self.plan_resolution)
x_goal = round_pt_to_grid(self.nav_sp[:2], self.plan_resolution)
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
# uncomment to add buffering to obstacles
bufferRadius = 4
astar.bufferOccupancy(bufferRadius)
rospy.loginfo("Computing new navigation plan")
if astar.solve():
# If initial state == goal, path len == 1
# Handle case where A* solves, but no 2nd element -> don't send msg
if len(astar.path) < self.short_path_len:
rospy.loginfo("Path goal matches current state")
# Typical use case
else:
wp_x, wp_y, wp_th = self.next_waypoint(astar)
self.publish_path(astar, wp_x, wp_y, wp_th)
else:
rospy.logwarn("Could not find path")
def get_move(self, grid_data, data):
self.height = data.get("board").get("height")
self.width = data.get("board").get("width")
self.head_x = data.get("you").get("body")[0].get("x")
self.head_y = data.get("you").get("body")[0].get("y")
self.my_snake_health = data.get("you").get("health")
self.my_snake_length = len(data.get("you").get("body"))
self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height)
self.grid_data = grid_data
self.data = data
move = self.move_to_food()
#NOTE FIND TAIL MODE
if self.my_snake_length > 3 and self.my_snake_health > 65 or move == None:
move = self.chase_tail(self.my_snake_health == 100)
if move:
return move
else:
backup_moves = self.helper.get_backup_move((self.head_x, self.head_y), self.grid_data[0], self.height, self.width)
if backup_moves:
return self.helper.get_move_letter((self.head_x, self.head_y), backup_moves[0])
else:
return 'up'
def __init__(self): self._cursor = (1, 1) self._start = (1, 1) self._goal = (33, 8) self._console = Console() self._map = Map() self._astar = AStar(self._map.cost)
def __init__(self):
self.astar=AStar(MAP_COLS,MAP_ROWS)
self.map=self.astar.map
self.astar.print_map()
self.game_over=False
self.score=0
self.done=False
def get_move(self, grid_data, data):
self.height = data.get("board").get("height")
self.width = data.get("board").get("width")
self.head_x = data.get("you").get("body")[0].get("x")
self.head_y = data.get("you").get("body")[0].get("y")
self.my_snake_health = data.get("you").get("health")
self.my_snake_length = len(data.get("you").get("body"))
self.pathfinder = AStar((self.head_x, self.head_y), grid_data[0], self.width, self.height)
self.grid_data = grid_data
self.data = data
snakes = data.get("board").get("snakes")
head = (self.head_x, self.head_y)
# find closest snake ID
target_snake_id = self.find_closest_snake_head(snakes)
# Guard to make sure our snake doesn't just starve
if self.my_snake_health < 40:
return self.default_behaviour()
if target_snake_id:
return self.attack_snake_head(snakes, target_snake_id)
else:
return self.default_behaviour()
def moveto_global(self, target):
"""
Compose global plan from current position and target.
Simply uses A* path-planning for right now.
The path is sort-of-pushed to navigable area by Sobel(),
although it doesn't seem to work that well.
"""
# compute ...
rover = self._rover
#map_obs = cv2.dilate(
# self._rover.worldmap[:,:,0].astype(np.uint8),
# cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3)),
# iterations=1
# ).astype(np.float32)
#map_obs /= 5.0
# alternatively:
map_obs = np.greater(rover.worldmap[:, :, 0],
OBS_THRESH).astype(np.float32)
#map_obs = np.less(self._rover.worldmap[:,:,2], 10)
#map_obs = cv2.erode(map_obs.astype(np.uint8), cv2.getStructuringElement(cv2.MORPH_ERODE, (3,3)))
#map_obs = cv2.dilate(map_obs.astype(np.uint8), cv2.getStructuringElement(cv2.MORPH_DILATE, (3,3)))
#map_obs = map_obs.astype(np.bool)
src = tuple(np.int32(rover.pos))
dst = tuple(np.int32(target))
map_obs[src[1], src[0]] = 0.0 # make sure init cell is open?
h = GDist(dst, map_obs, scale=10.0)
astar = AStar(map_obs, src, dst,
h=h) #, h=GDist(x1=dst[::-1], o=map_obs, scale=1.0))
_, path = astar() # given as (x,y)
# simplify path
if path is None:
# TODO :
# if (realized_cannot_get_to_target) then
# ask_for_new_target()
# No Path! Abort
# TODO : Fix
rover.goal = None
return 'abort', {}
else:
path = cv2.approxPolyDP(path, 3.0, closed=False)[:, 0, :]
## push out from wall
dy = cv2.Sobel(map_obs, cv2.CV_8UC1, 0, 1, ksize=5) / 5.
dx = cv2.Sobel(map_obs, cv2.CV_8UC1, 1, 0, ksize=5) / 5.
delta = np.stack((dx, dy), axis=-1)
dp = delta[path[:, 0], path[:, 1]]
#path = np.int32(path - dp)
####################
# registered as current goal
rover.goal = target
rover.path = path #np.int32(path - dp)
rover.p0 = path
# TODO : make sure poly approximation doesn't cross obstacles
return 'moveto_local', {'path': path, 'phase': 'turn'}
def __init__(self, name, speed=0.6):
rospy.init_node(name)
self.name = name
self.speed = speed
self.my_pose = GroundtruthPose(name)
self.occupancy_grid = OccupancyGrid(0.2, 0.5)
# assign cop to unique robber
self.target = 'robber_' + name[-1]
self.target_pose = GroundtruthPose(self.target)
self.rate_limit = rospy.Rate(100) # 200ms
self.path_finder = AStar('euclidean',
self.occupancy_grid,
resolution=0.2)
self.velocity_publisher = rospy.Publisher('/{}/cmd_vel'.format(name),
Twist,
queue_size=5)
self.catch_publisher = rospy.Publisher('/caught', String, queue_size=1)
rospy.Subscriber('/caught', String, self.catch_listener)
# file for pose history
self.pose_history = []
self.pose_history_fn = '/tmp/gazebo_{}.txt'.format(name)
with open(self.pose_history_fn, 'w'):
pass
# wait for ground truth poses node to init
while not self.my_pose.ready and not self.target_pose.ready:
self.rate_limit.sleep()
continue
def __init__(self):
super(AStarApplication, self).__init__()
self.close = self.cleanup
self.layout = QVBoxLayout(self)
self.setLayout(self.layout)
self.canvas = QGraphicsView()
self.canvas.setViewportUpdateMode(QGraphicsView.FullViewportUpdate)
self.grid = Grid(24, 20)
self.tiles = []
for x in range(self.grid.w):
for y in range(self.grid.h):
tile = Tile(self.grid.get_node(x, y), self)
self.tiles.append(tile)
self.scene = QGraphicsScene()
self.scene.setBackgroundBrush(QBrush(Qt.lightGray, Qt.SolidPattern))
for t in self.tiles:
self.scene.addItem(t)
self.canvas.setScene(self.scene)
self.layout.addWidget(self.canvas)
self.refresh_timer = QTimer()
self.refresh_timer.setInterval(1000 / 60)
self.refresh_timer.timeout.connect(self.refresh)
self.refresh_timer.start()
self.AStar = AStar(self.grid)
self.focusPolicy = Qt.StrongFocus
self.keyPressEvent = self.kbdHandler
def main():
global window, origin, astar, objective
window = pygame.display.set_mode(WINDOW_SIZE)
pygame.display.set_caption("A* Pathfinding")
gen_rects()
origin = grid_list[3][5]
objective = grid_list[12][28]
astar = AStar(origin, objective)
gen_objective_distance()
gen_adjacents()
loop = True
start = False
vel = 30
while loop:
window.fill(WHITE)
#events
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
if event.type == pygame.KEYDOWN:
if event.key == pygame.K_RETURN:
start = True
if event.type == pygame.MOUSEBUTTONUP:
if pygame.mouse.get_pressed(3)[0]:
click_wall(pygame.mouse.get_pos(), True)
if pygame.mouse.get_pressed(3)[2]:
click_wall(pygame.mouse.get_pos(), False)
if event.type == pygame.MOUSEMOTION:
if pygame.mouse.get_pressed(3)[0]:
click_wall(pygame.mouse.get_pos(), True)
if pygame.mouse.get_pressed(3)[2]:
click_wall(pygame.mouse.get_pos(), False)
if start:
vel = 5
if not astar.found:
astar.seach()
pygame.draw.rect(window, RED, objective.rect)
pygame.draw.rect(window, GREEN, origin.rect)
draw_grid()
# if astar.found:
# back(vel,astar)
# astar.seach_back()
# for sqr in astar.path:
# pygame.draw.rect(window,BLUE,sqr.rect)
pygame.display.update()
fps.tick(vel)
def update(cls, screen, survivor):
del_zmbs = set()
for zmb in cls.instances:
if zmb.health <= 0.:
Drop.spawn(zmb.pos)
del_zmbs.add(zmb)
stats["Zombies Killed"] += 1
continue
screen.blit(zmb.img, zmb.pos)
zmb.health_bar(surface=screen) # Health bar with rounded edges
if Options.debug:
for tile in zmb.path:
pygame.draw.circle(screen, zmb.path_colour,
tile.get_centre(), Tile.length // 3)
zmb_to_survivor_dist = (survivor.pos - zmb.pos).magnitude()
if zmb_to_survivor_dist <= cls.attack_range:
survivor.health -= 0.4
if zmb.to is not None: # if the zombie is not next to the player
angle = angle_between(*zmb.pos, *(zmb.to + zmb.vel))
if zmb.pos == zmb.to: # If the zombie is directly on a tile
zmb.to = None # Trigger A-Star, not run between tiles for performance
else:
if "freeze" not in Drop.actives:
zmb.pos += zmb.vel
if zmb.direction != angle: # New direction, frame after a turn
zmb.rotate(angle)
if zmb.to is None and zmb_to_survivor_dist > Tile.length:
AStar(zmb, survivor).solve()
cls.instances -= del_zmbs
def move():
data = bottle.request.json
board = data.get('board')
board_height = board.get('height')
board_width = board.get('width')
# print(board)
GRID = buildGrid(board, board_height, board_width)
# print(GRID)
# TODO: Do things with data
# get the snake
VINCE = data['you']
xhead = VINCE['body'][0]['x']
yhead = VINCE['body'][0]['y']
head_tuple = (xhead, yhead)
# first food bit
first_food_bit = (board['food'][0]['x'], board['food'][0]['y'])
AS = AStar()
path_to_food = AS.find_path(GRID, head_tuple, first_food_bit)
print(path_to_food)
directions = ['up', 'down', 'left', 'right']
direction = random.choice(directions)
direction = get_dir(head_tuple, path_to_food[1])
print(direction)
return {'move': direction, 'taunt': 'battlesnake-python!'}
def test_heuristic_gen(self):
root = self.gen_tree()
pvar = pred_variance(root)
max_var = 1.5 * max(pvar.values())
pvar1 = dict((p, max_var - v) for p, v in pvar.items())
pvar1s = sum(pvar1.values())
weights = dict((p, root.num_occurrences(p) * v / pvar1s)
for p, v in pvar1.items())
for p, w in weights.items():
print '%s: %f' % (p, w)
preds = list(root.get_all_predicates())
preds.sort()
weights = fmincon.run_fmincon(root)
h = heuristic.Heuristic(dict(zip(preds, weights)))
root.make_graphviz(open('temp.dot', 'w'))
search = AStar(root, lambda x: h(x.preds))
iter = search.gen
iterations = 0
try:
while not iter.next():
iterations += 1
except StopIteration:
print iterations
print[''.join(s.preds) for s in search.solution]
def find_solver(self, algorithm):
solver = None
while solver is None:
if algorithm == 'depth_first' or algorithm == 'dfs':
solver = Depth_first(self.map)
elif algorithm == 'breadth_first' or algorithm == 'bf' or algorithm == 'bfs':
solver = Breadth_first(self.map)
elif algorithm == 'astar' or algorithm == 'a*':
solver = AStar(self.map)
elif algorithm == 'greedy_best_first' or algorithm == 'gbf' or algorithm == 'gbfs':
solver = Greedy_best_first(self.map)
elif algorithm == 'iterative_deepening' or algorithm == 'id' or algorithm == 'ids':
solver = Iterative_deepening(self.map)
elif algorithm == 'greedy' or algorithm == 'g' or algorithm == 'gs':
solver = Greedy(self.map)
#Iterative astar works, but it does not work well.
#I did not research it much before building it, and I got a few key details wrong.
#I decided to scrap it and use a different informed custom search (greedy), as it would have taken me far too long to redo it.
#But I figured it would still be a valuable thing to include just to show my progress.
elif algorithm == 'iterative_astar' or algorithm == 'ia*':
solver = Iterative_astar(self.map)
else:
print(
'Valid searches are: dfs (depth first), bfs (breadth first), A*, greedy, ids (iterative deepening) or gbfs (greedy best first)'
)
print('Please enter an algorithm:')
algorithm = input()
return solver
def main():
pygame.init()
dims = Vec2(240, 160)
window = pygame.display.set_mode((dims.x * square_size, dims.y * square_size))
grid = Grid(dims, window)
pygame.display.set_caption("Maze")
maze_generator = MazeGenerator(grid)
astar = AStar(grid, Vec2(0, 0), grid.dims - Vec2(2, 2))
running = True
clock = pygame.time.Clock()
while running:
for e in pygame.event.get():
if e.type == pygame.QUIT:
running = False
if e.type == pygame.KEYDOWN:
if e.key == pygame.K_SPACE:
astar.step()
if not maze_generator.finished():
maze_generator.step()
if maze_generator.finished():
astar.step()
if not maze_generator.finished():
draw_cell(window, maze_generator.current_cell(), colors.red)
pygame.display.update()
# clock.tick(60)
pygame.quit()
def calculatePath(self, start, goal, clusterIds):
# Only length
starSolver = AStar(self)
t = starSolver.solveBetweenNodes(clusterIds, start, goal)
if t > 0:
self.graph.addEdge(start, goal, t)
return True
else:
return False
def __init__(self, city_name):
self._map = CarlaMap(city_name)
self._astar = AStar()
# Refers to the start position of the previous route computation
self._previous_node = []
# The current computed route
self._route = None
def traveling_salesman_exact(x_init, x_goals, statespace_lo, statespace_hi,
occupancy, resolution):
paths = {}
for i in range(len(x_goals)):
astar = AStar(statespace_lo, statespace_hi, x_init, x_goals[i],
occupancy, resolution)
if astar.solve():
paths[(0, i + 1)] = len(astar.path)
else:
paths[(0, i + 1)] = np.float('inf')
for pair in itertools.combinations(range(len(x_goals)), 2):
astar = AStar(statespace_lo, statespace_hi, x_goals[pair[0]],
x_goals[pair[1]], occupancy, resolution)
pair = [x + 1 for x in list(pair)]
pair = tuple(pair)
if astar.solve():
paths[pair] = len(astar.path)
else:
paths[pair] = np.float('inf')
min_length = np.float('inf')
min_circuit = [0]
for circuit in itertools.permutations(range(len(x_goals))):
circuit = [x + 1 for x in list(circuit)]
circuit = [0] + circuit
curr_length = get_circuit_length(circuit, paths)
if curr_length < min_length:
min_length = curr_length
min_circuit = circuit
min_circuit = min_circuit[1:]
min_circuit = [x - 1 for x in min_circuit]
wps = [x_goals[i] for i in min_circuit]
print("Found path with length: {}:".format(min_length))
print(min_circuit)
print(wps)
return min_circuit
def main(argv):
# queens = createBoard(argv[0])
queens = getDemoBoard(argv[0])
heur = 0 if argv[2] == "H1" else 1
if int(argv[1]) == 1: # A* Algorithm
astar_solver = AStar(queens, heur)
print(astar_solver.Solve())
elif int(argv[1]) == 2: # Hill Climb
hill_solver = HillClimb(queens, heur)
hill_solver.solve()
def harmonic(self):
"""
"""
heurs = [{
'comp': 'vel',
'metric': 'octile',
'alpha': 1.
}, {
'comp': 'stock',
'metric': 'octile',
'alpha': 1.
}]
self.a = AStar(meshSize=(30, 30),
heur=heurs[0],
heur_weight=4,
plots='images/harmonic/vel')
self.b = AStar(meshSize=(30, 30),
heur=heurs[1],
heur_weight=1,
plots='images/harmonic/stock')
def find_path(self, map2d, end_point):
"""
:param map2d: 地图
:param end_point: 寻路终点
"""
start_point = (self.mx, self.my)
path = AStar(map2d, start_point, end_point).start()
if path is None:
return
self.path = path
self.path_index = 0
