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