def check_active(new_vertices, i, active_bots, inactive_bots):
if new_vertices[i] is not None:
if new_vertices[
i].at_goal_set: # if previously inactive bot is now active
update_active(active_bots, inactive_bots,
i) # then update the inactive and active lists
print 'Robot', i, 'became active. Active robots:', active_bots
return
def main():
pywindow, obstacles, axis = init_pywindow(
'i-Nash Trajectory Final 3'
) # set up pygame window, dimensions and obstacles
start, goal_set, num_robots, robo_colors = user_prompt(
pywindow) # prompt for num bots, start, end positions
all_bots, active_bots, inactive_bots, paths, costs, paths_prev, goal_pts, path_num = init_arrays(
num_robots)
k = 1
k_ = 75000
while k < k_: # main loop
new_vertices = [
None
] * num_robots # get list of new vertices each k iteration
for i in all_bots: # for all robots
vertex_rand = sample_free(
paths[i], goal_set[i]) # get random Vertex in pywindow
vertex_new = extend_graph(vertex_rand, start[i], obstacles,
goal_set[i], pywindow, robo_colors[i], k,
axis)
goal_pts, new_vertices = update_pt_lists(vertex_new, goal_pts,
new_vertices, i)
for i in inactive_bots:
if new_vertices[i] is not None:
if new_vertices[
i].at_goal_set: # if previously inactive bot is now active
update_active(
active_bots, inactive_bots,
i) # then update the inactive and active lists
print 'robot', i, 'became active'
for i in active_bots:
paths_prev[i] = list(
paths[i]) # save previous path list before updating list
for q in range(len(
active_bots)): # use q for easier j < i and j2 > i calculation
perform_better_response(q, active_bots, paths_prev, paths, costs,
pywindow, new_vertices, goal_pts,
robo_colors)
#time.sleep(.05)
k = k + 1
print 'main loop exited'
"""repeat = False
for i in range(num_robots): # for all bots
for j in range(len(goal_pts[i])): # for all goal pts for that bot
for P in range(len(goal_pts[i][j].paths)): # for all points for that goal pt
string = []
string.append(goal_pts[i][j].costs[P])
for F in range(len(goal_pts[i][j].paths[P])):
string.append(floor(goal_pts[i][j].paths[P][F].x))
string.append(floor(goal_pts[i][j].paths[P][F].y))
#print 'vertex.paths for all vertices in path', P, ':', goal_pts[i][j].paths[P][F].paths
for F2 in range(len(goal_pts[i][j].paths[P])):
if goal_pts[i][j].paths[P][F] == goal_pts[i][j].paths[P][F2]:
repeat = True
#print 'robot', i, ', goalpt', j, ',path', P, 'cost', string
display_path(goal_pts[i][j].paths[P], pywindow, Colors.dark_green) # display
#print 'repeat is ', repeat"""
return