def __init__(self, data_file, index, x=0, y=0, angle=0.0, length=4, max_steering=30, max_acceleration=30.0):
current_dir = os.path.dirname(os.path.abspath(__file__)).replace('\\', '/')
super().__init__(x, y, angle, length, max_steering, max_acceleration)
self.data = read_coords(os.path.join(current_dir, "resources/traffic_cars_data/" + data_file). replace('\\', '/'))
self.data_x = [x[0] for x in self.data]
self.data_y = [x[1] for x in self.data]
self.index = index
def replay(self, car_data_path, enable_trajectory=False):
"""
Deprecated and will be removed
:param car_data_path:
:param enable_trajectory:
:return:
"""
if os.path.exists(car_data_path) is False:
raise OSError('car_data_path does not exists')
if self.traffic is True:
self.init_traffic_cars()
if self.print_activations is True:
desired_layer_output = "convolution0"
layer_names, image_buf, state_buf, activation_model = self.initialize_activation_model(
desired_layer_output)
# place car on road
car_data = read_coords(car_data_path)
print(car_data)
self.car = Car(car_data[0][0], car_data[0][1])
index = 1
activation_mask = pygame.Surface(
(self.screen_width, self.screen_height))
while not self.exit:
if len(car_data) - 50 <= index <= len(car_data) - 10:
index = 1
if self.traffic is True:
collision_list = [False] * len(self.traffic_list)
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
self.key_handler()
self.car, index = self.drive(self.car, car_data, index)
stage_pos = self.draw_sim_environment(print_coords=True)
stage_pos = (stage_pos[0], stage_pos[1])
if self.traffic is True:
self.check_collisions(collision_list)
self.traffic_movement(collision_list, stage_pos)
if self.sensors is True:
activation_mask.fill((0, 0, 0))
self.activate_sensors()
if self.print_activations is True:
image_rect = pygame.Rect((390, 110), (500, 500))
sub = activation_mask.subsurface(image_rect)
self.input_image = pygame.surfarray.array3d(sub)
image_buf[0] = self.input_image
activations = activation_model.predict([image_buf, state_buf])
print_activations(activations, layer_names,
desired_layer_output)
# draw trajectory
if enable_trajectory is True:
self.draw_trajectory(self.car, car_data, index)
pygame.display.update()
self.clock.tick(60)
pygame.quit()
def record_from_replay(self,
replay_csv_path,
save_simulator_frame_path=None,
save_sensor_frame_path=None,
save_debug_frame_path=None,
save_sensor_frame=False,
save_simulator_frame=False,
save_debug_frame=False,
draw_trajectory=False,
traffic=False,
display_obstacle_on_screen=False):
"""
Record images from runs
:param replay_csv_path: path to replay csv
:param save_simulator_frame_path: path where to save simulator frame
:param save_sensor_frame_path: path where to save sensor frame
:param save_debug_frame_path: path where to save debug(simulator and sensor) frame
:param save_sensor_frame: bool
:param save_simulator_frame: bool
:param save_debug_frame: bool
:param draw_trajectory: bool
:param traffic: bool(if run was with traffic)
:param display_obstacle_on_screen
:return:
"""
if traffic is True:
self.init_traffic_cars()
if os.path.exists(replay_csv_path) is False:
raise OSError("path to replay csv doesn't exists")
car_data = read_coords(replay_csv_path)
index = 0
activation_mask = pygame.Surface(
(self.screen_width, self.screen_height))
while not self.exit:
collision_list = [False] * len(self.traffic_list)
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
self.car.position = (car_data[index][0], car_data[index][1])
self.car.angle = car_data[index][2]
# index = (index + 1) % len(car_data)
index = index + 1
if index >= len(car_data):
quit()
# Logic
# car.update(dt)
# Drawing
stagePosX = self.car.position[0] * self.ppu
stagePosY = self.car.position[1] * self.ppu
rel_x = stagePosX % self.bgWidth
rel_y = stagePosY % self.bgHeight
# blit (BLock Image Transfer) the seamless background
self.screen.blit(self.background,
(rel_x - self.bgWidth, rel_y - self.bgHeight))
self.screen.blit(self.background, (rel_x, rel_y))
self.screen.blit(self.background, (rel_x - self.bgWidth, rel_y))
self.screen.blit(self.background, (rel_x, rel_y - self.bgHeight))
rotated = pygame.transform.rotate(self.car_image, self.car.angle)
center_x = int(self.screen_width / 2) - int(
rotated.get_rect().width / 2)
center_y = int(self.screen_height / 2) - int(
rotated.get_rect().height / 2)
# draw the ego car
self.screen.blit(rotated, (center_x, center_y))
# self.optimized_front_sensor(car)
stagePos = (stagePosX, stagePosY)
if traffic is True:
self.check_collisions(collision_list)
self.traffic_movement(collision_list, stagePos)
activation_mask.fill((0, 0, 0))
self.optimized_front_sensor(
activation_mask,
display_obstacle_on_sensor=display_obstacle_on_screen)
self.optimized_rear_sensor(
activation_mask,
display_obstacle_on_sensor=display_obstacle_on_screen)
image_name = 'image_' + str(index) + '.png'
image_rect = pygame.Rect((440, 160), (400, 400))
if draw_trajectory is True:
center_screen = (int(self.screen_width / 2),
int(self.screen_height / 2))
trajectory_points = []
for add_elem in range(5, 50, 10):
delta_position = (self.car.position[0] -
car_data[index + add_elem][0],
self.car.position[1] -
car_data[index + add_elem][1])
traj_point = (center_screen[0] +
int(delta_position[0] * self.ppu),
center_screen[1] +
int(delta_position[1] * self.ppu))
trajectory_points.append(traj_point)
# draw lines between trajectory points
for traj_point, next_traj_point in zip(trajectory_points,
trajectory_points[1:]):
pygame.draw.line(activation_mask, (255, 0, 0), traj_point,
next_traj_point, 3)
if save_sensor_frame is True:
if save_sensor_frame_path is None:
print('no path for sensor images given')
quit()
if os.path.exists(save_sensor_frame_path) is False:
os.makedirs(save_sensor_frame_path)
activation_sub = activation_mask.subsurface(image_rect)
activation_sub = resize_image(activation_sub, (200, 200))
save_frame(activation_sub, image_name, save_sensor_frame_path)
if save_simulator_frame is True:
if save_simulator_frame_path is None:
print('no path for simulator images given')
quit()
if os.path.exists(save_simulator_frame_path) is False:
os.makedirs(save_simulator_frame_path)
sub = self.screen.subsurface(image_rect)
sub = pygame.transform.scale(sub, (200, 200))
save_frame(sub, image_name, save_simulator_frame_path)
if save_debug_frame is True:
if save_debug_frame_path is None:
print('no path for sensor images given')
quit()
if os.path.exists(save_debug_frame_path) is False:
os.makedirs(save_debug_frame_path)
activation_sub = activation_mask.subsurface(image_rect)
activation_np = pygame.surfarray.array3d(activation_sub)
sub = self.screen.subsurface(image_rect)
sub_np = pygame.surfarray.array3d(sub)
full_image = np.vstack((sub_np, activation_np))
surf = pygame.surfarray.make_surface(full_image)
save_frame(surf, image_name, save_debug_frame_path)
pygame.display.update()
self.clock.tick(60)
pygame.quit()
def run(self, car_tags=[]):
# initialize cars
self.init_kinematic_cars()
# initialize traffic
if self.traffic is True:
self.init_traffic_cars()
rs_pos_list = [[6, 27, 0.0], [5, 27, 180.0], [4, 24, 180.0], [4, 23, 0.0], [5, 27, 90.0], [5, 27, 0.0]]
# boolean variable needed to check for single-click press
mouse_button_pressed = False
if self.record_data is True:
index_image = 0
"""
TEMP
"""
self.mpc_coords_car1[0] = self.kinematic_cars['car_1'].position[0]
self.mpc_coords_car1[1] = self.kinematic_cars['car_1'].position[1]
self.mpc_coords_car2[0] = self.kinematic_cars['car_2'].position[0]
self.mpc_coords_car2[1] = self.kinematic_cars['car_2'].position[1]
print(self.mpc_coords_car1)
print(self.mpc_coords_car2)
MUL_FACTOR = 1
car1_data = read_coords("resources/replay_car_1.csv")
car2_data = read_coords("resources/replay_car_2.csv")
car1_data = np.multiply(car1_data, MUL_FACTOR)
car2_data = np.multiply(car2_data, MUL_FACTOR)
thread = threading.Thread(target=self.mpc_thread, args=())
thread.start()
while not self.exit:
# VARIABLE_UPDATE
if self.traffic is True:
collision_list = [False] * len(self.traffic_list)
self.dt = self.clock.get_time() / 1000
self.event_handler(mouse_button_pressed)
# determine the current car, in case you need to control a specific one from the pool
self.determine_current_car()
car = self.kinematic_cars[self.current_car]
# LOGIC
self.key_handler(car, self.dt, rs_pos_list)
# FOR EXAMPLE 1 AND 2
# self.custom(car_tags, 'example_') # run examples 1, 2 from here
# DRAWING
stage_pos = self.draw_sim_environment(car, print_coords=True, print_other_cars_coords=False)
stage_pos = (stage_pos[0], stage_pos[1])
# UPDATE
# ------------------------ traffic car -----------------------------------------------
if self.traffic is True:
self.check_collisions(collision_list)
self.traffic_movement(collision_list, stage_pos)
# -------------------------------------------------------------------------------------------
# update the current and the others cars
self.kinematic_cars['car_1'].steering = np.rad2deg(self.mpc_delta_car1)
self.kinematic_cars['car_2'].steering = np.rad2deg(self.mpc_delta_car2)
self.kinematic_cars['car_1'].acceleration = self.mpc_acc_car1
self.kinematic_cars['car_2'].acceleration = self.mpc_acc_car2
car.update(self.dt)
self.update_cars()
self.draw_trajectory(self.kinematic_cars['car_1'], car1_data)
self.draw_trajectory(self.kinematic_cars['car_2'], car2_data)
self.mpc_coords_car1[0] = self.kinematic_cars['car_1'].position[0]
self.mpc_coords_car1[1] = self.kinematic_cars['car_1'].position[1]
self.mpc_coords_car2[0] = self.kinematic_cars['car_2'].position[0]
self.mpc_coords_car2[1] = self.kinematic_cars['car_2'].position[1]
self.mpc_angle_car1 = self.kinematic_cars['car_1'].angle
self.mpc_angle_car2 = self.kinematic_cars['car_2'].angle
self.activate_sensors(car)
self.activate_sensors_for_all_cars()
# CUSTOM FUNCTION TAB -> CHECK FUNCTION FOR EXAMPLES
# self.custom(car_tags, 'example_4') # run examples 3, 4 from here
# RECORD TAB
if self.record_data is True:
self.record_data_function(car_tags, index_image)
pygame.display.update()
self.clock.tick(self.ticks)
pygame.quit()
def run(self):
rs_pos_list = [[6, 27, 0.0], [5, 27, 180.0], [4, 24, 180.0], [4, 23, 0.0], [5, 27, 90.0], [5, 27, 0.0]]
# boolean variable needed to check for single-click press
mouse_button_pressed = False
if self.record_data is True:
index_image = 0
"""
TEMP
"""
self.mpc_coords_car1[0] = self.kinematic_cars['car_1'].position[0]
self.mpc_coords_car1[1] = self.kinematic_cars['car_1'].position[1]
self.mpc_coords_car2[0] = self.kinematic_cars['car_2'].position[0]
self.mpc_coords_car2[1] = self.kinematic_cars['car_2'].position[1]
MUL_FACTOR = 1
car1_data = read_coords("resources/highway_replay_car_1.csv")
car2_data = read_coords("resources/highway_replay_car_2.csv")
car1_data = np.multiply(car1_data, MUL_FACTOR)
car2_data = np.multiply(car2_data, MUL_FACTOR)
thread = threading.Thread(target=self.mpc_thread, args=())
thread.start()
while not self.exit:
# VARIABLE_UPDATE
self.dt = self.clock.get_time() / 1000
self.event_handler(mouse_button_pressed)
# determine the current car, in case you need to control a specific one from the pool
self.determine_current_car()
car = self.kinematic_cars[self.current_car]
# LOGIC
self.key_handler(car, self.dt, rs_pos_list)
# DRAWING
stage_pos = self.draw_sim_environment(car, print_coords=True, print_other_cars_coords=False)
stage_pos = (stage_pos[0], stage_pos[1])
self.kinematic_cars['car_1'].steering = np.rad2deg(self.mpc_delta_car1)
self.kinematic_cars['car_2'].steering = np.rad2deg(self.mpc_delta_car2)
self.kinematic_cars['car_1'].acceleration = self.mpc_acc_car1
self.kinematic_cars['car_2'].acceleration = self.mpc_acc_car2
# UPDATE
car.update(self.dt)
self.update_cars()
self.draw_trajectory(self.kinematic_cars['car_1'], car1_data)
self.draw_trajectory(self.kinematic_cars['car_2'], car2_data)
self.mpc_coords_car1[0] = self.kinematic_cars['car_1'].position[0]
self.mpc_coords_car1[1] = self.kinematic_cars['car_1'].position[1]
self.mpc_coords_car2[0] = self.kinematic_cars['car_2'].position[0]
self.mpc_coords_car2[1] = self.kinematic_cars['car_2'].position[1]
self.mpc_angle_car1 = self.kinematic_cars['car_1'].angle
self.mpc_angle_car2 = self.kinematic_cars['car_2'].angle
self.avoid_collisions()
self.generate_new_cars()
self.update_highway_traffic()
self.correct_traffic()
self.activate_sensors(car)
self.activate_sensors_for_all_cars()
# CUSTOM FUNCTION TAB FOR FURTHER ADDITIONS
self.custom()
# RECORD TAB
if self.record_data is True:
self.record_data_function(index_image)
pygame.display.update()
self.clock.tick(self.ticks)
pygame.quit()
def run_mpc(self):
# place car on road
global MUL_FACTOR
car = Car(MUL_FACTOR*5, MUL_FACTOR*27)
car.max_velocity = 10
self.mpc_coords[0] = car.position[0]
self.mpc_coords[1] = car.position[1]
car_data = read_coords("resources/mpc_ref.csv")
car_data = np.multiply(car_data, MUL_FACTOR)
thread = Thread(target=self.run_dll, args=())
thread.start()
while not self.exit:
dt = self.clock.get_time() / 1000
# User input
pressed = pygame.key.get_pressed()
# Event queue
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.exit = True
if pressed[pygame.K_UP]:
if car.velocity.x < 0:
car.acceleration = car.brake_deceleration
else:
car.acceleration += 10 * dt
elif pressed[pygame.K_DOWN]:
if car.velocity.x > 0:
car.acceleration = -car.brake_deceleration
else:
car.acceleration -= 10 * dt
elif pressed[pygame.K_SPACE]:
if abs(car.velocity.x) > dt * car.brake_deceleration:
car.acceleration = -copysign(car.brake_deceleration, car.velocity.x)
else:
car.acceleration = -car.velocity.x / dt
else:
if abs(car.velocity.x) > dt * car.free_deceleration:
car.acceleration = -copysign(car.free_deceleration, car.velocity.x)
else:
if dt != 0:
car.acceleration = -car.velocity.x / dt
car.acceleration = max(-car.max_acceleration, min(car.acceleration, car.max_acceleration))
if pressed[pygame.K_RIGHT]:
car.steering -= 180 * dt
elif pressed[pygame.K_LEFT]:
car.steering += 180 * dt
else:
car.steering = 0
car.steering = np.rad2deg(self.mpc_delta)
#print(self.mpc_delta)
car.acceleration = self.mpc_acc
car.steering = max(-car.max_steering, min(car.steering, car.max_steering))
# Logic
car.update(dt)
# Drawing
stagePosX = car.position[0] * self.ppu
stagePosY = car.position[1] * self.ppu
rel_x = stagePosX % self.bgWidth
rel_y = stagePosY % self.bgHeight
# blit (BLock Image Transfer) the seamless background
self.screen.blit(self.background, (rel_x - self.bgWidth, rel_y - self.bgHeight))
self.screen.blit(self.background, (rel_x, rel_y))
self.screen.blit(self.background, (rel_x - self.bgWidth, rel_y))
self.screen.blit(self.background, (rel_x, rel_y - self.bgHeight))
rotated = pygame.transform.rotate(self.car_image, car.angle)
center_x = int(self.screen_width / 2) - int(rotated.get_rect().width / 2)
center_y = int(self.screen_height / 2) - int(rotated.get_rect().height / 2)
# draw the ego car
self.screen.blit(rotated, (center_x, center_y))
self.draw_trajectory(car, car_data)
self.mpc_coords[0] = car.position[0]
self.mpc_coords[1] = car.position[1]
self.mpc_angle = car.angle
pygame.display.update()
self.clock.tick(60)
pygame.quit()