def send_prims_and_update_effective_times(self, the_car, path_prims):
if the_car.plate_number not in global_vars.all_cars:
global_vars.all_cars[the_car.plate_number] = the_car # add the car
for prim_id in path_prims:
global_vars.all_cars[the_car.plate_number].prim_queue.enqueue(
(prim_id, 0))
self._effective_times[the_car.plate_number] += get_prim_data(
prim_id, 't_end')[0]
def prim_next(self, dt):
"""
updates with primitive, if no primitive available, update with next with zero inputs
Inputs:
dt: integration time
Outputs:
None - the states of the car will get updated
"""
if self.extract_primitive(
) == False: # if there is no primitive to use
self.next((0, 0), dt)
else:
prim_id, prim_progress = self.extract_primitive()
# TODO: make this portion of the code more automated
if prim_id > -1:
# load primitive data
list_of_key = ['x0', 'x_ref', 'u_ref', 'alpha', 'K']
x0, x_ref, u_ref, alpha, K = get_bunch_prim_data(
prim_id, list_of_key)
if prim_progress == 0: # compute initial extended state
x_real = self.state.reshape((-1, 1))
x1 = x_real - x0
x2 = np.matmul(np.linalg.inv(diag([4, 0.02, 4, 4])), x1)
# initial state, consisting of actual and virtual states for the controller
self.extended_state = (np.vstack((x_real, x0, x1, x2)))[:,
0]
num_of_inputs = 2
G_u = np.diag(
[175, 1.29]
) # this diagonal matrix encodes the size of input set (a constraint)
dist = get_disturbance()
k = int(prim_progress *
params.num_subprims) # calculate primitive waypoint
q1 = K[k, 0].reshape((-1, 1), order='F')
q2 = 0.5 * (x_ref[:, k + 1] + x_ref[:, k]).reshape(-1, 1)
q3 = u_ref[:, k].reshape(-1, 1)
q4 = u_ref[:, k].reshape(-1, 1)
q5 = np.matmul(
G_u,
alpha[k * num_of_inputs:(k + 1) * num_of_inputs]).reshape(
(-1, 1), order='F')
# parameters for the controller
q = np.vstack((q1, q2, q3, q4, q5))
self.extended_state = odeint(func=prim_state_dot,
y0=self.extended_state,
t=[0, dt],
args=(dist, q))[-1, :]
self.state = self.extended_state[0:len(self.state)]
self.alive_time += dt
prim_progress = prim_progress + dt / get_prim_data(
prim_id, 't_end')[0]
self.prim_queue.replace_top((prim_id, prim_progress))
else: # if is stopping primitive
self.next((0, 0), dt)
def get_scheduled_intervals(self, path, effective_time, graph, complete_path=True):
path_prims = self.path_to_primitives(path)
scheduled_intervals = dict()
time_shift = effective_time
for prim_id in path_prims:
prim_time = get_prim_data(prim_id, 't_end')[0]
dt = prim_time / params.num_subprims
for subprim_id in range(params.num_subprims):
subprim = (prim_id, subprim_id)
subprim_interval = self.shift_interval((0, dt), time_shift)
scheduled_intervals[subprim] = subprim_interval
time_shift += dt
if not complete_path:
last_subprim = (path_prims[-1], params.num_subprims-1)
last_interval = scheduled_intervals[last_subprim]
scheduled_intervals[last_subprim] = (last_interval[0], float('inf'))
return path_prims, scheduled_intervals
def draw_monitor(last_state, current_state, exception, prim_id, person_spec,
car_spec, background):
test_pedestrian.monitor_pedestrians(last_state[2], current_state[2],
exception['pedestrian'])
test_light.monitor_light(last_state[0], last_state[1], current_state[0],
current_state[1])
test_car.monitor_vehicles(last_state[3], current_state[3],
exception['vehicle'])
pedestrian_fig = dir_path + '/imglib/pedestrian.png'
person_fig = Image.open(pedestrian_fig)
light_fig = dir_path + '/imglib/light.png'
light_fig = Image.open(light_fig)
car_fig = dir_path + '/imglib/car.png'
car_fig = Image.open(car_fig)
x0 = get_prim_data(prim_id, 'x0')[2:4]
xf = get_prim_data(prim_id, 'x_f')[2:4]
x0 = [int(x0[0]), int(x0[1])]
xf = [int(xf[0]), int(xf[1])]
fontpath1 = 'NotoSans-BoldItalic.ttf'
fontpath = 'AbhayaLibre-SemiBold.ttf'
font = ImageFont.truetype(fontpath, 28)
smallfont = ImageFont.truetype(fontpath, 23)
middlefont = ImageFont.truetype(fontpath, 23)
font1 = ImageFont.truetype(fontpath1, 25)
largefont = ImageFont.truetype(fontpath1, 40)
nofill = (0, 0, 0)
fill = (0, 100, 0)
color_person_spec = [nofill, nofill, nofill]
color_light_spec = [nofill, nofill, nofill, nofill, nofill]
color_car_spec = [nofill, nofill]
if person_spec in (1, 2, 3):
color_person_spec[person_spec - 1] = fill
if current_state[0] + current_state[1] == 'rg':
color_light_spec[0] = fill
color_light_spec[1] = fill
elif current_state[0] + current_state[1] == 'ry':
color_light_spec[0] = fill
color_light_spec[2] = fill
elif current_state[0] + current_state[1] == 'gr':
color_light_spec[0] = fill
color_light_spec[1] = fill
elif current_state[0] + current_state[1] == 'yr':
color_light_spec[0] = fill
color_light_spec[2] = fill
elif current_state[0] + current_state[1] == 'rr':
color_light_spec[0] = fill
color_light_spec[3] = fill
if car_spec in (1, 2):
color_car_spec[car_spec - 1] = fill
# check exception
if exception['vehicle']:
color_car_spec[1] = 'red'
car_fig = car_fig.resize((120, 120))
background.paste(car_fig, (500, 1150))
else:
#car_fig = car_fig.resize((740,220))
background.paste(car_fig, (410, 1100))
background.paste(person_fig, (650, 80))
background.paste(light_fig, (650, 435))
draw = ImageDraw.Draw(background)
draw.text((145, 230),
'\u25CA arrive = destination;',
fill=color_person_spec[0],
font=font)
draw.text((145, 270),
'(\u00AC g) -> (\u00AC cross the street);',
fill=color_person_spec[1],
font=font)
draw.text((145, 310),
'(t_cross >= t_w) -> \n(\u00AC cross the street).',
fill=color_person_spec[2],
font=font)
draw.text((145, 650), '\u25CB', fill=color_light_spec[0], font=largefont)
draw.text((190, 660), 'r;', fill=color_light_spec[0], font=font)
draw.text((167, 660), '30', fill=color_light_spec[0], font=smallfont)
draw.text((220, 650), '\u25CB', fill=color_light_spec[1], font=largefont)
draw.text((265, 660), 'g;', fill=color_light_spec[1], font=font)
draw.text((242, 660), '25', fill=color_light_spec[1], font=smallfont)
draw.text((295, 650), '\u25CB', fill=color_light_spec[2], font=largefont)
draw.text((337, 660), 'y;', fill=color_light_spec[2], font=font)
draw.text((317, 660), '5', fill=color_light_spec[2], font=smallfont)
draw.text((145, 700), '\u25A2', fill=color_light_spec[3], font=font1)
draw.text((160, 700), '\u25CA rr.', fill=color_light_spec[3], font=font)
draw.text((145, 1300),
'\u25CA arrive = destination;',
fill=color_car_spec[0],
font=font)
draw.text((145, 1330), '\u25A2', fill=color_car_spec[1], font=font1)
draw.text((160, 1330),
'\u00AC collision.',
fill=color_car_spec[1],
font=font)
def draw_scheduler_table(cars_to_keep, light_color, background, vehicle_state,
picked_car, prim_id, picked_ped):
fontpath = 'AbhayaLibre-SemiBold.ttf'
font = ImageFont.truetype(fontpath, 50)
draw = ImageDraw.Draw(background)
# Adding black text into scheduler table if no action
# Lights Contract Section
# change color of the text depending on light color
if light_color[0] == 'r':
draw.text((1000, 210), '?r_h', fill='red', font=font)
draw.text((440, 280), '!r_h', fill='red', font=font)
else:
draw.text((440, 280), '!r_h', fill='black', font=font)
draw.text((1000, 210), '?r_h', fill='black', font=font)
if light_color[1] == 'r':
draw.text((440, 210), '?r_v', fill='red', font=font)
draw.text((1000, 280), '!r_v', fill='red', font=font)
else:
draw.text((440, 210), '?r_v', fill='black', font=font)
draw.text((1000, 280), '!r_v', fill='black', font=font)
if light_color[3]:
draw.text((650, 280), '!h_walk', fill='green', font=font)
else:
draw.text((650, 280), '!h_walk', fill='black', font=font)
if light_color[2]:
draw.text((1210, 280), '!v_walk', fill='green', font=font)
else:
draw.text((1210, 280), '!v_walk', fill='black', font=font)
# Vehicle and Scheduler Section
# state CAR ID
if picked_car:
draw.text((270, 470), str(picked_car.id), fill='black', font=font)
draw.text((1000, 550), 'len(request_queue)', fill='green', font=font)
if picked_car:
car_xy = (picked_car.state[2], picked_car.state[3])
waiting_signal = False
# show if request queue has cars waiting
if scheduler.Scheduler.length:
draw.text((1000, 550),
'len(request_queue)',
fill='green',
font=font)
# check if car is done
if at_certain_position(car_xy, picked_car.destination):
draw.text((440, 550), 'not_done', fill='black', font=font)
else:
draw.text((440, 550), 'not_done', fill='green', font=font)
# show the reason for rejected request
if picked_car.state[0] < 2 and not at_certain_position(
car_xy, picked_car.destination):
if not picked_car.alive_time < 1:
draw.text((970, 710), '!reject', fill='red', font=font)
draw.text((380, 630), '?reject', fill='red', font=font)
draw.text((530, 630), '?accept', fill='black', font=font)
draw.text((700, 630), '?primitives', fill='black', font=font)
draw.text((1120, 710), '!accept', fill='black', font=font)
draw.text((1280, 710), '!primitives', fill='black', font=font)
if at_waiting_line(car_xy, 'horizontal'):
if light_color[0] == 'r':
draw.text((440, 790), 'Red light', fill='red', font=font)
waiting_signal = True
elif light_color[0] == 'y':
draw.text((440, 790),
'Not enough time',
fill='red',
font=font)
waiting_signal = True
elif at_waiting_line(car_xy, 'vertical'):
if light_color[1] == 'r':
draw.text((440, 790), 'Red light', fill='red', font=font)
waiting_signal = True
elif light_color[1] == 'y':
draw.text((440, 790),
'Not enough time',
fill='red',
font=font)
waiting_signal = True
if not waiting_signal: # if not because of waiting for the signal
draw.text((440, 790), 'Not clear', fill='red', font=font)
else:
draw.text((530, 630), '?accept', fill='green', font=font)
draw.text((700, 630), '?primitives', fill='green', font=font)
draw.text((1120, 710), '!accept', fill='green', font=font)
draw.text((1280, 710), '!primitives', fill='green', font=font)
draw.text((970, 710), '!reject', fill='black', font=font)
draw.text((380, 630), '?reject', fill='black', font=font)
if prim_id != -1:
x0 = get_prim_data(prim_id, 'x0')[2:4]
xf = get_prim_data(prim_id, 'x_f')[2:4]
x0 = [int(x0[0]), int(x0[1])]
xf = [int(xf[0]), int(xf[1])]
draw.text((440, 790),
'Guiding prim_id = ' + str(prim_id),
fill=(0, 100, 0),
font=font)
draw.text((950, 790),
str(x0) + '->' + str(xf),
fill=(0, 100, 0),
font=font)
# check if the picked car is making a request
if scheduler.Scheduler.check_car_request:
draw.text((1000, 630), '?request', fill='green', font=font)
draw.text((440, 710), '!request', fill='green', font=font)
else: # if no car is picked, all text is black
draw.text((440, 710), '!request', fill='black', font=font)
draw.text((380, 630), '?reject', fill='black', font=font)
draw.text((530, 630), '?accept', fill='black', font=font)
draw.text((700, 630), '?primitives', fill='black', font=font)
#draw.text((1000,550), 'len(request_queue)',fill='black',font=font)
draw.text((1000, 630), '?request', fill='black', font=font)
draw.text((970, 710), '!reject', fill='black', font=font)
draw.text((1120, 710), '!accept', fill='black', font=font)
draw.text((1280, 710), '!primitives', fill='black', font=font)
draw.text((440, 550), 'not_done', fill='black', font=font)
if picked_ped:
person_xy = (picked_ped.state[0], picked_ped.state[1])
#if person_xy in (pedestrian_graph.lane1 + pedestrian_graph.lane2) and light_color[0] == 'g':#+ pedestrian_graph.lane3 + pedestrian_graph.lane4):
# draw.text((380,1140), '?h_walk',fill='green',font=font)
if picked_ped.monitor_state in (2, 3, 4):
draw.text((380, 1140), '?h_walk', fill='black', font=font)
draw.text((800, 1220), '!h_walk', fill='black', font=font)
draw.text((560, 1140), '?v_walk', fill='black', font=font)
draw.text((1170, 1220), '!v_walk', fill='black', font=font)
draw.text((440, 1060), 't_cross', fill='black', font=font)
draw.text((850, 1060), 'h_timer', fill='black', font=font)
if picked_ped.state[2] in (
-pi / 2,
pi / 2) and light_color[2]: # walking S-N left side
draw.text((560, 1140), '?v_walk', fill='green', font=font)
draw.text((1170, 1220), '!v_walk', fill='green', font=font)
draw.text((440, 1060), 't_cross', fill='green', font=font)
draw.text((1200, 1060), 'v_timer', fill='green', font=font)
draw.text((440, 1300),
'Crossing vertically',
fill='green',
font=font)
# else:
# draw.text((560,1140), '?v_walk',fill='black',font=font)
# draw.text((1170,1220), '!v_walk',fill='black',font=font)
# draw.text((440,1060), 't_cross',fill='black',font=font)
# draw.text((1200,1060), 'v_timer',fill='black',font=font)
if picked_ped.state[2] in (pi,
0) and light_color[3]: # walking E-W
draw.text((380, 1140), '?h_walk', fill='green', font=font)
draw.text((800, 1220), '!h_walk', fill='green', font=font)
draw.text((440, 1060), 't_cross', fill='green', font=font)
draw.text((850, 1060), 'h_timer', fill='green', font=font)
draw.text((440, 1300),
'Crossing horizontally',
fill='green',
font=font)
# else:
# draw.text((380,1140), '?h_walk',fill='black',font=font)
# draw.text((800,1220), '!h_walk',fill='black',font=font)
# draw.text((440,1060), 't_cross',fill='black',font=font)
# draw.text((850,1060), 'h_timer',fill='black',font=font)
if picked_ped.monitor_state == 4:
draw.text((440, 1300),
'Waiting at the signal',
fill='red',
font=font)
if picked_ped.monitor_state == 3:
draw.text((440, 1300),
'Not enough time',
fill='black',
font=font)
draw.text((440, 1060), 't_cross', fill='red', font=font)
if picked_ped.state[2] in (-pi / 2, pi / 2):
draw.text((1200, 1060), 'v_timer', fill='red', font=font)
if picked_ped.state[2] in (pi, 0):
draw.text((850, 1060), 'h_timer', fill='red', font=font)
else:
draw.text((440, 1300), 'Walking', fill='black', font=font)
draw.text((440, 1060), 't_cross', fill='black', font=font)
draw.text((380, 1140), '?h_walk', fill='black', font=font)
draw.text((560, 1140), '?v_walk', fill='black', font=font)
draw.text((850, 1060), 'h_timer', fill='black', font=font)
draw.text((1200, 1060), 'v_timer', fill='black', font=font)
draw.text((800, 1220), '!h_walk', fill='black', font=font)
draw.text((1000, 1220), '!r_h', fill='black', font=font)
draw.text((1170, 1220), '!v_walk', fill='black', font=font)
draw.text((1400, 1220), '!r_v', fill='black', font=font)
else:
# Pedestrian and light section in black if there is no pedestrian chosen
draw.text((440, 1060), 't_cross', fill='black', font=font)
draw.text((380, 1140), '?h_walk', fill='black', font=font)
draw.text((560, 1140), '?v_walk', fill='black', font=font)
draw.text((850, 1060), 'h_timer', fill='black', font=font)
draw.text((1200, 1060), 'v_timer', fill='black', font=font)
draw.text((800, 1220), '!h_walk', fill='black', font=font)
draw.text((1000, 1220), '!r_h', fill='black', font=font)
draw.text((1170, 1220), '!v_walk', fill='black', font=font)
draw.text((1400, 1220), '!r_v', fill='black', font=font)
start, end, weight = edge
# check source
x = start[2]
y = start[3]
has_source = x < 50 or x > 1000 or y > 700 or y < 50
if has_source:
G._sources.add(start)
x = end[2]
y = end[3]
has_sink = x < 50 or x > 1000 or y > 700 or y < 50
if has_sink:
G._sinks.add(end)
for prim_id in range(load_primitives.num_of_prims):
controller_found = get_prim_data(prim_id, 'controller_found')[0]
if controller_found and prim_id not in ignore_set:
from_node = tuple(get_prim_data(prim_id, 'x0'))
to_node = tuple(get_prim_data(prim_id, 'x_f'))
time_weight = get_prim_data(prim_id, 't_end')[0]
new_edge = (from_node, to_node, time_weight)
label_set = [prim_id]
G.add_edges([new_edge],
use_euclidean_weight=False,
label_edges=True,
edge_label_set=label_set)
add_source_sink(new_edge)
if visualize:
import matplotlib.pyplot as plt
from PIL import Image