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()