def plot_boxes_monitor(ax, all_components):
global_vars.boxes_monitor = [ax.plot([], [], 'orangered')[0] for _ in range(len(all_components))]
for i in range(len(all_components)):
curr_comp = all_components[i]
vertex_set,_,_,_ = get_bounding_box(curr_comp)
xs = [vertex[0] for vertex in vertex_set]
ys = [vertex[1] for vertex in vertex_set]
xs.append(vertex_set[0][0])
ys.append(vertex_set[0][1])
global_vars.boxes_monitor[i].set_data(xs,ys)
def plot_boxes(ax, cars_to_keep):
global_vars.boxes = [ax.plot([], [], 'c')[0] for _ in range(len(cars_to_keep))]
for i in range(len(cars_to_keep)):
curr_car = cars_to_keep[i]
vertex_set,_,_,_ = get_bounding_box(curr_car)
xs = [vertex[0] for vertex in vertex_set]
ys = [vertex[1] for vertex in vertex_set]
xs.append(vertex_set[0][0])
ys.append(vertex_set[0][1])
if with_probability(1):
global_vars.boxes[i].set_data(xs,ys)
for j in range(i + 1, len(cars_to_keep)):
if not collision_free(curr_car, cars_to_keep[j]):
global_vars.boxes[j].set_color('r')
global_vars.boxes[i].set_color('r')
def animate(frame_idx): # update animation by dt
ax.cla() # clear Axes before plotting
print(frame_idx)
""" online frame update """
global background
# update traffic lights
traffic_lights.update(dt)
horizontal_light = traffic_lights.get_states('horizontal', 'color')
vertical_light = traffic_lights.get_states('vertical', 'color')
# update background
# TODO: implement option to lay waypoint graph over background
background = Image.open(intersection_fig + horizontal_light + '_' +
vertical_light + '.png')
x_lim, y_lim = background.size
# update pedestrians
for person in pedestrians:
if (person.state[0] <= x_lim and person.state[0] >= 0
and person.state[1] >= 0 and person.state[1] <= y_lim):
person.prim_next(dt)
draw_pedestrian(person)
# update planner
# TODO: integrate planner
# update enemy cars
corners = []
for vehicle in enemy_cars:
nu = 0
acc = 0
if (vehicle.state[2] >= 0 and vehicle.state[3] >= 0
and vehicle.state[2] <= x_lim and vehicle.state[3] <= y_lim):
if random.random() > 0.1:
nu = random.uniform(-0.02, 0.02)
acc = random.uniform(-5, 10)
vehicle.next((acc, nu), dt)
xc, yc = draw_car(vehicle)
if np.random.uniform() < 0.5:
corners = ax.plot(xc, yc, 'ro')
if frame_idx > delay_time:
for vehicle in delayed_enemy_cars:
nu = 0
acc = 0
if (vehicle.state[2] >= 0 and vehicle.state[3] >= 0
and vehicle.state[2] <= x_lim
and vehicle.state[3] <= y_lim):
if random.random() > 0.1:
nu = random.uniform(-0.02, 0.02)
acc = random.uniform(-5, 10)
vehicle.next((acc, nu), dt)
draw_car(vehicle)
if frame_idx <= delay_time:
for vehicle in waiting_enemy_cars:
nu = 0
acc = 0
vehicle.next((acc, nu), dt)
draw_car(vehicle)
## update controlled cars with primitives
for vehicle in controlled_cars:
vehicle.prim_next(dt=dt)
if vehicle.prim_queue.len() > 0:
draw_car(vehicle)
#collision check
#boxes = []
all_components = controlled_cars + enemy_cars + waiting_enemy_cars + pedestrians
# initialize boxes
boxes = [ax.plot([], [], 'g')[0] for _ in range(len(all_components))]
for i in range(len(all_components)):
curr_comp = all_components[i]
vertex_set, _, _, _ = get_bounding_box(curr_comp)
xs = [vertex[0] for vertex in vertex_set]
ys = [vertex[1] for vertex in vertex_set]
xs.append(vertex_set[0][0])
ys.append(vertex_set[0][1])
boxes[i].set_data(xs, ys)
for j in range(i + 1, len(all_components)):
collision_free1, min_sep_vector = collision_free(
all_components[i], all_components[j]
) # returns True if collision free and an empty vector, else returns False and the min vector needed to separate the objects
if not collision_free1: # had to change variable name from the function to remove error
print("Collision, object indices:")
print(i, j)
cp = contact_points(all_components[i], all_components[j],
min_sep_vector)
print(cp)
if len(cp) > 0:
motions = collision_response(all_components[i],
all_components[j], cp,
min_sep_vector)
print(motions)
all_components[i].next((0.5, motions[1]), dt)
draw_car(all_components[i])
all_components[j].next((0.5, motions[3]), dt)
draw_car(all_components[j])
boxes[j].set_color('r')
boxes[i].set_color('r')
stage = ax.imshow(background,
origin="lower") # this origin option flips the y-axis
return [
stage
] + boxes + corners # returned object must be iterable, a requirement of FuncAnimation