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