def record_data_function(self, car_tags, index):
"""
Data recording.
:param car_tags: tags of the cars you want to record
:param index: image index
:return:
"""
image_name = 'image_' + str(index) + '.png'
index += 1
# check if the car tags exists
for tag in car_tags:
if tag in self.kinematic_cars:
car = self.kinematic_cars[tag]
if self.state_buf_path is None:
raise OSError('state_buf_path is empty.')
if self.replay_data_path is None:
raise OSError('replay_data_path is empty.')
actions = [car.position.x, car.position.y, float(round(car.angle, 3)),
float(round(car.acceleration, 3)),
float(round(car.velocity.x, 3)), image_name]
# Save state_buf
write_state_buf(self.state_buf_path + '/' + str(tag) + '_state_buf' + '.csv', actions)
# Save replay
write_data(self.replay_data_path + '/' + str(tag) + '_replay' + '.csv', car.position, car.angle)
else:
raise ValueError('given car_tag does not exists')
def record_data_function(self, index):
"""
recording tab
:param index: image index
:return:
"""
image_name = 'image_' + str(index) + '.png'
index += 1
if self.state_buf_path is None:
raise OSError('state_buf_path is empty.')
if self.replay_data_path is None:
raise OSError('replay_data_path is empty.')
actions = [
self.car.position.x, self.car.position.y,
float(round(self.car.angle, 3)),
float(round(self.car.acceleration, 3)),
float(round(self.car.velocity.x, 3)), image_name
]
# Save state_buf
write_state_buf(self.state_buf_path, actions)
# Save replay
write_data(self.replay_data_path, self.car.position, self.car.angle)
def run(self):
# place car on road
car = Car(5, 27)
# initialize traffic
self.init_traffic_cars()
# sensor checkboxes on top right corner
cbox_front_sensor = Checkbox(self.screen_width - 200, 10,
'Enable front sensor', True)
cbox_rear_sensor = Checkbox(self.screen_width - 200, 35,
'Enable rear sensor', True)
# reset position list -> to be updated
rs_pos_list = [[650, 258, 90.0], [650, 258, 270.0], [0, 0, 180.0],
[0, 0, 0.0], [302, 200, 45.0], [40, 997, 0.0],
[40, 997, 180.0], [100, 997, 0.0], [100, 997, 180.0],
[400, 998, 0.0], [400, 998, 180.0], [385, 315, 135.0]]
# boolean variable needed to check for single-click press
mouse_button_pressed = False
# initialize object mask
object_mask = pygame.Surface((self.screen_width, self.screen_height))
if self.record_data is True:
index_image = 0
while not self.exit:
# VARIABLE_UPDATE
if self.traffic is True:
collision_list = [False] * len(self.traffic_list)
dt = self.clock.get_time() / 1000
self.event_handler(cbox_front_sensor, cbox_rear_sensor,
mouse_button_pressed)
# LOGIC
self.key_handler(car, dt, rs_pos_list)
car.acceleration = max(-car.max_acceleration,
min(car.acceleration, car.max_acceleration))
car.steering = max(-car.max_steering,
min(car.steering, car.max_steering))
# DRAWING
stagePos = self.draw_sim_environment(car,
object_mask,
cbox_front_sensor,
cbox_rear_sensor,
print_coords=True)
relPos = (stagePos[2], stagePos[3])
stagePos = (stagePos[0], stagePos[1])
# UPDATE
# ------------------------ traffic car -----------------------------------------------
if self.traffic is True:
self.check_collisions(collision_list)
self.traffic_movement(collision_list, object_mask, stagePos)
# -------------------------------------------------------------------------------------------
car.update(dt)
act_mask = pygame.Surface((self.screen_width, self.screen_height))
if cbox_front_sensor.isChecked():
self.optimized_front_sensor(car,
object_mask,
act_mask,
display_obstacle_on_sensor=True)
if cbox_rear_sensor.isChecked():
self.optimized_rear_sensor(car,
object_mask,
act_mask,
display_obstacle_on_sensor=True)
if self.record_data is True:
image_name = 'image_' + str(index_image) + '.png'
print(index_image)
index_image += 1
if self.record_data is True:
# RECORD TAB
# Save replay
# Write reference trajectory
write_data(
os.path.join(os.path.dirname(__file__), "recorded_data",
"reference.csv"), car.position, car.angle)
write_data(
os.path.join(os.path.dirname(__file__), "recorded_data",
"obstacles.csv"),
self.traffic_obstacle_points)
# reset obstacle point list
self.traffic_obstacle_points = list()
pygame.display.update()
self.clock.tick(self.ticks)
pygame.quit()
def draw_sim_environment(self,
car,
object_mask,
cbox_front_sensor,
cbox_rear_sensor,
print_coords=False,
record_coords=False,
file_path=None,
file_name=None,
record_traffic_car_coords=False,
traffic_file_path=None,
traffic_file_name=None):
# 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))
cbox_front_sensor.update()
cbox_rear_sensor.update()
rotated = pygame.transform.rotate(self.car_image, car.angle)
rot_rect = rotated.get_rect()
center_x = int(self.screen_width / 2) - int(rot_rect.width / 2)
center_y = int(self.screen_height / 2) - int(rot_rect.height / 2)
# draw the ego car
self.screen.blit(rotated, (center_x, center_y))
object_mask.fill((0, 0, 0))
object_mask.blit(self.screen, (0, 0))
update_object_mask(object_mask, rel_x, rel_y, self.bgWidth,
self.bgHeight)
if print_coords is True:
myfont = pygame.font.SysFont('Arial', 30)
text1 = myfont.render('Car pos x: ' + str(round(stagePosX, 2)),
True, (250, 0, 0))
text2 = myfont.render('Car pos y: ' + str(round(stagePosY, 2)),
True, (250, 0, 0))
text3 = myfont.render('rel x: ' + str(round(rel_x, 2)), True,
(250, 0, 0))
text4 = myfont.render('rel y: ' + str(round(rel_y, 2)), True,
(250, 0, 0))
text5 = myfont.render(
'velocity: ' + str(round(car.velocity.x, 2) * self.ppu / 4) +
' km/h', True, (250, 0, 0))
self.screen.blit(text1, (20, 20))
self.screen.blit(text2, (20, 50))
self.screen.blit(text3, (20, 80))
self.screen.blit(text4, (20, 110))
self.screen.blit(text5, (20, 140))
# Record ego_car positions in GridSim
if record_coords is True:
if file_name is None:
print('no file name given')
quit()
if file_path is None:
print('no file path given')
quit()
write_data(file_path + '/' + file_name, round(stagePosX, 2),
round(stagePosY, 2), round(rel_x, 2), round(rel_y, 2),
(round(car.velocity.x, 2) * self.ppu / 4))
# Record traffic car trajectory
if record_traffic_car_coords is True:
if traffic_file_name is None:
print('no file name given')
quit()
if traffic_file_path is None:
print('no file path given')
quit()
write_data(traffic_file_path + '/' + traffic_file_name, stagePosX,
stagePosY, car.angle)
return stagePosX, stagePosY, rel_x, rel_y
def replay_mode(self):
self.init_traffic_cars()
# place car on road
car = Car(5, 27)
object_mask = pygame.Surface((self.screen_width, self.screen_height))
u = [0.0, 0.0]
x = self._get_dwa_initial_state(car)
self._prepare_dwa_planner(car)
goals = self._get_dwa_goals(20)
goal_idx = 0
num_iterations = 0
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
# 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._update_map(car, rel_x, rel_y)
# self.optimized_front_sensor(car)
object_mask.fill((0, 0, 0))
object_mask.blit(self.screen, (0, 0))
update_object_mask(object_mask, rel_x, rel_y, self.bgWidth,
self.bgHeight)
stagePos = (stagePosX, stagePosY)
self.check_collisions(collision_list)
self.traffic_movement(collision_list, object_mask, stagePos)
obstacles = np.array(
self.get_obstacles(car=car, object_mask=object_mask))
goal = goals[goal_idx]
g_c = self.map_car_coordinates_to_drawing_coordinates(car, [goal])
pygame.draw.circle(self.screen, (0, 255, 255), g_c[0], 3, 3)
u, ltraj = dwa.dwa_control(x, u, self.dwa_config, goal, obstacles)
traj_coordinates = self.map_car_coordinates_to_drawing_coordinates(
car, ltraj)
# draw each trajectory point
for i in range(0, len(traj_coordinates)):
pygame.draw.circle(self.screen, (255, 255, 0),
traj_coordinates[i], 3, 3)
x = dwa.motion(x, u, self.dwa_config.dt)
self.record_data = False
if self.record_data is True:
write_data(
os.path.join(os.path.dirname(__file__), "resources",
"dwa_data", "dwa_path.csv"), x[0], x[1],
-dwa.rad2deg(x[2]))
image_name = 'image_' + str(num_iterations) + '.png'
save_frame(
self.screen, image_name,
os.path.join(os.path.dirname(__file__), 'resources',
'dwa_data', "images"))
car.position = (x[0], x[1])
car.angle = -dwa.rad2deg(x[2])
num_iterations += 1
if sqrt((x[0] - goal[0])**2 +
(x[1] - goal[1])**2) <= self.dwa_config.robot_radius:
goal_idx += 1
if goal_idx == len(goals) - 1:
break
if num_iterations >= 2000:
break
pygame.display.update()
self.clock.tick(60)
pygame.quit()