def spawn_vehicles(self, num_of_vehicles: int):
if num_of_vehicles == 0: return
for _ in range(num_of_vehicles):
blueprint = random.choice(self.vehicle_blueprints)
if blueprint.has_attribute('color'):
color = random.choice(
blueprint.get_attribute('color').recommended_values)
blueprint.set_attribute('color', color)
if blueprint.has_attribute('driver_id'):
driver_id = random.choice(
blueprint.get_attribute('driver_id').recommended_values)
blueprint.set_attribute('driver_id', driver_id)
blueprint.set_attribute('role_name', 'autopilot')
try:
actor = self.world.spawn_actor(
blueprint, random.choice(self.spawn_points))
if actor:
actor.set_autopilot(True)
try:
actor.set_light_state(
carla.VehicleLightState(self.lights))
except:
pass
self.vehicles_list.append(actor)
except:
pass
self.traffic_manager.set_global_distance_to_leading_vehicle(2.0)
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_h or (event.key == K_SLASH and
pygame.key.get_mods() & KMOD_SHIFT):
world.hud.help.toggle()
elif event.key == K_r and not (pygame.key.get_mods()
& KMOD_CTRL):
world.camera_manager.toggle_recording()
elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL):
if (world.recording_enabled):
client.stop_recorder()
world.recording_enabled = False
world.hud.notification("Recorder is OFF")
else:
client.start_recorder("manual_recording.rec")
world.recording_enabled = True
world.hud.notification("Recorder is ON")
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
if not self._autopilot_enabled:
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(),
clock.get_time(), world)
world.player.apply_control(self._control)
def apply_lights_to_cars(args, world):
if args.cars is None:
return
light_mask = carla.VehicleLightState.NONE
for option in args.cars:
light_mask |= CAR_LIGHTS[option][0]
# Get all cars in level
all_vehicles = world.get_actors()
for ve in all_vehicles:
if "vehicle." in ve.type_id:
ve.set_light_state(carla.VehicleLightState(light_mask))
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_F1:
world.hud.toggle_info()
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_m:
self._control.manual_gear_shift = not self._control.manual_gear_shift
self._control.gear = world.player.get_control().gear
world.hud.notification(
'%s Transmission' %
('Manual' if self._control.manual_gear_shift else
'Automatic'))
elif self._control.manual_gear_shift and event.key == K_COMMA:
self._control.gear = max(-1, self._control.gear - 1)
elif self._control.manual_gear_shift and event.key == K_PERIOD:
self._control.gear = self._control.gear + 1
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(),
world)
world.player.apply_control(self._control)
def synchronize_vehicle(self, vehicle_id, transform, lights=None):
"""
Updates vehicle state.
:param vehicle_id: id of the actor to be updated.
:param transform: new vehicle transform (i.e., position and rotation).
:param lights: new vehicle light state.
:return: True if successfully updated. Otherwise, False.
"""
vehicle = self.world.get_actor(vehicle_id)
if vehicle is None:
return False
vehicle.set_transform(transform)
if lights is not None:
vehicle.set_light_state(carla.VehicleLightState(lights))
return True
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL:
self._autopilot_enabled = not self._autopilot_enabled
world.player.set_autopilot(self._autopilot_enabled)
if not self._autopilot_enabled:
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(
pygame.key.get_pressed(), clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(
pygame.key.get_pressed(), clock.get_time(), world)
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
if not self._autopilot_enabled:
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world)
world.player.apply_control(self._control)
def spawn(self):
self.clear_agent()
self.episode_start = None
# Spawn main vehicle
spawn_selected = False
while not spawn_selected:
try:
transform = random.choice(
self.world.get_map().get_spawn_points())
self.agent_vehicle = self.world.spawn_actor(
self.vehicle_bp, transform)
spawn_selected = True
except Exception:
pass
self.agent_vehicle.set_light_state(carla.VehicleLightState(
self.lights))
self.actor_list.append(self.agent_vehicle)
# Add preview camera
prev_camera = self.world.spawn_actor(self.p_camera_bp,
self.p_camera_transform,
attach_to=self.agent_vehicle)
prev_camera.listen(lambda data: self.__process_image(data))
self.actor_list.append(prev_camera)
# Add front camera
front_camera = self.world.spawn_actor(self.camera_bp,
self.front_transform,
attach_to=self.agent_vehicle)
front_camera.listen(lambda data: self.__process_image(data, "front"))
self.actor_list.append(front_camera)
# Add left camera
left_camera = self.world.spawn_actor(self.camera_bp,
self.left_transform,
attach_to=self.agent_vehicle)
left_camera.listen(lambda data: self.__process_image(data, "left"))
self.actor_list.append(left_camera)
# Add left camera
right_camera = self.world.spawn_actor(self.camera_bp,
self.right_transform,
attach_to=self.agent_vehicle)
right_camera.listen(lambda data: self.__process_image(data, "right"))
self.actor_list.append(right_camera)
self.agent_vehicle.apply_control(
carla.VehicleControl(throttle=1.0, brake=1.0))
time.sleep(1)
# Add colision sensor to vehicle
col_sensor = self.world.spawn_actor(self.colsenzor_bp,
self.front_transform,
attach_to=self.agent_vehicle)
col_sensor.listen(lambda event: self.__collision_data(event))
self.actor_list.append(col_sensor)
selected_default_action = settings.ACTIONS[settings.DEFAULT_ACTION]
self.agent_vehicle.apply_control(
carla.VehicleControl(
throttle=selected_default_action[0] * settings.THROT_AM,
steer=selected_default_action[1] * settings.STEER_AM,
brake=selected_default_action[2] * settings.BRAKE_AM))
time.sleep(0.125)
while self.cameras["front"] is None:
time.sleep(0.01)
while self.cameras["left"] is None:
time.sleep(0.01)
while self.cameras["right"] is None:
time.sleep(0.01)
while self.prev_camera is None:
time.sleep(0.01)
time.sleep(0.5)
if len(self.collision_hist) != 0:
self.spawn()
self.initialized = True
self.episode_start = time.time()
return [
self.cameras["front"], self.cameras["left"], self.cameras["right"],
self.get_speed(), settings.DEFAULT_ACTION
]
def parse_events(self, world, clock):
if VehicleControl.signal_received:
print('\nAccepted signal. Stopping loop...')
return True
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_BACKSPACE:
if self._autopilot_enabled:
world.player.set_autopilot(False)
world.restart()
world.player.set_autopilot(True)
else:
world.restart()
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_h or (event.key == K_SLASH and
pygame.key.get_mods() & KMOD_SHIFT):
world.hud.help.toggle()
elif event.key == K_TAB:
world.rss_unstructured_scene_visualizer.toggle_camera()
elif event.key == K_n:
world.toggle_pause()
elif event.key == K_r:
world.toggle_recording()
elif event.key == K_F2:
if self._world and self._world.rss_sensor:
self._world.rss_sensor.toggle_debug_visualization_mode(
)
elif event.key == K_b:
if self._world and self._world.rss_sensor:
if self._world.rss_sensor.sensor.road_boundaries_mode == carla.RssRoadBoundariesMode.Off:
self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.On
print("carla.RssRoadBoundariesMode.On")
else:
self._world.rss_sensor.sensor.road_boundaries_mode = carla.RssRoadBoundariesMode.Off
print("carla.RssRoadBoundariesMode.Off")
elif event.key == K_g:
if self._world and self._world.rss_sensor:
self._world.rss_sensor.drop_route()
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_p and not pygame.key.get_mods(
) & KMOD_CTRL:
self._autopilot_enabled = not self._autopilot_enabled
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
'Autopilot %s' %
('On' if self._autopilot_enabled else 'Off'))
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_CTRL:
current_lights ^= carla.VehicleLightState.Special1
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_SHIFT:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_i:
current_lights ^= carla.VehicleLightState.Interior
elif event.key == K_z:
current_lights ^= carla.VehicleLightState.LeftBlinker
elif event.key == K_x:
current_lights ^= carla.VehicleLightState.RightBlinker
elif event.type == MOUSEBUTTONDOWN:
# store current mouse position for mouse-steering
if event.button == 1:
self._mouse_steering_center = event.pos
elif event.type == MOUSEBUTTONUP:
if event.button == 1:
self._mouse_steering_center = None
if not self._autopilot_enabled:
prev_steer_cache = self._steer_cache
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
if pygame.mouse.get_pressed()[0]:
self._parse_mouse(pygame.mouse.get_pos())
self._control.reverse = self._control.gear < 0
vehicle_control = self._control
world.hud.original_vehicle_control = vehicle_control
world.hud.restricted_vehicle_control = vehicle_control
# limit speed to 30kmh
v = self._world.player.get_velocity()
if (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)) > 30.0:
self._control.throttle = 0
# if self._world.rss_sensor and self._world.rss_sensor.ego_dynamics_on_route and not self._world.rss_sensor.ego_dynamics_on_route.ego_center_within_route:
# print ("Not on route!" + str(self._world.rss_sensor.ego_dynamics_on_route))
if self._restrictor:
rss_proper_response = self._world.rss_sensor.proper_response if self._world.rss_sensor and self._world.rss_sensor.response_valid else None
if rss_proper_response:
if not (pygame.key.get_mods() & KMOD_CTRL):
vehicle_control = self._restrictor.restrict_vehicle_control(
vehicle_control, rss_proper_response,
self._world.rss_sensor.ego_dynamics_on_route,
self._vehicle_physics)
world.hud.restricted_vehicle_control = vehicle_control
world.hud.allowed_steering_ranges = self._world.rss_sensor.get_steering_ranges(
)
if world.hud.original_vehicle_control.steer != world.hud.restricted_vehicle_control.steer:
self._steer_cache = prev_steer_cache
# Set automatic control-related vehicle lights
if vehicle_control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Brake
if vehicle_control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
world.player.apply_control(vehicle_control)
def game_loop(self):
"""Run main program loop."""
date_time = datetime.now().strftime("%m_%d_%Y_%H:%M:%S")
self.run_id = "ver1_" + "".join(
random.choices(string.ascii_uppercase + string.digits,
k=10)) + date_time
print("run id is", self.run_id)
try:
if self.visualize:
pygame.init()
self.client = carla.Client("127.0.0.1", 2000)
self.client.set_timeout(60.0)
self.world = self.client.load_world(f"Town{self.map_id:02d}_Opt")
self.world.unload_map_layer(carla.MapLayer.ParkedVehicles)
self.weather = carla.WeatherParameters(
cloudiness=random.randint(0, 100),
precipitation=random.randint(0, 1) * random.randint(1, 100),
sun_altitude_angle=random.randint(-45, 90),
precipitation_deposits=random.randint(0, 100),
)
self.world.set_weather(self.weather)
self.current_run_dir = (Path(self.destination_dir) / self.run_id)
self.current_run_dir.mkdir(parents=True, exist_ok=True)
with (self.current_run_dir / "session_info.json").open("w") as f:
d = {
"town": self.map_id,
"traffic_density": self.traffic_density,
"cloudiness": self.weather.cloudiness,
"precipitation": self.weather.precipitation,
"sun_altitude_angle": self.weather.sun_altitude_angle,
"precipitation_deposits":
self.weather.precipitation_deposits,
}
json.dump(d, f)
print(json.dumps(d))
settings = self.world.get_settings()
settings.fixed_delta_seconds = 0.1
self.world.apply_settings(settings)
self.tm = self.client.get_trafficmanager()
self.tm.set_synchronous_mode(True)
actors, spawns = self.spawn_vehicles(self.traffic_density)
self.setup_car(spawns)
self.setup_sensors()
if self.visualize:
self.display = pygame.display.set_mode(
(VIEW_WIDTH, VIEW_HEIGHT),
pygame.HWSURFACE | pygame.DOUBLEBUF)
self.set_synchronous_mode(True)
vehicles = self.world.get_actors().filter("vehicle.*")
self.vehicles = vehicles
if self.weather.sun_altitude_angle < 10:
for v in self.vehicles:
v.set_light_state(
carla.VehicleLightState(
carla.VehicleLightState.LowBeam
| carla.VehicleLightState.Position))
def on_tick(timestamp):
with mutex:
print("frame", timestamp.frame)
if timestamp.frame % PROCESS_EVERY_N_FRAMES == 0:
print("processing")
if timestamp.frame_count not in self.data_buffer:
self.data_buffer[
timestamp.frame_count] = WorldBoxInfo()
self.data_buffer[
timestamp.
frame_count].boxes_FC = ClientSideBoundingBoxes.get_bounding_boxes(
vehicles, self.camera_FC)
self.data_buffer[
timestamp.
frame_count].boxes_FL = ClientSideBoundingBoxes.get_bounding_boxes(
vehicles, self.camera_FL)
self.data_buffer[
timestamp.
frame_count].boxes_FR = ClientSideBoundingBoxes.get_bounding_boxes(
vehicles, self.camera_FR)
self.data_buffer[
timestamp.
frame_count].transform = self.car.get_transform()
self.data_buffer[
timestamp.
frame_count].velocity = self.car.get_velocity()
self.data_buffer[
timestamp.
frame_count].angular_velocity = self.car.get_angular_velocity(
)
self.data_buffer[
timestamp.
frame_count].acceleration = self.car.get_acceleration(
)
self.data_buffer[
timestamp.
frame_count].is_at_traffic_light = self.car.is_at_traffic_light(
)
self.aggregate_buffer()
else:
print("skipping")
if self.visualize:
pygame.display.flip()
callback_id = self.world.on_tick(on_tick)
t = time.time()
for i in range(10 * self.session_length): # 3 minutes
self.world.tick(60)
if self.visualize:
pygame.event.pump()
print("current fps is ", i / (time.time() - t))
return True
except Exception as e:
print(e)
traceback.print_exc()
return False
finally:
self.world.remove_on_tick(callback_id)
with mutex: # acquire this to prevent ticks from happening
print("terminating, destroying sensors and agens")
self.camera_FC.stop()
self.camera_FC.destroy()
self.camera_FR.stop()
self.camera_FR.destroy()
self.camera_FL.stop()
self.camera_FL.destroy()
self.lidar_FC.stop()
self.lidar_FC.destroy()
self.lidar_FR.stop()
self.lidar_FR.destroy()
self.lidar_FL.stop()
self.lidar_FL.destroy()
self.car.destroy()
self.client.apply_batch_sync(
[carla.command.DestroyActor(x) for x in actors])
if self.visualize:
pygame.quit()
def _initialize_actors(self, config):
"""
Custom initialization
"""
ego_location = config.trigger_points[0].location
ego_wp = CarlaDataProvider.get_map().get_waypoint(ego_location)
# Get the junction
starting_wp = ego_wp
while not starting_wp.is_junction:
starting_wps = starting_wp.next(1.0)
if len(starting_wps) == 0:
raise ValueError(
"Failed to find junction as a waypoint with no next was detected"
)
starting_wp = starting_wps[0]
junction = starting_wp.get_junction()
# Get the opposite entry lane wp
possible_directions = ['right', 'left']
self._rng.shuffle(possible_directions)
for direction in possible_directions:
entry_wps, _ = get_junction_topology(junction)
source_entry_wps = filter_junction_wp_direction(
starting_wp, entry_wps, direction)
if source_entry_wps:
self._direction = direction
break
if not self._direction:
raise ValueError(
"Trying to find a lane to spawn the opposite actor but none was found"
)
# Get the source transform
spawn_wp = source_entry_wps[0]
added_dist = 0
while added_dist < self._source_dist:
spawn_wps = spawn_wp.previous(1.0)
if len(spawn_wps) == 0:
raise ValueError(
"Failed to find a source location as a waypoint with no previous was detected"
)
if spawn_wps[0].is_junction:
break
spawn_wp = spawn_wps[0]
added_dist += 1
self._spawn_wp = spawn_wp
source_transform = spawn_wp.transform
self._spawn_location = carla.Transform(
source_transform.location + carla.Location(z=0.1),
source_transform.rotation)
# Spawn the actor and move it below ground
opposite_bp_wildcard = self._rng.choice(self._opposite_bp_wildcards)
opposite_actor = CarlaDataProvider.request_new_actor(
opposite_bp_wildcard, self._spawn_location)
if not opposite_actor:
raise Exception("Couldn't spawn the actor")
opposite_actor.set_light_state(
carla.VehicleLightState(carla.VehicleLightState.Special1
| carla.VehicleLightState.Special2))
self.other_actors.append(opposite_actor)
opposite_transform = carla.Transform(
source_transform.location - carla.Location(z=500),
source_transform.rotation)
opposite_actor.set_transform(opposite_transform)
opposite_actor.set_simulate_physics(enabled=False)
# Get the sink location
sink_exit_wp = generate_target_waypoint(
self._map.get_waypoint(source_transform.location), 0)
sink_wps = sink_exit_wp.next(self._sink_dist)
if len(sink_wps) == 0:
raise ValueError(
"Failed to find a sink location as a waypoint with no next was detected"
)
self._sink_wp = sink_wps[0]
# get the collision location
self._collision_location = get_geometric_linear_intersection(
starting_wp.transform.location,
source_entry_wps[0].transform.location)
if not self._collision_location:
raise ValueError("Couldn't find an intersection point")
# Get the relevant traffic lights
tls = self._world.get_traffic_lights_in_junction(junction.id)
ego_tl = get_closest_traffic_light(ego_wp, tls)
source_tl = get_closest_traffic_light(
self._spawn_wp,
tls,
)
self._tl_dict = {}
for tl in tls:
if tl in (ego_tl, source_tl):
self._tl_dict[tl] = carla.TrafficLightState.Green
else:
self._tl_dict[tl] = carla.TrafficLightState.Red
def parse_events(self, client, world, clock):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_BACKSPACE:
if self._autopilot_enabled:
world.player.set_autopilot(False)
world.restart()
world.player.set_autopilot(True)
else:
world.restart()
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_TAB:
world.camera_manager.toggle_camera()
elif event.key == K_r and not (pygame.key.get_mods()
& KMOD_CTRL):
world.camera_manager.toggle_recording()
elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL):
if (world.recording_enabled):
client.stop_recorder()
world.recording_enabled = False
world.hud.notification("Recorder is OFF")
else:
client.start_recorder("manual_recording.rec")
world.recording_enabled = True
world.hud.notification("Recorder is ON")
elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL):
# stop recorder
client.stop_recorder()
world.recording_enabled = False
# work around to fix camera at start of replaying
current_index = world.camera_manager.index
world.destroy_sensors()
# disable autopilot
self._autopilot_enabled = False
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
"Replaying file 'manual_recording.rec'")
# replayer
client.replay_file("manual_recording.rec",
world.recording_start, 0, 0)
world.camera_manager.set_sensor(current_index)
elif event.key == K_MINUS and (pygame.key.get_mods()
& KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start -= 10
else:
world.recording_start -= 1
world.hud.notification("Recording start time is %d" %
(world.recording_start))
elif event.key == K_EQUALS and (pygame.key.get_mods()
& KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start += 10
else:
world.recording_start += 1
world.hud.notification("Recording start time is %d" %
(world.recording_start))
elif event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_m:
self._control.manual_gear_shift = not self._control.manual_gear_shift
self._control.gear = world.player.get_control().gear
world.hud.notification(
'%s Transmission' %
('Manual'
if self._control.manual_gear_shift else 'Automatic'))
elif self._control.manual_gear_shift and event.key == K_COMMA:
self._control.gear = max(-1, self._control.gear - 1)
elif self._control.manual_gear_shift and event.key == K_PERIOD:
self._control.gear = self._control.gear + 1
elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL:
self._autopilot_enabled = not self._autopilot_enabled
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
'Autopilot %s' %
('On' if self._autopilot_enabled else 'Off'))
elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL:
current_lights ^= carla.VehicleLightState.Special1
elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_i:
current_lights ^= carla.VehicleLightState.Interior
elif event.key == K_z:
current_lights ^= carla.VehicleLightState.LeftBlinker
elif event.key == K_x:
current_lights ^= carla.VehicleLightState.RightBlinker
if not self._autopilot_enabled:
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
world.player.apply_control(self._control)
def _load_and_run_scenario(self, args, config, customized_data):
"""
Load and run the scenario given by config
"""
# hack:
if args.weather_index == -1:
weather = customized_data["fine_grained_weather"]
else:
weather = WEATHERS[args.weather_index]
config.weather = weather
config.friction = customized_data["friction"]
config.cur_server_port = customized_data["port"]
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
_, route = interpolate_trajectory(self.world, config.trajectory)
customized_data["center_transform"] = route[int(len(route) // 2)][0]
# from customized_utils import visualize_route
# visualize_route(route)
# transforms = []
# for x in route:
# transforms.append(x[0])
# print('len(transforms)', len(transforms))
if customized_data['using_customized_route_and_scenario']:
"""
customized non-default center transforms for actors
['waypoint_ratio', 'absolute_location']
"""
for k, v in customized_data["customized_center_transforms"].items(
):
if v[0] == "waypoint_ratio":
r = v[1] / 100
ind = np.min([int(len(route) * r), len(route) - 1])
loc = route[ind][0].location
customized_data[k] = create_transform(
loc.x, loc.y, 0, 0, 0, 0)
print("waypoint_ratio", loc.x, loc.y)
elif v[0] == "absolute_location":
customized_data[k] = create_transform(
v[1], v[2], 0, 0, 0, 0)
else:
print("unknown key", k)
print("-" * 100)
print("port :", customized_data["port"])
print(
"center_transform :",
"(",
customized_data["center_transform"].location.x,
customized_data["center_transform"].location.y,
")",
)
print("friction :", customized_data["friction"])
print("weather_index :", customized_data["weather_index"])
print("num_of_static :", customized_data["num_of_static"])
print("num_of_pedestrians :",
customized_data["num_of_pedestrians"])
print("num_of_vehicles :", customized_data["num_of_vehicles"])
print("-" * 100)
agent_class_name = getattr(self.module_agent, "get_entry_point")()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config)
# addition
# self.agent_instance.set_trajectory(config.trajectory)
self.agent_instance.set_args(args)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor["type"]]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
traceback.print_exc()
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
# print('\n'*10, 'RouteScenario config.cur_server_port', config.cur_server_port, '\n'*10)
scenario = RouteScenario(
world=self.world,
config=config,
debug_mode=args.debug,
customized_data=customized_data,
)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness,
)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
"static.trigger.friction")
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute("friction", str(config.friction))
friction_bp.set_attribute("extent_x", str(extent.x))
friction_bp.set_attribute("extent_y", str(extent.y))
friction_bp.set_attribute("extent_z", str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
# addition: to turn on lights of
actor_list = self.world.get_actors()
vehicle_list = actor_list.filter("*vehicle*")
for vehicle in vehicle_list:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}.log".format(
args.record, config.name))
self.manager.load_scenario(scenario, self.agent_instance)
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
print("start to run scenario")
self.manager.run_scenario()
print("stop to run scanario")
# Stop scenario
self.manager.stop_scenario()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config,
self.manager.scenario_duration_system,
self.manager.scenario_duration_game,
)
# save
# modification
self.statistics_manager.save_record(current_stats_record,
config.index, self.save_path)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()
def _set_new_velocity(self, next_location):
"""
Calculate and set the new actor veloctiy given the current actor
location and the _next_location_
If _consider_obstacles is true, the speed is adapted according to the closest
obstacle in front of the actor, if it is within the _proximity_threshold distance.
If _consider_trafficlights is true, the vehicle will enforce a stop at a red
traffic light.
If _max_deceleration is set, the vehicle will reduce its speed according to the
given deceleration value.
If the vehicle reduces its speed, braking lights will be activated.
Args:
next_location (carla.Location): Next target location of the actor
returns:
direction (carla.Vector3D): Length of direction vector of the actor
"""
current_time = GameTime.get_time()
target_speed = self._target_speed
if not self._last_update:
self._last_update = current_time
current_speed = math.sqrt(self._actor.get_velocity().x**2 + self._actor.get_velocity().y**2)
if self._consider_obstacles:
# If distance is less than the proximity threshold, adapt velocity
if self._obstacle_distance < self._proximity_threshold:
distance = max(self._obstacle_distance, 0)
if distance > 0:
current_speed_other = math.sqrt(
self._obstacle_actor.get_velocity().x**2 + self._obstacle_actor.get_velocity().y**2)
if current_speed_other < current_speed:
acceleration = -0.5 * (current_speed - current_speed_other)**2 / distance
target_speed = max(acceleration * (current_time - self._last_update) + current_speed, 0)
else:
target_speed = 0
if self._consider_traffic_lights:
if (self._actor.is_at_traffic_light() and
self._actor.get_traffic_light_state() == carla.TrafficLightState.Red):
target_speed = 0
if target_speed < current_speed:
if not self._brake_lights_active:
self._brake_lights_active = True
light_state = self._actor.get_light_state()
light_state |= carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
if self._max_deceleration is not None:
target_speed = max(target_speed, current_speed - (current_time -
self._last_update) * self._max_deceleration)
else:
if self._brake_lights_active:
self._brake_lights_active = False
light_state = self._actor.get_light_state()
light_state &= ~carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
if self._max_acceleration is not None:
tmp_speed = min(target_speed, current_speed + (current_time -
self._last_update) * self._max_acceleration)
# If the tmp_speed is < 0.5 the vehicle may not properly accelerate.
# Therefore, we bump the speed to 0.5 m/s if target_speed allows.
target_speed = max(tmp_speed, min(0.5, target_speed))
# set new linear velocity
velocity = carla.Vector3D(0, 0, 0)
direction = next_location - CarlaDataProvider.get_location(self._actor)
direction_norm = math.sqrt(direction.x**2 + direction.y**2)
velocity.x = direction.x / direction_norm * target_speed
velocity.y = direction.y / direction_norm * target_speed
self._actor.set_target_velocity(velocity)
# set new angular velocity
current_yaw = CarlaDataProvider.get_transform(self._actor).rotation.yaw
# When we have a waypoint list, use the direction between the waypoints to calculate the heading (change)
# otherwise use the waypoint heading directly
if self._waypoints:
delta_yaw = math.degrees(math.atan2(direction.y, direction.x)) - current_yaw
else:
new_yaw = CarlaDataProvider.get_map().get_waypoint(next_location).transform.rotation.yaw
delta_yaw = new_yaw - current_yaw
if math.fabs(delta_yaw) > 360:
delta_yaw = delta_yaw % 360
if delta_yaw > 180:
delta_yaw = delta_yaw - 360
elif delta_yaw < -180:
delta_yaw = delta_yaw + 360
angular_velocity = carla.Vector3D(0, 0, 0)
if target_speed == 0:
angular_velocity.z = 0
else:
angular_velocity.z = delta_yaw / (direction_norm / target_speed)
self._actor.set_target_angular_velocity(angular_velocity)
self._last_update = current_time
return direction_norm
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key > K_0 and event.key <= K_9:
world.view = event.key - K_0
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_r:
world.plotPointCloud = True
elif event.key == K_i:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_k:
#attivo il cruise control:
world.toggleCruise()
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(world, pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
world.player.apply_control(self._control)
def _load_and_run_scenario(self, args, config):
"""
Load and run the scenario given by config.
Depending on what code fails, the simulation will either stop the route and
continue from the next one, or report a crash and stop.
"""
crash_message = ""
entry_status = "Started"
print("\n\033[1m========= Preparing {} (repetition {}) =========".format(config.name, config.repetition_index))
print("> Setting up the agent\033[0m")
# Prepare the statistics of the route
self.statistics_manager.set_route(config.name, config.index)
# Set up the user's agent, and the timer to avoid freezing the simulation
try:
self._agent_watchdog.start()
agent_class_name = getattr(self.module_agent, 'get_entry_point')()
self.agent_instance = getattr(self.module_agent, agent_class_name)(args.agent_config)
config.agent = self.agent_instance
# Check and store the sensors
if not self.sensors:
self.sensors = self.agent_instance.sensors()
track = self.agent_instance.track
AgentWrapper.validate_sensor_configuration(self.sensors, track, args.track)
self.sensor_icons = [sensors_to_icons[sensor['type']] for sensor in self.sensors]
self.statistics_manager.save_sensors(self.sensor_icons, args.checkpoint)
self._agent_watchdog.stop()
except SensorConfigurationInvalid as e:
# The sensors are invalid -> set the ejecution to rejected and stop
print("\n\033[91mThe sensor's configuration used is invalid:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Agent's sensors were invalid"
entry_status = "Rejected"
self._register_statistics(config, args.checkpoint, entry_status, crash_message)
self._cleanup()
sys.exit(-1)
except Exception as e:
# The agent setup has failed -> start the next route
print("\n\033[91mCould not set up the required agent:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Agent couldn't be set up"
self._register_statistics(config, args.checkpoint, entry_status, crash_message)
self._cleanup()
return
print("\033[1m> Loading the world\033[0m")
# Load the world and the scenario
try:
self._load_and_wait_for_world(args, config.town, config.ego_vehicles)
self._prepare_ego_vehicles(config.ego_vehicles, False)
scenario = RouteScenario(world=self.world, config=config, debug_mode=args.debug)
self.statistics_manager.set_scenario(scenario.scenario)
# Night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(carla.VehicleLightState(self._vehicle_lights))
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}_rep{}.log".format(args.record, config.name, config.repetition_index))
self.manager.load_scenario(scenario, self.agent_instance, config.repetition_index)
except Exception as e:
# The scenario is wrong -> set the ejecution to crashed and stop
print("\n\033[91mThe scenario could not be loaded:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Simulation crashed"
entry_status = "Crashed"
self._register_statistics(config, args.checkpoint, entry_status, crash_message)
if args.record:
self.client.stop_recorder()
self._cleanup()
sys.exit(-1)
print("\033[1m> Running the route\033[0m")
# Run the scenario
try:
self.manager.run_scenario()
except AgentError as e:
# The agent has failed -> stop the route
print("\n\033[91mStopping the route, the agent has crashed:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Agent crashed"
except Exception as e:
print("\n\033[91mError during the simulation:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Simulation crashed"
entry_status = "Crashed"
# Stop the scenario
try:
print("\033[1m> Stopping the route\033[0m")
self.manager.stop_scenario()
self._register_statistics(config, args.checkpoint, entry_status, crash_message)
if args.record:
self.client.stop_recorder()
# Remove all actors
scenario.remove_all_actors()
self._cleanup()
except Exception as e:
print("\n\033[91mFailed to stop the scenario, the statistics might be empty:")
print("> {}\033[0m\n".format(e))
traceback.print_exc()
crash_message = "Simulation crashed"
if crash_message == "Simulation crashed":
sys.exit(-1)
def control(self, client, world, clock, trajectory, goal, mpc_state):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
elif event.key == K_BACKSPACE:
if self._autopilot_enabled:
world.player.set_autopilot(False)
world.restart()
world.player.set_autopilot(True)
else:
world.restart()
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_h or (event.key == K_SLASH and pygame.key.get_mods() & KMOD_SHIFT):
world.hud.help.toggle()
elif event.key == K_TAB:
world.camera_manager.toggle_camera()
elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT:
world.next_weather(reverse=True)
elif event.key == K_c:
world.next_weather()
elif event.key == K_g:
world.toggle_radar()
elif event.key == K_BACKQUOTE:
world.camera_manager.next_sensor()
elif event.key == K_n:
world.camera_manager.next_sensor()
elif event.key > K_0 and event.key <= K_9:
world.camera_manager.set_sensor(event.key - 1 - K_0)
elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL):
world.camera_manager.toggle_recording()
elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL):
if (world.recording_enabled):
client.stop_recorder()
world.recording_enabled = False
world.hud.notification("Recorder is OFF")
else:
client.start_recorder("manual_recording.rec")
world.recording_enabled = True
world.hud.notification("Recorder is ON")
elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL):
# stop recorder
client.stop_recorder()
world.recording_enabled = False
# work around to fix camera at start of replaying
current_index = world.camera_manager.index
world.destroy_sensors()
# disable autopilot
self._autopilot_enabled = False
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification("Replaying file 'manual_recording.rec'")
# replayer
client.replay_file("manual_recording.rec", world.recording_start, 0, 0)
world.camera_manager.set_sensor(current_index)
elif event.key == K_MINUS and (pygame.key.get_mods() & KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start -= 10
else:
world.recording_start -= 1
world.hud.notification("Recording start time is %d" % (world.recording_start))
elif event.key == K_EQUALS and (pygame.key.get_mods() & KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start += 10
else:
world.recording_start += 1
world.hud.notification("Recording start time is %d" % (world.recording_start))
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_m:
self._control.manual_gear_shift = not self._control.manual_gear_shift
self._control.gear = world.player.get_control().gear
world.hud.notification('%s Transmission' %
('Manual' if self._control.manual_gear_shift else 'Automatic'))
elif self._control.manual_gear_shift and event.key == K_COMMA:
self._control.gear = max(-1, self._control.gear - 1)
elif self._control.manual_gear_shift and event.key == K_PERIOD:
self._control.gear = self._control.gear + 1
elif event.key == K_p and not pygame.key.get_mods() & KMOD_CTRL:
self._autopilot_enabled = not self._autopilot_enabled
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
'Autopilot %s' % ('On' if self._autopilot_enabled else 'Off'))
elif event.key == K_l and pygame.key.get_mods() & KMOD_CTRL:
current_lights ^= carla.VehicleLightState.Special1
elif event.key == K_l and pygame.key.get_mods() & KMOD_SHIFT:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_i:
current_lights ^= carla.VehicleLightState.Interior
elif event.key == K_z:
current_lights ^= carla.VehicleLightState.LeftBlinker
elif event.key == K_x:
current_lights ^= carla.VehicleLightState.RightBlinker
if not self._autopilot_enabled:
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(), clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(), world)
#control(self, client, world, clock, trajectory, goal, mpc_state):
t = world.player.get_transform()
v = world.player.get_velocity()
c = world.player.get_control()
surr_vel_num = 0
surr_vel_dis = []
surr_vel_v = []
vehicles = world.world.get_actors().filter('vehicle.*')
distance = lambda l: math.sqrt((l.x - t.location.x)**2 + (l.y - t.location.y)**2 + (l.z - t.location.z)**2)
vehicles = [(distance(surr.get_location()), surr) for surr in vehicles if surr.id != world.player.id]
for d, vehicle in sorted(vehicles, key=lambda vehicles: vehicles[0]):
if d > 50.0:
break
else:
surr_vel_num += 1
surr_v = vehicle.get_velocity()
surr_vel_dis.append(vehicle.get_location())
surr_vel_v.append(3.6 * math.sqrt(surr_v.x**2 + surr_v.y**2 + surr_v.z**2))
[cx, cy, cyaw, ck, sp, dl, time_step, state]= mpc_state
# initial yaw compensation
state.x = t.location.x
state.y = t.location.y
state.v = math.sqrt(v.x**2 + v.y**2 + v.z**2)
state.yaw = t.rotation.yaw
print(state.yaw)
if state.yaw - cyaw[0] >= math.pi:
state.yaw -= math.pi * 2.0
elif state.yaw - cyaw[0] <= -math.pi:
state.yaw += math.pi * 2.0
c_a = c.throttle #0.0, 1.0
c_delta = c.steer #-1.0, +1.0
c_b = c.brake #0.0, 1.0
if np.abs(c_a) >= np.abs(c_b):
a = c_a
else:
a = c_b
time = 0
xx = [state.x]
yy = [state.y]
yyaw = [state.yaw]
vv = [state.v]
dd = [0]
aa = [0]
target_ind, _ = self._calc_nearest_index(state, cx, cy, cyaw, 0)
odelta, oa = None, None
cyaw = self._smooth_yaw(cyaw)
while self.MAX_TIME >= time:
xref, target_ind, dref = self._calc_ref_trajectory(
state, cx, cy, cyaw, ck, sp, dl, target_ind)
x0 = [state.x, state.y, state.v, state.yaw]
oa, odelta, ox, oy, oyaw, ov = self._iterative_linear_mpc_control(
xref, x0, dref, oa, odelta)
if odelta is not None:
di, ai = odelta[0], oa[0]
state = self._update_state(state, ai, di)
time = time + self.DT
xx.append(state.x)
yy.append(state.y)
yyaw.append(state.yaw)
vv.append(state.v)
dd.append(di/180)
aa.append(ai)
if ai > 0:
self._control.throttle = np.abs(ai)
self._control.brake = 0
else:
self._control.throttle =0
self._control.brake = np.abs(ai)
"""
if np.abs(di*np.pi/180) > 0.2:
self._control.steer = -di
else:
self._control.steer = di*np.pi/180
#if time > 120 and time <140:
# self._control.steer = -0.2
"""
self._control.steer = -di/180
world.player.apply_control(self._control)
time_step = time_step + time
if self._check_goal(state, goal, target_ind, len(cx)):
self._control.throttle =0
self._control.brake = 1.0
print("Goal")
break
if show_animation: # pragma: no cover
selected_x = trajectory['x']
selected_y = trajectory['y']
plt.rcParams['figure.figsize'] = 4,7
plt.rcParams['font.size'] = 14
plt.style.use('seaborn-dark')
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if ox is not None:
plt.plot(oy, ox, "xr", label="pred_traj.")
#plt.plot(selected_x, selected_y, color='gray', label="Lane change trajectory", linewidth=2)
plt.plot(cy, cx, color='gray', label="plan_traj.", linewidth=5)
#plt.plot(xx, yy, "ob", label="trajectory")
plt.plot(xref[1, :], xref[0, :], "ob", label="ref_traj.")
plt.plot(cy[target_ind], cx[target_ind], "xg", label="target")
#plot_car(state.y, state.x, state.yaw, steer=di)
plt.plot(state.y, state.x, "*r", markersize=20)
plt.text(state.y+2, state.x, 'ego')
plt.text(state.y+7, state.x, str(state.v*3.6)[:5]+'[km/h]')
for j in range(surr_vel_num):
surr_vel = surr_vel_dis[j]
surr_num = 'surr_{}'.format(j+1)
plt.plot(surr_vel.y, surr_vel.x, marker=markers[j], color='k', label=surr_num, markersize=5)
plt.text(surr_vel.y+5,surr_vel.x, str(surr_vel_v[j]*3.6)[:5])
plt.xlim(-0, 35)
plt.ylim(-400, 15)
plt.grid(True)
plt.legend(loc="upper right")
plt.title("Time[s]:" + str(round(time_step, 2))
+ ", speed[km/h]:" + str(round(state.v * 3.6, 2)))
plt.pause(0.0001)
return xx, yy, yyaw, vv, dd, aa, time_step, state
def _load_and_run_scenario(self, args, config, customized_data):
"""
Load and run the scenario given by config
"""
# hack:
config.weather = WEATHERS[args.weather_index]
config.friction = customized_data['friction']
config.cur_server_port = customized_data['port']
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
_, route = interpolate_trajectory(self.world, config.trajectory)
customized_data['center_transform'] = route[int(len(route) // 2)][0]
'''
customized non-default center transforms for actors
['waypoint_ratio', 'absolute_location']
'''
for k, v in customized_data['customized_center_transforms'].items():
if v[0] == 'waypoint_ratio':
ind = np.min([int(len(route) * v[1]), len(route) - 1])
loc = route[ind][0].location
customized_data[k] = create_transform(loc.x, loc.y, 0, 0, 0, 0)
elif v[0] == 'absolute_location':
customized_data[k] = create_transform(v[1], v[2], 0, 0, 0, 0)
else:
print('unknown key', k)
if 'weather_index' in customized_data:
print('-' * 100)
print('port :', customized_data['port'])
print('center_transform :', '(',
customized_data['center_transform'].location.x,
customized_data['center_transform'].location.y, ')')
print('friction :', customized_data['friction'])
print('weather_index :', customized_data['weather_index'])
print('num_of_static :', customized_data['num_of_static'])
print('num_of_pedestrians :',
customized_data['num_of_pedestrians'])
print('num_of_vehicles :', customized_data['num_of_vehicles'])
print('-' * 100)
agent_class_name = getattr(self.module_agent, 'get_entry_point')()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config)
# addition
# self.agent_instance.set_trajectory(config.trajectory)
self.agent_instance.set_deviations_path(args.deviations_folder)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor['type']]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
traceback.print_exc()
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
# print('\n'*10, 'RouteScenario config.cur_server_port', config.cur_server_port, '\n'*10)
scenario = RouteScenario(world=self.world,
config=config,
debug_mode=args.debug,
customized_data=customized_data)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute('friction', str(config.friction))
friction_bp.set_attribute('extent_x', str(extent.x))
friction_bp.set_attribute('extent_y', str(extent.y))
friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
# addition: to turn on lights of
actor_list = self.world.get_actors()
vehicle_list = actor_list.filter('*vehicle*')
for vehicle in vehicle_list:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}.log".format(
args.record, config.name))
self.manager.load_scenario(scenario, self.agent_instance)
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
print('start to run scenario')
self.manager.run_scenario()
print('stop to run scanario')
# Stop scenario
self.manager.stop_scenario()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config, self.manager.scenario_duration_system,
self.manager.scenario_duration_game)
# save
# modification
self.statistics_manager.save_record(current_stats_record,
config.index, self.save_path)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()
def _load_and_run_scenario(self, args, config, data_folder, route_folder,
k, model_path):
"""
Load and run the scenario given by config
"""
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
agent_class_name = getattr(self.module_agent, 'get_entry_point')()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config,
data_folder,
route_folder, k,
model_path)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor['type']]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
scenario = RouteScenario(world=self.world,
config=config,
debug_mode=args.debug)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute('friction', str(config.friction))
friction_bp.set_attribute('extent_x', str(extent.x))
friction_bp.set_attribute('extent_y', str(extent.y))
friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder(
data_folder +
"/data%d.log".format(args.record, config.name) % k)
self.manager.load_scenario(
scenario, self.agent_instance) #scenarionmanager.py
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
distance_path = data_folder + "/route_completed.csv"
gpu_stats_path = data_folder + "/gpu_stats%d.csv" % k
x = time.time()
self.manager.run_scenario(
distance_path, gpu_stats_path
) #This is where the scenario runs scenariomanager.py
print("Simulation Run time:%s" % (time.time() - x))
# Stop scenario
self.manager.stop_scenario()
print(
self.client.show_recorder_collisions(
data_folder + "/data%d.log" % k, "v", "a"))
file1 = open(data_folder + "/data%d.txt" % k, "a")
file1.writelines(
self.client.show_recorder_collisions(
data_folder + "/data%d.log" % k, "v", "a"))
# Closing file
file1.close()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config, self.manager.scenario_duration_system,
self.manager.scenario_duration_game)
# save
self.statistics_manager.save_record(current_stats_record,
config.index, args.checkpoint)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()
def parse_events(self, client, world, clock):
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return True
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return True
# elif event.key == K_BACKSPACE:
# if self._autopilot_enabled:
# world.player.set_autopilot(False)
# world.restart()
# world.player.set_autopilot(True)
# else:
# world.restart()
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_h or (event.key == K_SLASH and
pygame.key.get_mods() & KMOD_SHIFT):
world.hud.help.toggle()
elif event.key == K_TAB:
world.camera_manager.toggle_camera()
elif event.key == K_c and pygame.key.get_mods() & KMOD_SHIFT:
world.next_weather(reverse=True)
elif event.key == K_c:
world.next_weather()
elif event.key == K_g:
world.toggle_radar()
elif event.key == K_BACKQUOTE:
world.camera_manager.next_sensor()
elif event.key == K_n:
world.camera_manager.next_sensor()
elif event.key == K_w and (pygame.key.get_mods() & KMOD_CTRL):
if world.constant_velocity_enabled:
world.player.disable_constant_velocity()
world.constant_velocity_enabled = False
world.hud.notification(
"Disabled Constant Velocity Mode")
else:
world.player.enable_constant_velocity(
carla.Vector3D(17, 0, 0))
world.constant_velocity_enabled = True
world.hud.notification(
"Enabled Constant Velocity Mode at 60 km/h")
elif event.key > K_0 and event.key <= K_9:
world.camera_manager.set_sensor(event.key - 1 - K_0)
elif event.key == K_r and not (pygame.key.get_mods()
& KMOD_CTRL):
world.camera_manager.toggle_recording()
# elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL):
# if (world.recording_enabled):
# client.stop_recorder()
# world.recording_enabled = False
# world.hud.notification("Recorder is OFF")
# else:
# client.start_recorder("manual_recording.rec")
# world.recording_enabled = True
# world.hud.notification("Recorder is ON")
elif event.key == K_t:
world.save_img = True
elif event.key == K_k:
world.car_chase = not world.car_chase
world.hud.notification(
'Car Chase %s' % ('On' if world.car_chase else 'Off'))
elif event.key == K_p and (pygame.key.get_mods() & KMOD_CTRL):
# stop recorder
client.stop_recorder()
world.recording_enabled = False
# work around to fix camera at start of replaying
current_index = world.camera_manager.index
world.destroy_sensors()
# disable autopilot
self._autopilot_enabled = False
world.player.set_autopilot(self._autopilot_enabled)
world.hud.notification(
"Replaying file 'manual_recording.rec'")
# replayer
client.replay_file("manual_recording.rec",
world.recording_start, 0, 0)
world.camera_manager.set_sensor(current_index)
elif event.key == K_MINUS and (pygame.key.get_mods()
& KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start -= 10
else:
world.recording_start -= 1
world.hud.notification("Recording start time is %d" %
(world.recording_start))
elif event.key == K_EQUALS and (pygame.key.get_mods()
& KMOD_CTRL):
if pygame.key.get_mods() & KMOD_SHIFT:
world.recording_start += 10
else:
world.recording_start += 1
world.hud.notification("Recording start time is %d" %
(world.recording_start))
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
# elif event.key == K_m:
# self._control.manual_gear_shift = not self._control.manual_gear_shift
# self._control.gear = world.player.get_control().gear
# world.hud.notification('%s Transmission' %
# ('Manual' if self._control.manual_gear_shift else 'Automatic'))
# elif self._control.manual_gear_shift and event.key == K_COMMA:
# self._control.gear = max(-1, self._control.gear - 1)
# elif self._control.manual_gear_shift and event.key == K_PERIOD:
# self._control.gear = self._control.gear + 1
elif event.key == K_p and not pygame.key.get_mods(
) & KMOD_CTRL:
world.autopilot_flag = not world.autopilot_flag
self._autopilot_enabled = world.autopilot_flag
world.hud.notification(
'Autopilot %s' %
('On' if world.autopilot_flag else 'Off'))
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_CTRL:
current_lights ^= carla.VehicleLightState.Special1
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_SHIFT:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_i:
# current_lights ^= carla.VehicleLightState.Interior
# Modification
world.gps_flag = True
world.hud.notification(
'GPS Navigation %s' %
('On' if world.gps_flag else 'Off'))
elif event.key == K_z:
current_lights ^= carla.VehicleLightState.LeftBlinker
elif event.key == K_x:
current_lights ^= carla.VehicleLightState.RightBlinker
if not self._autopilot_enabled:
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= ~carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= ~carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(),
clock.get_time(), world)
world.player.apply_control(self._control)
def parse_events(self, client, world, clock) -> \
Tuple[bool, carla.VehicleControl]:
"""
Parse keyboard press.
:param client: carla.Client
:param world: carla.Client
:param clock: pygame clock
:return:
bool - True if should continue, aka no exit key was pressed
control - carla.VehicleControl
"""
if isinstance(self._control, carla.VehicleControl):
current_lights = self._lights
for event in pygame.event.get():
if event.type == pygame.QUIT:
return False, None
elif event.type == pygame.KEYUP:
if self._is_quit_shortcut(event.key):
return False, None
elif event.key == K_F1:
world.hud.toggle_info()
elif event.key == K_h or (event.key == K_SLASH and
pygame.key.get_mods() & KMOD_SHIFT):
world.hud.help.toggle()
elif event.key == K_TAB:
world.camera_manager.toggle_camera()
elif event.key == K_g:
world.toggle_radar()
elif event.key > K_0 and event.key <= K_9:
world.camera_manager.set_sensor(event.key - 1 - K_0)
elif event.key == K_r and not (pygame.key.get_mods()
& KMOD_CTRL):
world.camera_manager.toggle_recording()
elif event.key == K_r:
if world.recording_enabled:
client.stop_recorder()
world.recording_enabled = False
world.hud.notification("Recorder is OFF")
else:
client.start_recorder("manual_recording.rec")
world.recording_enabled = True
world.hud.notification("Recorder is ON")
if isinstance(self._control, carla.VehicleControl):
if event.key == K_q:
self._control.gear = 1 if self._control.reverse else -1
elif event.key == K_m:
self._control.manual_gear_shift = (
not self._control.manual_gear_shift)
self._control.gear = world.player.get_control().gear
world.hud.notification(
"%s Transmission" %
("Manual" if self._control.manual_gear_shift else
"Automatic"))
elif self._control.manual_gear_shift and event.key == K_COMMA:
self._control.gear = max(-1, self._control.gear - 1)
elif self._control.manual_gear_shift and event.key == K_PERIOD:
self._control.gear = self._control.gear + 1
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_CTRL:
current_lights ^= carla.VehicleLightState.Special1
elif event.key == K_l and pygame.key.get_mods(
) & KMOD_SHIFT:
current_lights ^= carla.VehicleLightState.HighBeam
elif event.key == K_l:
# Use 'L' key to switch between lights:
# closed -> position -> low beam -> fog
if not self._lights & carla.VehicleLightState.Position:
world.hud.notification("Position lights")
current_lights |= carla.VehicleLightState.Position
else:
world.hud.notification("Low beam lights")
current_lights |= carla.VehicleLightState.LowBeam
if self._lights & carla.VehicleLightState.LowBeam:
world.hud.notification("Fog lights")
current_lights |= carla.VehicleLightState.Fog
if self._lights & carla.VehicleLightState.Fog:
world.hud.notification("Lights off")
current_lights ^= carla.VehicleLightState.Position
current_lights ^= carla.VehicleLightState.LowBeam
current_lights ^= carla.VehicleLightState.Fog
elif event.key == K_i:
current_lights ^= carla.VehicleLightState.Interior
elif event.key == K_z:
current_lights ^= carla.VehicleLightState.LeftBlinker
elif event.key == K_x:
current_lights ^= carla.VehicleLightState.RightBlinker
if isinstance(self._control, carla.VehicleControl):
self._parse_vehicle_keys(pygame.key.get_pressed(),
clock.get_time())
self._control.reverse = self._control.gear < 0
# Set automatic control-related vehicle lights
if self._control.brake:
current_lights |= carla.VehicleLightState.Brake
else: # Remove the Brake flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Brake
if self._control.reverse:
current_lights |= carla.VehicleLightState.Reverse
else: # Remove the Reverse flag
current_lights &= carla.VehicleLightState.All ^ carla.VehicleLightState.Reverse
if current_lights != self._lights: # Change the light state only if necessary
self._lights = current_lights
world.player.set_light_state(
carla.VehicleLightState(self._lights))
elif isinstance(self._control, carla.WalkerControl):
self._parse_walker_keys(pygame.key.get_pressed(), clock.get_time(),
world)
return True, self._control
def run_step(self):
"""
Execute on tick of the controller's control loop
Note: Negative target speeds are not yet supported.
Try using simple_vehicle_control or vehicle_longitudinal_control.
If _waypoints are provided, the vehicle moves towards the next waypoint
with the given _target_speed, until reaching the final waypoint. Upon reaching
the final waypoint, _reached_goal is set to True.
If _waypoints is empty, the vehicle moves in its current direction with
the given _target_speed.
If _init_speed is True, the control command is post-processed to ensure that
the initial actor velocity is maintained independent of physics.
"""
self._reached_goal = False
if self._waypoints_updated:
self._waypoints_updated = False
self._update_plan()
if self._offset_updated:
self._offset_updated = False
self._update_offset()
target_speed = self._target_speed
# If target speed is negavite, raise an exception
if target_speed < 0:
raise NotImplementedError(
"Negative target speeds are not yet supported")
self._local_planner.set_speed(target_speed * 3.6)
control = self._local_planner.run_step(debug=False)
# Check if the actor reached the end of the plan
if self._local_planner.done():
self._reached_goal = True
self._actor.apply_control(control)
current_speed = math.sqrt(self._actor.get_velocity().x**2 +
self._actor.get_velocity().y**2)
if self._init_speed:
# If _init_speed is set, and the PID controller is not yet up to the point to take over,
# we manually set the vehicle to drive with the correct velocity
if abs(target_speed - current_speed) > 3:
yaw = self._actor.get_transform().rotation.yaw * (math.pi /
180)
vx = math.cos(yaw) * target_speed
vy = math.sin(yaw) * target_speed
self._actor.set_target_velocity(carla.Vector3D(vx, vy, 0))
# Change Brake light state
if (current_speed > target_speed
or target_speed < 0.2) and not self._brake_lights_active:
light_state = self._actor.get_light_state()
light_state |= carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
self._brake_lights_active = True
if self._brake_lights_active and current_speed < target_speed:
self._brake_lights_active = False
light_state = self._actor.get_light_state()
light_state &= ~carla.VehicleLightState.Brake
self._actor.set_light_state(carla.VehicleLightState(light_state))
def _load_and_run_scenario(self, args, config, customized_data):
"""
Load and run the scenario given by config
"""
# addition: hack
config.weather = WEATHERS[args.weather_index]
if not self._load_and_wait_for_world(args, config.town,
config.ego_vehicles):
self._cleanup()
return
agent_class_name = getattr(self.module_agent, 'get_entry_point')()
try:
self.agent_instance = getattr(self.module_agent,
agent_class_name)(args.agent_config)
config.agent = self.agent_instance
self.sensors = [
sensors_to_icons[sensor['type']]
for sensor in self.agent_instance.sensors()
]
except Exception as e:
print("Could not setup required agent due to {}".format(e))
self._cleanup()
return
# Prepare scenario
print("Preparing scenario: " + config.name)
try:
self._prepare_ego_vehicles(config.ego_vehicles, False)
scenario = RouteScenario(world=self.world,
config=config,
debug_mode=args.debug,
customized_data=customized_data)
except Exception as exception:
print("The scenario cannot be loaded")
if args.debug:
traceback.print_exc()
print(exception)
self._cleanup()
return
# Set the appropriate weather conditions
weather = carla.WeatherParameters(
cloudiness=config.weather.cloudiness,
precipitation=config.weather.precipitation,
precipitation_deposits=config.weather.precipitation_deposits,
wind_intensity=config.weather.wind_intensity,
sun_azimuth_angle=config.weather.sun_azimuth_angle,
sun_altitude_angle=config.weather.sun_altitude_angle,
fog_density=config.weather.fog_density,
fog_distance=config.weather.fog_distance,
wetness=config.weather.wetness)
self.world.set_weather(weather)
# Set the appropriate road friction
if config.friction is not None:
friction_bp = self.world.get_blueprint_library().find(
'static.trigger.friction')
extent = carla.Location(1000000.0, 1000000.0, 1000000.0)
friction_bp.set_attribute('friction', str(config.friction))
friction_bp.set_attribute('extent_x', str(extent.x))
friction_bp.set_attribute('extent_y', str(extent.y))
friction_bp.set_attribute('extent_z', str(extent.z))
# Spawn Trigger Friction
transform = carla.Transform()
transform.location = carla.Location(-10000.0, -10000.0, 0.0)
self.world.spawn_actor(friction_bp, transform)
# night mode
if config.weather.sun_altitude_angle < 0.0:
for vehicle in scenario.ego_vehicles:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
# addition: to turn on lights of
actor_list = self.world.get_actors()
vehicle_list = actor_list.filter('*vehicle*')
for vehicle in vehicle_list:
vehicle.set_light_state(
carla.VehicleLightState(self._vehicle_lights))
try:
# Load scenario and run it
if args.record:
self.client.start_recorder("{}/{}.log".format(
args.record, config.name))
self.manager.load_scenario(scenario, self.agent_instance)
self.statistics_manager.set_route(config.name, config.index,
scenario.scenario)
print('start to run scenario')
self.manager.run_scenario()
print('stop to run scanario')
# Stop scenario
self.manager.stop_scenario()
# register statistics
current_stats_record = self.statistics_manager.compute_route_statistics(
config, self.manager.scenario_duration_system,
self.manager.scenario_duration_game)
# save
# modification
self.statistics_manager.save_record(current_stats_record,
config.index, self.save_path)
# Remove all actors
scenario.remove_all_actors()
settings = self.world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
self.world.apply_settings(settings)
except SensorConfigurationInvalid as e:
self._cleanup(True)
sys.exit(-1)
except Exception as e:
if args.debug:
traceback.print_exc()
print(e)
self._cleanup()
