def run(self) -> bool:
assert self.evaluation
if self.world is None:
raise ValueError('world was not initialized')
if self.agent is None:
raise ValueError('agent was not initialized')
if self.control_evaluator is None or self.stop_evaluator is None:
raise ValueError('evluation call function was not set')
old_indices = self.traj_indices_from_state_dir()
exited = False
while len(old_indices) < len(self.eval_transforms) and not exited:
try:
t = 0
while t < len(self.eval_transforms):
if t in old_indices:
t += 1
continue
transform = self.eval_transforms[t]
run_status = self.run_single_trajectory(t, transform)
if run_status['finished']:
break
if run_status['restart']:
continue
if run_status['saved']:
old_indices.add(t)
t += 1
finally:
old_indices = self.traj_indices_from_state_dir()
set_world_asynchronous(self.world)
if self.agent is not None:
self.agent.destroy()
self.save_high_level_data()
return True
def run(self):
if self.world is None:
raise ValueError('world was not initialized')
if self.agent is None:
raise ValueError('agent was not initialized')
if self.evaluator is None:
raise ValueError('evluation call function was not set')
try:
i = 0
while i < len(self.index_data_list):
index, data = self.index_data_list[i]
if not self.dataset.has_trajectory(index):
run_status = self.run_single_trajectory(index, data)
if run_status['exited']:
break
if run_status['restart']:
continue
i += 1
finally:
set_world_asynchronous(self.world)
if self.agent is not None:
self.agent.destroy()
def run_single_trajectory(self, t: int, data: dict) -> Dict[str, bool]:
status = {
'exited': False, # has to finish the entire loop
'finished': False, # this procedure has been finished successfully
'collided': False, # the agent has collided
'restart': False # this has to be restarted
}
self.evaluator.cmd = data['action_index']
self.agent.reset()
logger.info(
'moved the vehicle to the position {}, set action to {}'.format(
t, data['action_index']))
local_image_dict = dict()
local_drive_dict = dict()
count = 0
frame = None
clock = pygame.time.Clock() if self.show_image else FrameCounter()
# todo: implement this function as in the same one in evaluator.py
set_world_asynchronous(self.world)
self.agent.agent.set_destination(data['src_transform'].location,
data['dst_location'])
self.agent.move_vehicle(data['src_transform'])
sleep(0.5)
set_world_synchronous(self.world)
len_waypoints = LengthComputer()
for waypoint_with_info in self.agent.agent._route:
len_waypoints(waypoint_with_info.waypoint.transform.location)
max_len = 0.9 * len_waypoints.length
max_iter = 5.0 * EVAL_FRAMERATE_SCALE * len(self.agent.agent._route)
len_agent = LengthComputer()
while count < max_iter and len_agent.length < max_len:
if self.show_image and should_quit():
status['exited'] = True
return status
if frame is not None and self.agent.collision_sensor.has_collided(
frame):
logger.info(
'collision was detected at frame #{}'.format(frame))
status['collided'] = True
break
clock.tick()
self.world.tick()
try:
ts = self.world.wait_for_tick()
except RuntimeError as e:
logger.error('runtime error: {}'.format(e))
status['restart'] = True
return status
if frame is not None:
if ts.frame_count != frame + 1:
logger.info('frame skip!')
frame = ts.frame_count
# image_frame_number, image = self.agent.image_queue.get()
if self.agent.image_frame is None:
continue
# register images
image = self.agent.image_frame
image_frame_number = self.agent.image_frame_number
local_image_dict[image_frame_number] = image
# store action values from the expert
waypoint, road_option, drive_data_frame = self.agent.step_from_pilot(
frame, apply=False, update=True)
local_drive_dict[frame] = self.agent.data_frame_dict[frame]
if waypoint is None:
status['finished'] = True
break
# apply action values from the agent
if count % EVAL_FRAMERATE_SCALE == 0:
model_control = self.evaluator.run_step(self.agent.image_frame)
control_str = 'throttle {:+6.4f}, steer {:+6.4f}, delayed {}'.format(
model_control.throttle, model_control.steer,
frame - image_frame_number)
if image_frame_number in local_drive_dict:
expert_control = local_drive_dict[
image_frame_number].control
control_str += ' steer {:+6.4f}, steer-diff {:+6.4f}'.format(
expert_control.steer,
model_control.steer - expert_control.steer)
logger.info(control_str)
self.agent.step_from_control(frame,
model_control,
apply=True,
update=False)
len_agent(drive_data_frame.state.transform.location)
if self.show_image:
self.show(image, clock)
count += 1
aligned, image_indices, drive_indices = align_indices_from_dicts(
local_image_dict, local_drive_dict, EVAL_FRAMERATE_SCALE // 2)
if aligned:
road_option_name = fetch_name_from_road_option(data['road_option'])
self.dataset.put_data_from_dagger(t, road_option_name,
local_image_dict,
local_drive_dict, image_indices,
drive_indices)
logger.info('successfully added {} dagger trajectory'.format(t))
else:
status['restart'] = True
return status
def run(self):
if self.world is None:
raise ValueError('world was not initialized')
if self.agent is None:
raise ValueError('agent was not initialized')
try:
frame = None
count = 0
clock = pygame.time.Clock() if self.show_image else FrameCounter()
set_world_asynchronous(self.world)
sleep(0.5)
set_world_synchronous(self.world)
waypoint_dict = dict()
road_option_dict = dict()
while count < 200000:
if self.show_image and should_quit():
break
if frame is not None and self.agent.collision_sensor.has_collided(
frame):
logger.info(
'collision was detected at frame #{}'.format(frame))
set_world_asynchronous(self.world)
self.transform_index += 1
self.agent.move_vehicle(
self.transforms[self.transform_index])
sleep(0.5)
self.agent.agent.reset_planner()
set_world_synchronous(self.world)
clock.tick()
self.world.tick()
ts = self.world.wait_for_tick()
if frame is not None:
if ts.frame_count != frame + 1:
logger.info('frame skip!')
frame = ts.frame_count
# self.injector.step()
if len(self.agent.data_frame_buffer) > 100:
self.agent.export_data()
if not self.show_image:
print(str(clock))
if self.agent.image_frame is None:
continue
waypoint, road_option, _ = self.agent.step_from_pilot(
frame, update=True, apply=True, inject=0.0)
waypoint_dict[frame] = waypoint
road_option_dict[frame] = road_option
if self.show_image:
image = self.agent.image_frame
image_frame_number = self.agent.image_frame_number
image_road_option = road_option_dict[
image_frame_number] if image_frame_number in road_option_dict else None
image_waypoint = waypoint_dict[
image_frame_number] if image_frame_number in waypoint_dict else None
self.show(
image, clock, image_road_option,
image_waypoint.is_intersection
if image_waypoint is not None else None)
count += 1
self.agent.export_data()
finally:
set_world_asynchronous(self.world)
if self.agent is not None:
self.agent.destroy()
def run_single_trajectory(self, t: int, transform: carla.Transform) -> Dict[str, bool]:
status = {
'exited': False, # has to finish the entire loop
'finished': False, # this procedure has been finished successfully
'saved': False, # successfully saved the evaluation data
'collided': False, # the agent has collided
'restart': False, # this has to be restarted
'stopped': True # low-level controller returns stop
}
self.agent.reset()
self.agent.move_vehicle(transform)
self.control_evaluator.initialize()
self.stop_evaluator.initialize()
self.high_evaluator.initialize()
self.high_data_dict[t] = []
self.final_images = []
self.sentence = get_random_sentence_from_keyword(self.eval_keyword)
logger.info('moved the vehicle to the position {}'.format(t))
count = 0
frame = None
clock = pygame.time.Clock()
set_world_asynchronous(self.world)
sleep(0.5)
set_world_synchronous(self.world)
stop_buffer = []
while not status['exited'] or not status['collided']:
keyboard_input = listen_keyboard()
if keyboard_input == 'q':
status['exited'] = True
break
elif keyboard_input in __keyword_from_input__.keys():
keyword = __keyword_from_input__[keyboard_input]
if keyword != self.eval_keyword:
self.eval_keyword = keyword
self.sentence = get_random_sentence_from_keyword(self.eval_keyword)
self.control_param.eval_keyword = keyword
self.stop_param.eval_keyword = keyword
self.high_param.eval_keyword = keyword
self.control_evaluator.param = self.control_param
self.stop_evaluator.param = self.stop_param
self.high_evaluator.cmd = keyword
self.high_evaluator.param = self.high_param
self.high_evaluator.sentence = keyword.lower()
self.control_evaluator.initialize()
self.stop_evaluator.initialize()
self.high_evaluator.initialize()
logger.info('updated sentence {}'.format(self.sentence))
if frame is not None and self.agent.collision_sensor.has_collided(frame):
logger.info('collision was detected at frame #{}'.format(frame))
status['collided'] = True
break
clock.tick()
self.world.tick()
try:
ts = self.world.wait_for_tick()
except RuntimeError as e:
logger.error('runtime error: {}'.format(e))
status['restart'] = True
return status
if frame is not None:
if ts.frame_count != frame + 1:
logger.info('frame skip!')
frame = ts.frame_count
if self.agent.image_frame is None:
continue
if self.agent.segment_frame is None:
continue
# run high-level evaluator when stopped was triggered by the low-level controller
final_image = self.final_image
if status['stopped']:
action = self.high_evaluator.run_step(final_image, self.sentence)
action = self.softmax(action)
action_index = torch.argmax(action[-1], dim=0).item()
location = self.agent.fetch_car_state().transform.location
self.high_data_dict[t].append((final_image, {
'sentence': self.sentence,
'location': (location.x, location.y),
'action_index': action_index}))
if action_index < 4:
self.control_evaluator.cmd = action_index
self.stop_evaluator.cmd = action_index
stop_buffer = []
else:
logger.info('the task was finished by "finish"')
status['finished'] = True
break
# run low-level evaluator to apply control and update stopped status
if count % EVAL_FRAMERATE_SCALE == 0:
control: carla.VehicleControl = self.control_evaluator.run_step(final_image)
stop: float = self.stop_evaluator.run_step(final_image)
sub_goal = fetch_high_level_command_from_index(self.control_evaluator.cmd).lower()
logger.info('throttle {:+6.4f}, steer {:+6.4f}, delayed {}, current {:d}, stop {:+6.4f}'.
format(control.throttle, control.steer, frame - self.agent.image_frame_number, action_index,
stop))
self.agent.step_from_control(frame, control)
self.agent.save_stop(frame, stop, sub_goal)
self.agent.save_cmd(frame, self.sentence)
stop_buffer.append(stop)
recent_buffer = stop_buffer[-3:]
status['stopped'] = len(recent_buffer) > 2 and sum(list(map(lambda x: x > 0.0, recent_buffer))) > 1
if self.show_image and self.agent.image_frame is not None:
self.show(self.agent.image_frame, clock, extra_str=self.sentence)
self.final_images.append(final_image)
count += 1
logger.info('saving information')
curr_eval_data = self.agent.export_eval_data(status['collided'], self.sentence)
if curr_eval_data is not None:
status['saved'] = self.export_evaluation_data(t, curr_eval_data)
return status
def run_single_trajectory(self, t: int, transform: carla.Transform) -> Dict[str, bool]:
status = {
'exited': False, # has to finish the entire loop
'finished': False, # this procedure has been finished successfully
'saved': False, # successfully saved the evaluation data
'collided': False, # the agent has collided
'restart': False, # this has to be restarted
'stopped': True # low-level controller returns stop
}
self.agent.reset()
self.agent.move_vehicle(transform)
self.control_evaluator.initialize()
self.stop_evaluator.initialize()
self.high_level_evaluator.initialize()
self.high_level_data_dict[t] = []
self.final_images = []
sentence = random.choice(self.high_level_sentences)
logger.info('moved the vehicle to the position {}'.format(t))
count = 0
frame = None
clock = pygame.time.Clock() if self.show_image else FrameCounter()
set_world_asynchronous(self.world)
sleep(0.5)
set_world_synchronous(self.world)
agent_len, expert_len = LengthComputer(), LengthComputer()
for l in self.eval_dataset[t]:
expert_len(l.state.transform.location)
criterion_len = 2.5 * expert_len.length # 0.9 * expert_len.length
max_iter = 10.0 * len(self.eval_dataset[t]) # 5.0 * len(self.eval_dataset[t])
stop_buffer = []
while agent_len.length < criterion_len and count < max_iter:
if self.show_image and should_quit():
status['exited'] = True
break
if frame is not None and self.agent.collision_sensor.has_collided(frame):
logger.info('collision was detected at frame #{}'.format(frame))
status['collided'] = True
break
if count > 30 and agent_len.length < 1:
logger.info('simulation has a problem in going forward')
status['exited'] = True
break
clock.tick()
self.world.tick()
try:
ts = self.world.wait_for_tick()
except RuntimeError as e:
logger.error('runtime error: {}'.format(e))
status['restart'] = True
return status
if frame is not None:
if ts.frame_count != frame + 1:
logger.info('frame skip!')
frame = ts.frame_count
if self.agent.image_frame is None:
continue
if self.agent.segment_frame is None:
continue
# run high-level evaluator when stopped was triggered by the low-level controller
final_image = self.final_image
if status['stopped']:
logger.info((final_image.shape))
action = self.high_level_evaluator.run_step(final_image, sentence)
action = self.softmax(action)
logger.info((action, action.shape, sentence))
action_index = torch.argmax(action[-1], dim=0).item()
logger.info('action {}, action_index {}'.format(action, action_index))
location = self.agent.fetch_car_state().transform.location
self.high_level_data_dict[t].append((final_image, {
'sentence': sentence,
'location': (location.x, location.y),
'action_index': action_index}))
if action_index < 4:
self.control_evaluator.cmd = action_index
self.stop_evaluator.cmd = action_index
stop_buffer = []
else:
logger.info('the task was finished by "finish"')
status['finished'] = True
break
# run low-level evaluator to apply control and update stopped status
if count % EVAL_FRAMERATE_SCALE == 0:
control: carla.VehicleControl = self.control_evaluator.run_step(final_image)
stop: float = self.stop_evaluator.run_step(final_image)
sub_goal = fetch_high_level_command_from_index(self.control_evaluator.cmd).lower()
logger.info('throttle {:+6.4f}, steer {:+6.4f}, delayed {}, stop {:+6.4f}'.format(
control.throttle, control.steer, frame - self.agent.image_frame_number, stop))
self.agent.step_from_control(frame, control)
self.agent.save_stop(frame, stop, sub_goal)
agent_len(self.agent.data_frame_dict[self.agent.data_frame_number].state.transform.location)
stop_buffer.append(stop)
recent_buffer = stop_buffer[-3:]
status['stopped'] = len(recent_buffer) > 2 and sum(list(map(lambda x: x > 0.0, recent_buffer))) > 1
if self.show_image and self.agent.image_frame is not None:
self.show(self.agent.image_frame, clock)
self.final_images.append(final_image)
count += 1
if agent_len.length >= criterion_len:
logger.info('trajectory length is longer than the threshold')
if count >= max_iter:
logger.info('reached the maximum number of iterations')
if not status['finished']:
status['finished'] = status['collided'] or agent_len.length >= criterion_len or count >= max_iter
if not status['finished']:
return status
curr_eval_data = self.agent.export_eval_data(status['collided'], sentence)
if curr_eval_data is not None:
status['saved'] = self.export_evaluation_data(t, curr_eval_data)
return status