def run_simulator(pywindow, robots, paths_, costs_, colors, buttons): # display robot movement along paths
paths, costs = eliminate_collision_paths(paths_, costs_, robots) # erase active bots that don't have coll_free path
del paths_, costs_
for i in range(len(paths)):
display_path(paths[i], pywindow, colors[robots[i]])
time.sleep(.2)
positions = get_new_path_list(robots, paths, costs) # get discrete values with same time_step for all bots
for i in range(len(positions[0])): # for number of time_steps
for j in range(len(robots)): # for each robot
x = positions[j][i].x
y = positions[j][i].y
if i > 0: # if not first point
x_prev = positions[j][i - 1].x # "erase" previous point
y_prev = positions[j][i - 1].y
pygame.draw.circle(pywindow, Colors.white, (int(x_prev), int(y_prev)), Settings.robo_size, 0)
pygame.draw.circle(pywindow, colors[robots[j]], (int(x), int(y)), Settings.robo_size, 0) # display new point
pygame.display.flip()
time.sleep(Settings.simulation_speed)
print 'Done 2D simulation'
call_gazebo = wait_for_gazebo_call(buttons)
if call_gazebo is True:
label_button(pywindow, buttons[1], 1, 'Running 3D', Colors.dark_green, Colors.white)
pygame.display.flip()
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
print 'Running file that does not connect to Gazebo. Find the one that does. :)'
# call gazebo
return
def better_response(
pi_, goalpts, vertex, path_prev_i, pywindow, costs_i, i,
color_): # minimize cost while avoiding collisions with paths in pi_
if vertex is not None:
if vertex.at_goal_set: # only obtain new paths if new pt is at goal
#print 'new vertex at goal set, calling path_gen from better_response procedure'
path_list_vertex, cost_list_vertex = path_generation2(vertex)
collision_free_paths = []
costs = []
for j in range(len(goalpts)):
for k in range(len(goalpts[j].paths)):
#print 'goal path:', goalpts[j].paths[k]
#print 'pi 0:', pi_[0]
#print 'pi 1:', pi_[1]
if collision_free_path(goalpts[j].paths[k], pi_, i) == 1:
#print 'path is collision free'
collision_free_paths.append(goalpts[j].paths[k])
costs.append(goalpts[j].costs[k])
if len(collision_free_paths) == 0:
print 'paths exist but no collision free paths for robot', i
return [], 9999999
optimal_path = list(path_prev_i)
optimal_cost = costs_i
optimal_path, optimal_cost, changed = find_optimal_path(
collision_free_paths, costs, optimal_path, optimal_cost, i)
if changed:
display_path(path_prev_i, pywindow, Colors.black)
display_path(optimal_path, pywindow, color_)
return optimal_path, optimal_cost # feasible paths here is list of paths for one robot
def better_response(pi_, goalpts, vertex, path_prev_i, pywindow, costs_i, i,
color_, new_paths, goal, root, time_to_path):
if vertex is not None:
if vertex.at_goal_set and vertex.tree_num == 1: # only obtain new paths if new pt is at goal
path_list_vertex, cost_list_vertex = path_generation2(
vertex,
root) # updates vertex objects to contain path/cost list
elif new_paths is True:
path_list_vertex, cost_list_vertex = path_generation2(
goal, root) # if new path has formed between trees,
collision_free_paths = []
costs = []
for j in range(len(goalpts) + 1):
if j == len(goalpts):
#print 'number of paths from exact goal point', len(goal.paths)
for k in range(len(goal.paths)):
if collision_free_path(goal.paths[k], pi_, i) == 1:
collision_free_paths.append(goal.paths[k])
costs.append(goal.costs[k])
else:
for k in range(len(goalpts[j].paths)):
if collision_free_path(goalpts[j].paths[k], pi_, i) == 1:
collision_free_paths.append(goalpts[j].paths[k])
costs.append(goalpts[j].costs[k])
if len(collision_free_paths) == 0:
print 'paths exist but no inter-robot-collision-free paths for robot', i, 'yet'
return [], Settings.collision_cost
optimal_path = list(path_prev_i)
optimal_cost = costs_i
optimal_path, optimal_cost, changed = find_optimal_path(
collision_free_paths, costs, optimal_path, optimal_cost, i)
if path_prev_i == [] and len(optimal_path) > 0:
time_to_path[i] = time.time()
if changed:
#print 'path_prev:'
#for k in range(len(path_prev_i)):
#print path_prev_i[k].x, path_prev_i[k].y
#print 'path now:'
#for k in range(len(optimal_path)):
#print optimal_path[k].x, optimal_path[k].y
display_path(path_prev_i, pywindow, Colors.white)
display_path(optimal_path, pywindow, color_)
return optimal_path, optimal_cost # feasible paths here is list of paths for one robot
def main():
pywindow, obstacles, axis = init_pywindow(
'i-Nash Trajectory Final 4'
) # 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)
if vertex_new is not None:
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