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():
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 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 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 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 __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 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
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 create_astar(self):
self.bfs = AStar(self.bfs_grid, 'BFS')
self.dfs = AStar(self.dfs_grid, 'DFS')
self.astar = AStar(self.astar_grid, 'AStar')
self.active_search = self.bfs
self.active_grid = self.bfs_grid
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 create(self, method_name): if method_name == "dijkstra": return Dijkstra() elif method_name == "astar": return AStar() else: return AStar()
def main():
parser = argparse.ArgumentParser(
description="Compute the optimal path using A*"
)
group = parser.add_mutually_exclusive_group()
group.add_argument(
"-w", "--world", help="The filename of the world to load",
)
group.add_argument(
"-s", "--size", type=int, help="Size of the random board to be generated"
)
parser.add_argument(
"-f", "--file", help="The file to output the world to"
)
parser.add_argument(
"heuristic", type=int,
help="The heuristic number to run (in range(1, 7))"
)
args = parser.parse_args()
if args.world:
newWorld = World(filepath=args.world)
elif args.size:
newWorld = World(height=args.size, width=args.size)
else:
newWorld = World(height=10, width=10)
if args.file:
newWorld.write_world(args.file)
astar = AStar(newWorld, args.heuristic)
astar.start()
class AStarApplication(QWidget):
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
# Avoid the segfault caused by some obscure GC stuff.
@pyqtSlot()
def cleanup(self, obj):
pass
@pyqtSlot()
def refresh(self):
self.scene.invalidate()
@pyqtSlot()
def kbdHandler(self, e):
if e.key() == Qt.Key_Space:
self.AStar.step()
# Called when a tile changes type, so all tiles can be updated
def update_tiles(self, caller):
for t in self.tiles:
if self.grid.target is not None:
t.subtext = t.node.heuristic(self.grid.target)
else:
t.subtext = ''
if t != caller and t.type != 'clear' and t.type != 'wall' and t.type == caller.type:
t.set_type('clear')
self.AStar.reset()
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 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 __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):
self.list = []
count = 0
# 依次添加行人至self.list
for i in range(0, 15):
self.list.append(People("o" + str(count), 60, 60 + i * 40))
count = count + 1
self.list.append(People("o" + str(count), 100, 60 + i * 40))
count = count + 1
self.list.append(People("o" + str(count), 140, 60 + i * 40))
count = count + 1
# 在一开始就计算好各个位置的下一步方向向量，并存储到矩阵中，以节省算力
k = 5
self.matrix = [[0 for i in range(17)] for i in range(27)]
for i in range(0, 27):
for j in range(0, 17):
if i == 0: # 将障碍物的位置也设置对应的方向向量，以便在特殊情况撞墙后能及时调整回来
self.matrix[i][j] = (k, 0)
elif i == 26:
self.matrix[i][j] = (-1 * k, 0)
elif j == 0:
self.matrix[i][j] = (0, k)
elif j == 16:
self.matrix[i][j] = (0, -1 * k)
elif i == 10:
if (j == 4) or (j == 10):
self.matrix[i][j] = (-1 * k, -1 * k)
elif (j == 5) or (j == 11):
self.matrix[i][j] = (-1 * k, 0)
elif (j == 6) or (j == 12):
self.matrix[i][j] = (-1 * k, k)
else:
self.matrix[i][j] = AStar.next_loc(i, j)
elif i == 17:
if (j == 4) or (j == 10):
self.matrix[i][j] = (k, -1 * k)
elif (j == 5) or (j == 11):
self.matrix[i][j] = (k, 0)
elif (j == 6) or (j == 12):
self.matrix[i][j] = (k, k)
else:
self.matrix[i][j] = AStar.next_loc(i, j)
elif (j == 4) or (j == 10):
if (i > 10) and (i < 17):
self.matrix[i][j] = (0, -1 * k)
else:
self.matrix[i][j] = AStar.next_loc(i, j)
elif (j == 6) or (j == 12):
if (i > 10) and (i < 17):
self.matrix[i][j] = (0, k)
else:
self.matrix[i][j] = AStar.next_loc(i, j)
elif (i > 10) and (i < 17) and ((j == 5) or (j == 11)):
self.matrix[i][j] = (0, k)
else:
self.matrix[i][j] = AStar.next_loc(i, j)
self.matrix[26][4] = (1, 0)
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 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 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 startGame():
mystdout = StdOutWrapper()
sys.stdout = mystdout
sys.stderr = mystdout
curses.initscr()
curses.beep()
curses.beep()
window = curses.newwin(HEIGHT, WIDTH, 0, 0)
window.timeout(TIMEOUT)
window.keypad(1)
curses.noecho()
curses.curs_set(0)
window.border(0)
snake = Snake(SNAKE_X, SNAKE_Y, window)
food = Food(window, snake)
astar = AStar()
while True:
window.clear()
window.border(0)
# rendering the objects
snake.render()
food.render()
window.addstr(0, 5, snake.getScore)
event = window.getch()
if event == 27:
break
if snake.head.x == food.x and snake.head.y == food.y:
snake.eatFood(food)
curses.beep()
event = astar.getKey(food, snake)
# print(event)
if event in (KEY_UP, KEY_DOWN, KEY_LEFT, KEY_RIGHT):
snake.makeMove(event)
snake.update()
if snake.collided():
break
curses.endwin()
print(f"High score :: {snake.score}")
sys.stdout = sys.__stdout__
sys.stderr = sys.__stderr__
sys.stdout.write(mystdout.get_text())
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 find_alternative_path(self, start, blackout, goal):
if start == goal: return True
if (start, blackout, goal) in self.alternative_paths: return True
backup_val = self.grid[blackout]
self.grid[blackout] = inf
pather = AStar(self.grid, start, goal)
success = pather.findpath()
if success:
self.alternative_paths[(start, blackout, goal)] = pather.path
self.alternative_paths[(goal, blackout, start)] = deque(reversed(pather.path))
self.grid[blackout] = backup_val
return success
def find_closest(x_init, x_goals, statespace_lo, statespace_hi, occupancy,
resolution):
min_dist = np.float('inf')
closest = None
for x_goal in x_goals:
astar = AStar(statespace_lo, statespace_hi, x_init, x_goal, occupancy,
resolution)
if astar.solve() and len(astar.path) < min_dist:
min_dist = len(astar.path)
closest = x_goal
return closest, min_dist
def send_pose_sp(self):
try:
(robot_translation,
robot_rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.has_robot_location = True
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
robot_translation = (0, 0, 0)
robot_rotation = (0, 0, 0, 1)
self.has_robot_location = False
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)))
x_init = (round(robot_translation[0] / self.plan_resolution) *
self.plan_resolution,
round(robot_translation[1] / self.plan_resolution) *
self.plan_resolution)
x_goal = ((round(self.nav_sp[0] / self.plan_resolution)) *
self.plan_resolution,
round(self.nav_sp[1] / self.plan_resolution) *
self.plan_resolution)
rospy.logwarn("x_init (after rounding): (%6.4f,%6.4f)", x_init[0],
x_init[1])
rospy.logwarn("x_goal (after rounding): (%6.4f,%6.4f)", x_goal[0],
x_goal[1])
astar = AStar(state_min, state_max, x_init, x_goal, self.occupancy,
self.plan_resolution)
rospy.loginfo("Computing Navigation Plan")
if astar.solve():
rospy.loginfo(
"Navigation Success. Found %d waypoint path to (%6.3f, %6.3f)",
len(astar.path), astar.path[-1][0], astar.path[-1][1])
# a naive path follower we could use
# pose_sp = (astar.path[1][0],astar.path[1][1],self.nav_sp[2])
# msg = Float32MultiArray()
# msg.data = pose_sp
# self.pose_sp_pub.publish(msg)
# astar.plot_path()
path_msg = self.buildPath(astar.path)
self.nav_path_pub.publish(path_msg)
else:
rospy.logwarn("Navigation Failure")
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
def run():
# Initialize a 10x10 stage. No other stage sizes are
# coded yet, but these constructor arguments exist in
# case we want to later on
board = Board()
# Initialize the A-Star algorithm class with our
# current graph and reference to the board because
# I'm lazy. It shouldn't have a dependency on a board,
# but it's been 8 hours already and I'm tired.
astar = AStar(board.graph, board)
# Render the board to show where we stand
board.render()
inp = get_input()
while inp[0].lower() not in "qe":
if inp[0].lower() in "h":
# Players is babies and wants helps!
with open(os.path.join(resource_root, "maze", "help.txt"), "rb") as help_file:
print(help_file.read())
board.render()
inp = raw_input("Next move? (enter h or help for help)").strip()
if inp == "":
inp = " "
continue
# Try to move the player. Run, Forrest! Run!
if inp[0].lower() == "u":
(x, y) = board.player
board.move_entity("player", x, y - 1)
elif inp[0].lower() == "d":
(x, y) = board.player
board.move_entity("player", x, y + 1)
elif inp[0].lower() == "l":
(x, y) = board.player
board.move_entity("player", x - 1, y)
elif inp[0].lower() == "r":
(x, y) = board.player
board.move_entity("player", x + 1, y)
# The enemy is a greedy bastard and moves every chance he can
path = astar.get_path(astar.search(board.enemy, board.player)[0], board.enemy, board.player)
board.move_entity("enemy", *path[1])
board.render(path=path[2:-1])
if board.enemy == board.player:
print("You lost!")
exit()
if board.player == board.goal:
print("You escaped death! Huzzah!")
exit()
inp = get_input()
print("Scaredy-cat!")
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
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 main(self):
clock = pygame.time.Clock()
graph = grid2graph(self.world) # Convert grid into graph
astar = AStar() # Initialise Astar
res = 25 # Resolution = cm per grid square
while not self.quit:
source = self.node_nb(self.buggy_pos, res) # Source = buggy pos
sink = self.node_nb(self.cur_pos,
res * self.scale) # Sink = cursor pos
[node.reset() for node in graph] # Reset every node
path = astar.solve(graph, source, sink) # Solve graph
self.event_handler(
) # Put all keypress events into key_status dictionary
self.update_window(path, res) # Update display
clock.tick(self.fps) # Restrict rate
def callAstar(self):
occupancy = DetOccupancyGrid2D(self.map_width, self.map_height,
self.obstacles)
self.astar = AStar((0, 0), (self.map_width, self.map_height),
self.start_pos, self.end_pos, occupancy)
if not self.astar.solve():
exit(0)
self.trajectory = Trajectory()
self.trajectory.x.data = (self.astar.path * self.astar.resolution)[:,
0]
self.trajectory.y.data = (self.astar.path * self.astar.resolution)[:,
1]
self.trajectory.theta.data = np.zeros((len(self.trajectory.x.data), 1))
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 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 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]
class AStarGAC:
def __init__(self):
# Keep track of all the nodes in the system
self.nodes = []
# Set node type for AStar
AStar.State = GACState
# Reference to AStar and CSP
self.astar = AStar()
self.gac_state = None
# Set behavior
self.astar.set_behavior(BestFirst)
def start(self):
# Run CSP-initialize and CSP-domain-filtering-loop
self.gac_state.gac.initialize()
self.gac_state.gac.domain_filtering()
# Assign the current state to AStar and begin to solve
self.astar.states[self.gac_state.get_hash()] = self.gac_state
self.astar.behavior.add(self.gac_state, self.astar.open)
# Check if we are finished or if the current state is invalid
if self.gac_state.gac.check_finished() or self.gac_state.gac.check_contradictory_state():
if self.gac_state.gac.check_finished():
# We found the solution without actually running the agenda loop, make sure we save goal state
self.gac_state.type = self.astar.State.GOAL
return True
return False
def run(self):
if self.gac_state.gac.check_finished():
return True
# Do one step in the agenda loop
finished = self.astar.agenda_loop()
# Update the CSP state
self.gac_state = self.astar.closed[-1]
# Return the result from the agenda loop
return finished
def __init__(self):
# Keep track of all the nodes in the system
self.nodes = []
# Set node type for AStar
AStar.State = GACState
# Reference to AStar and CSP
self.astar = AStar()
self.gac_state = None
# Set behavior
self.astar.set_behavior(BestFirst)
def step(self, execute):
if self.mmap.isLiftOpen():
self.tar = self.mmap.getLift()
aStar = AStar(self.mmap.copy(), self.tar)
self.path = aStar.process().path()
else:
self.tars = []
for l in self.mmap.getLambdas():
aStar = AStar(self.mmap.copy(), l)
self.path = aStar.process().path()
self.tars.append((l, self.path))
minmin = reduce(lambda a, b: min(a, b), map(lambda t: len(t[1]), self.tars))
self.tar, self.path = filter(lambda t: len(t[1]) == minmin, self.tars)[0]
if (not self.wouldGetKilledFor(self.path) and
self.mmap.getLeftCommands() > len(self.path) and
len(self.path) > 0):
execute(self.path)
self.skip = []
else:
self.skip.append(self.tar)
return self
def move(self):
if self.next_step_x == None and self.next_step_y == None:
if self.player.x != self.player_last_position[0] or self.player.y != self.player_last_position[1]:
self.player_last_position = (self.player.x, self.player.y)
if self.player.next_step_x == None and self.player.next_step_y == None:
self.path = AStar.get_path(self)
if self.next_step_x == None and self.next_step_y == None:
if len(self.path) > 0:
l = self.path.pop()
self.next_step_x = l.x
self.next_step_y = l.y
else:
if self.x > self.next_step_x:
self.x -= 4
self.direction = "W"
elif self.x < self.next_step_x:
self.x += 4
self.direction = "E"
if self.y > self.next_step_y:
self.y -= 4
self.direction = "N"
elif self.y < self.next_step_y:
self.y += 4
self.direction = "S"
if self.x == self.next_step_x and self.y == self.next_step_y:
self.next_step_x = None
self.next_step_y = None
class Astar_GAC(Graph):
"""Astar_GAC integrates Astar and GAC"""
#both
def __init__(self, variables, domains, expressions):
super(Astar_GAC, self).__init__()
self.cnet = CNET(variables, domains, expressions)
self.currentState = self.initializeState(self.cnet)
self.gac = GAC(self.currentState)
self.Astar = AStar(self)
@abstractmethod
def createNewState( self, variables, constraints):
pass
def initializeState(self, cnet):
"""in initState each variable has its full domain. It will be set as root node
initilizes cnet"""
s = self.createNewState( cnet.variables, cnet.constraints )
s.update('start')
self.startNode = s
self.stateCounter = 0
self.nofAssumption = 0
self.nofExpanded = 0
return s
def search(self):
self.currentState = self.gac.domainFiltering(self.currentState)
self.stateCounter += 1
if self.currentState.isSolution():
self.printStatistics(self.currentState)
return self.currentState
return self.iterateSearch()
def iterateSearch(self):
prev = self.currentState
if prev.isSolution():
return prev
self.currentState = self.Astar.iterateAStar()
# self.currentState.updateColors()
self.stateCounter += 1
self.currentState.parent = prev #used for backtracking to find 'shortest path' for statistics
self.nofExpanded = self.Astar.nofExpandedNodes
if self.currentState.isSolution():
self.printStatistics(self.currentState)
return self.currentState
self.currentState = self.gac.domainFiltering(self.currentState)
return self.currentState
def makeAssumption(self, newVI, parentState):
"""Generate one successor, and make sure all pointers are correct"""
newVertices = {}
newVIList = []
for vi in parentState.viList:
viID = vi.getID()
tmpVI = VI(viID, vi.domain.copy())
newVertices[viID] = tmpVI
newVIList.append( tmpVI )
for vi in parentState.viList:
viID = vi.getID()
for neighbor in vi.neighbors:
n = newVertices[ neighbor.getID() ]
newVertices[viID].add_neighbor( n )
for vi in newVIList:
if vi.getID() == newVI.getID():
newVIList.remove(vi)
newVIList.append(newVI)
else:
for vi_n in vi.neighbors:
if vi_n.getID() == newVI.getID():
vi.neighbors.remove(vi_n)
vi.neighbors.append(newVI)
succ = self.createNewState(newVIList, parentState.constraintList)
succ.parent = parentState
succ.updateUndecided() # maybe not needed
succ.ciList = []
constraints = self.cnet.getConstraints()
for v in succ.undecidedVariables:
for n in v.neighbors:
for c in constraints:
succ.ciList.append( CI(c,[v,n]) )
return succ
def generateSucc(self, state):
""" make a guess. start gussing value for variables with min. domain length"""
succStates = []
finishedVIs = []
varsCopy = state.undecidedVariables.copy()
if not len(varsCopy):
return []
otherVIs = sorted(varsCopy, key=lambda v: len(v.domain), reverse=True)
betterVI = otherVIs.pop()
if betterVI.domain:
initID = betterVI.getID()
for d in betterVI.domain:
newVI = VI( initID, [d])
newVI.neighbors = betterVI.neighbors.copy()
successor = self.makeAssumption(newVI, state)
succStates.append( self.gac.rerun(successor) )
return succStates
else:
return []
# Not complete : TODO
def printStatistics(self, state):
print ( 'The number of unsatisfied constraints = ', self.countUnsatisfiedConstraints(state), '\n' )
print ( 'The total number of verticies without color assignment = ', self.countColorLess(state), '\n' )
print ( 'The total number of nodes in search tree = ', self.stateCounter, '\n' )
print ( 'The total number of nodes poped from agenda and expanded = ', self.nofExpanded, '\n' )
print ( 'The length of the path = ', self.nofAssumption ,'\n')
def countColorLess(self, state):
nofColorLess = 0
for vi in state.viList:
if len(vi.domain) != 1:
nofColorLess += 1
return nofColorLess
# def countUnsatisfiedConstraints(self, state):
# unsatisfied = 0
# varList = state.viList
# for c in state.ciList:
# for var in varList:
# if var in c.variables:
# if self.countInconsistentDomainValues(var, c) or not len(var.domain):
# unsatisfied += 1
# return unsatisfied
def countUnsatisfiedConstraints(self, state):
unsatisfied = 0
varList = state.viList
for var in varList:
if len(var.domain) != 1:
unsatisfied += 1
return unsatisfied
# Needed for Graph
def getGoal(self):
return None
def countInconsistentDomainValues(self, x, c):
pairs = []
nofInconsistency = 0
for k in c.variables:
for value in x.domain:
pairs.extend( list(itertools.product([value], k.domain)) )
for p in pairs :
if not c.constraint( p[0], p[1] ):
nofInconsistency += 1
return nofInconsistency
def __init__(self, variables, domains, expressions):
super(Astar_GAC, self).__init__()
self.cnet = CNET(variables, domains, expressions)
self.currentState = self.initializeState(self.cnet)
self.gac = GAC(self.currentState)
self.Astar = AStar(self)
def A(m, h=0):
a = AStar(m, h)
for i in a.step():
pass
return None if not a.path else True
def write_file(self, name, closed_set):
"""
Escrita de um ficheiro .txt para o armazenamento dos
pontos resultantes do algoritmo
@param name nome do ficheiro .txt onde serao armazenados os dados
@closed_set lista de pontos resultantes do algoritmo
"""
data = open(name, "w")
for x in closed_set:
data.writelines((str(x[0]), " , ", str(x[1]), "\n"))
data.close()
if __name__ == "__main__":
open_image = OpenImage("peppersgrad.pgm")
a_star = AStar()
open_image.open_image(a_star.START_POINT, a_star.END_POINT)
start = time.time()
a_star.a_star(open_image.img)
finish = time.time() - start
for point in a_star.closed_set:
open_image.draw_point(point)
print "Tempo de execucao: %.15f" % finish
print "Iteracoes:", a_star.count
print "Media por cada iteracao: %.15f" % (finish / a_star.count)
open_image.show_image()
open_image.save_image("result.png")
write_file = WriteFile()
write_file.write_file("data.txt", a_star.closed_set)
class Window:
def create_grid(self, rows, columns):
self.bfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)
self.dfs_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)
self.astar_grid = Grid(self.width, self.height*2//3, rows, columns, self.screen)
def update_cell(self, row, column, state):
self.bfs_grid.update_cell(row, column, state)
self.dfs_grid.update_cell(row, column, state)
self.astar_grid.update_cell(row, column, state)
def create_astar(self):
self.bfs = AStar(self.bfs_grid, 'BFS')
self.dfs = AStar(self.dfs_grid, 'DFS')
self.astar = AStar(self.astar_grid, 'AStar')
self.active_search = self.bfs
self.active_grid = self.bfs_grid
def create_buttons(self):
l = 10
h = 25
t = self.height*2//3 + 10
w = (self.width-l*4)//3
p = [(l,t,w,h),(l*2 + w,t, w,h),(l*3 + w*2, t,w, h)]
self.buttons = [pygbutton.PygButton(p[0], "BFS"), pygbutton.PygButton(p[1], "DFS"),
pygbutton.PygButton(p[2], "A*")]
def __init__(self, width=500,height=750):
pygame.init()
self.width = width
self.height = height
self.screen = pygame.display.set_mode((self.width, self.height))
self.create_buttons()
# self.font = pygame.font.SysFont('Arial', 25)
self.font = pygame.font.Font('freesansbold.ttf', 16)
self.WHITE = (255, 255, 255)
self.BLACK = (0, 0, 0)
def show_text(self):
x = 10
y = self.height*2//3 + 50
text = self.bfs.getStats()
self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))
y += 25
text = self.dfs.getStats()
self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))
y += 25
text = self.astar.getStats()
self.screen.blit(self.font.render(text, True, self.BLACK), (x, y))
def drawPath(self):
self.active_search.backtrackPath()
pathNodes = self.active_search.bestPath
for node in pathNodes[:-1]:
((x,y),s) = node.getID()
if node.state != 'goal':
self.active_grid.drawPath(x, y)
def loop(self):
clock = pygame.time.Clock()
results = [None, None, None]
active_result_index = 0
active_result = results[active_result_index]
while True:
pygame.event.pump()
active_result = results[active_result_index]
if active_result == None or active_result.state != 'goal':
active_result = self.active_search.iterateAStar()
results[active_result_index] = active_result
self.screen.fill(self.WHITE)
self.active_grid.draw()
self.drawPath()
self.show_text()
for event in pygame.event.get():
if 'click' in self.buttons[0].handleEvent(event):
self.active_search = self.bfs
self.active_grid = self.bfs_grid
active_result_index = 0
if 'click' in self.buttons[1].handleEvent(event):
self.active_search = self.dfs
self.active_grid = self.dfs_grid
active_result_index = 1
if 'click' in self.buttons[2].handleEvent(event):
self.active_search = self.astar
self.active_grid = self.astar_grid
active_result_index = 2
if event.type == pygame.QUIT:
sys.exit()
for button in self.buttons:
button.draw(self.screen)
# pygame.display.update(changed_rectangles) is faster
pygame.display.flip()
# --- Limit to 60 frames per second
clock.tick(60)
'''
grid_mdp = GridWorldMDP(map_struct['seed_map'], map_struct['goal'],
map_struct['start'], map_struct['bridge_probabilities'],
map_struct['bridge_locations'])
init_value = {}
for s in grid_mdp.states:
init_value[s.tostring()] = np.linalg.norm(s - grid_mdp.goal_state)
mdp = MDP(grid_mdp.states, grid_mdp.valid_actions_function, grid_mdp.cost_function)
#value_fcn = mdp.value_iteration(value = value_fcn, plot=True, world_size = 50)
value_fcn = mdp.value_iteration(value = init_value, plot=True, world_size = 50)
#set up dubins astar
dub = dubins_astar(world_points, value_fcn)
astar = AStar(motion_primitives, dub.cost_function, dub.heuristic,
dub.valid_edge, dub.state_equality, plot = False)
astar_state = np.array([state['x'],state['y'],state['theta']])
else:
'''
following_dist = 0.0
temp_idx = dub.last_idx
while following_dist < dub.look_ahead_dist
temp_idx -= 1
path_diff = numpy.array([,])
following_dist += np.linalg.norm(path_diff)
index = temp_idx
'''
#ind_diff = np.abs(np.array(indices) - carrot_idx)
#index = indices[np.argmin(ind_diff)]
astar_state = np.array([state['x'],state['y'],state['theta']]) #path_states[index,:]
def move(self):
self.step_num += 1
# if bomb in the neigbourhood
if self.parent.target in neighbours((self.x, self.y), True):
# The agent found the bomb
self.bomb_location = self.parent.target
if self.is_scout:
# bomb deactivated
self.parent.bomb_found = True
return True
else:
self.color = QColor(200, 96, 109, 255)
# the path is being reinitialized in
# order to be show to the scout
self.bomb_location = self.parent.target
# if agent is scout and scout knows the bomb
if self.bomb_location and self.is_scout:
if self.path_to_bomb:
(self.x, self.y) = self.path_to_bomb.pop()
return True
# create a shortcut
target = self.bomb_location
known_region = self.path
start = (self.x, self.y)
print 'calling astar'
shortest_path = AStar()
print 'will find the shortest path'
self.path_to_bomb = shortest_path.find_path(start, target, known_region)
return True
# select to move randomly avoiding
# to go to the last position
order_list = ['u', 'r', 'd', 'l']
random.shuffle(order_list)
# get the available moves
moves = []
for order in order_list:
if order == 'u':
moves = self.up(moves)
elif order == 'r':
moves = self.right(moves)
elif order == 'd':
moves = self.down(moves)
elif order == 'l':
moves = self.left(moves)
# if moves available
if len(moves) != 0:
# check if agents meet each other
for agent in self.parent.agents:
if (agent.x, agent.y) in neighbours((self.x, self.y)):
self.exchanges += 1
agent.path += self.path
self.path += agent.path
self.path = list(set(self.path))
agent.path = list(set(agent.path))
if (agent.is_scout) and (self.bomb_location) and (not agent.bomb_location):
agent.bomb_location = self.bomb_location
return True
else:
if self.bomb_location and not agent.bomb_location:
agent.bomb_location = self.bomb_location
agent.color = QColor(200, 96, 109, 255)
for m in moves:
# The agent prefers to go to a
# square he has not visited
if m in self.path:
#we put this move at the end
moves.remove(m)
moves.append(m)
(self.x, self.y) = moves[0]
if moves[0] in self.path:
self.path.remove((self.x, self.y))
self.path.append((self.x, self.y))
else:
print 'cannot move'
return False
return True
self.delta_state = delta_state
def get_end_state(self, state):
return state + self.delta_state
delta_states = [np.array([ 1, 0]),
np.array([ 1, 1]),
np.array([ 0, 1]),
np.array([-1, 1]),
np.array([-1, 0]),
np.array([-1, -1]),
np.array([ 0, -1]),
np.array([ 1, -1])]
motion_primitives = [motion_primitive(delta_state) for delta_state in delta_states]
def cost_function(state, motion_primitive):
return np.linalg.norm(motion_primitive.delta_state)
astar = AStar(motion_primitives, cost_function, heuristic, valid_edge_function, state_equality)
plan = astar.plan(start_state, goal_state)
pathx = [state[0] for state in plan]
pathy = [state[1] for state in plan]
plt.plot(pathx,pathy)
plt.plot([0, 7],[3, 3])
plt.plot([3, 10],[6, 6])
plt.show()
def A(m, h=0):
a = AStar(m, h)
for i in a.step():
pass
if not a.path:
raise Exception("\a[!] A* PATH NOT FOUND!")
def get_path(self, flags, start, end):
astar = AStar(flags, start, end)
astar.process()
return astar.path()
def amiCoSimulator():
"""simple AmiCo env interaction"""
print "Py: AmiCo Simulation Started:"
print "library found: "
#add syspath
new_path = os.path.abspath("./").lower()
java_path = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin").lower()
java_path2 = os.path.abspath("C:\\Program Files (x86)\\Java\\jre6\\bin\\client").lower()
sys.path.append(new_path)
sys.path.append(java_path)
sys.path.append(java_path2)
if (sys.platform == 'linux2'):
##########################################
# find_library on Linux could only be used if your libAmiCoPyJava.so is
# on system search path or path to the library is added in to LD_LIBRARY_PATH
#
# name = 'AmiCoPyJava'
# loadName = find_library(name)
##########################################
loadName = './libAmiCoPyJava.so'
libamico = ctypes.CDLL(loadName)
print libamico
else: #else if OS is a Mac OS X (libAmiCo.dylib is searched for) or Windows (AmiCo.dll)
name = 'AmiCoPyJava.DLL'
loadName = find_library(name)
print loadName
libamico = ctypes.CDLL(name)
print libamico
javaClass = "ch/idsia/benchmark/mario/environments/MarioEnvironment"
libamico.amicoInitialize(1, "-Djava.class.path=." + os.pathsep +":../lib/jdom.jar")
libamico.createMarioEnvironment(javaClass)
reset = cfunc('reset', libamico, None, ('list', ListPOINTER(c_int), 1))
getEntireObservation = cfunc('getEntireObservation', libamico, py_object,
('list', c_int, 1),
('zEnemies', c_int, 1))
performAction = cfunc('performAction', libamico, None, ('list', ListPOINTER(c_int), 1))
getEvaluationInfo = cfunc('getEvaluationInfo', libamico, py_object)
getObservationDetails = cfunc('getObservationDetails', libamico, py_object)
agent =AStar()
options = ""
if len(sys.argv) > 1:
options = sys.argv[1]
print "options: ", options
k = 1
seed =0
print "Py: ======Evaluation STARTED======"
totalIterations = 0
for i in range(k, k+100):
options1 = options + "-ls " + str(seed)+" -fps 30 -ld 0"
reset(options1)
obsDetails = getObservationDetails()
agent.setObservationDetails(obsDetails[0], obsDetails[1], obsDetails[2], obsDetails[3])
while (not libamico.isLevelFinished()):
totalIterations +=1
libamico.tick();
obs = getEntireObservation(1, 0)
agent.integrateObservation(obs[0], obs[1], obs[2], obs[3], obs[4]);
action = agent.getAction()
performAction(action);
evaluationInfo = getEvaluationInfo()
print evaluationInfo
seed += 1
