def execute_npcs_step(self): # TODO this can be parallelized try: for agent, actor in self.npc_agents.items(): new_vehicle = self.carla_bridge.convert_vehicle_from_source_to_agent( actor) curr_control: VehicleControl = agent.run_step( sensors_data=SensorsData(), vehicle=new_vehicle) carla_control = self.carla_bridge.convert_control_from_agent_to_source( curr_control) actor.apply_control(carla_control) except Exception as e: self.logger.error(f"Failed to execute step for NPC. " f"Error: {e}")
def convert_sensor_data_from_source_to_agent(self, source: dict) -> SensorsData: return SensorsData( front_rgb=self.convert_rgb_from_source_to_agent( source=source.get("front_rgb", None) ), rear_rgb=self.convert_rgb_from_source_to_agent( source=source.get("rear_rgb", None) ), front_depth=self.convert_depth_from_source_to_agent( source=source.get("front_depth", None) ), imu_data=self.convert_imu_from_source_to_agent( source=source.get("imu", None) ), )
def convert_sensor_data_from_source_to_agent(self, source: dict) -> SensorsData: """Returns CARLA Sensors Data from raw front RGB, rear RGB, front depth, and IMU Data.""" return SensorsData( front_rgb=self.convert_rgb_from_source_to_agent( source=source.get("front_rgb", None) ), rear_rgb=self.convert_rgb_from_source_to_agent( source=source.get("rear_rgb", None) ), front_depth=self.convert_depth_from_source_to_agent( source=source.get("front_depth", None) ), imu_data=self.convert_imu_from_source_to_agent( source=source.get("imu", None) ), )
def convert_sensor_data_from_source_to_agent(self, source) -> SensorsData: """ Returns Jetson Sensors Data from raw front RGB, rear RGB, front depth, and IMU Data. Args: source (): Returns: SensorData(front_RGB, rear_RGB, front_depth, IMU_Data) """ return SensorsData(front_rgb=self.convert_rgb_from_source_to_agent( source=source.get("front_rgb", None)), rear_rgb=self.convert_rgb_from_source_to_agent( source=source.get("rear_rgb", None)), front_depth=self.convert_depth_from_source_to_agent( source=source.get("front_depth", None)), imu_data=self.convert_imu_from_source_to_agent( source=source.get("imu", None)), location=self.convert_location_from_source_to_agent( source=source.get("location", None)), rotation=self.convert_rotation_from_source_to_agent( source=source.get("rotation", None)), velocity=self.convert_location_from_source_to_agent( source=source.get("velocity", None)))
class CarlaRunner: def __init__(self, carla_settings: CarlaConfig, agent_settings: AgentConfig, npc_agent_class, competition_mode=False, start_bbox: np.ndarray = np.array([5, -5, 0, 13, 5, 50]), lap_count=10): """ Args: carla_settings: CarlaConfig instance agent_settings: AgentConfig instance npc_agent_class: an agent class competition_mode: [Optional] True/False start_bbox: [Optional] array of [minx, miny, minz, maxx, maxy, maxz]. [5, -5, 0, 13, 5, 50] is the bbox for easy_map. [-815, 20, -760, -770, 120, -600] is the bbox for berkeley_minor_map lap_count: [Optional] total lap count """ self.carla_settings = carla_settings self.agent_settings = agent_settings self.carla_bridge = CarlaBridge() self.npc_agent_class = npc_agent_class self.world = None self.client = None self.controller = None self.display = None self.agent = None self.npc_agents: Dict[npc_agent_class, Any] = {} self.agent_collision_counter = 0 self.competition_mode = competition_mode self.start_bbox = start_bbox self.lap_count = lap_count self.completed_lap_count = 0 self.sensor_data = SensorsData() self.vehicle_state = Vehicle() self.start_simulation_time: Optional[float] = None self.start_vehicle_position: Optional[np.array] = None self.end_simulation_time: Optional[float] = None self.end_vehicle_position: Optional[np.array] = None self.logger = logging.getLogger(__name__) self.timestep_counter = 0 def set_carla_world(self) -> Vehicle: """ Initiating the vehicle with loading messages Returns: Vehicle Information """ try: pygame.init() pygame.font.init() self.logger.debug(f"Connecting to {self.carla_settings.host}: " f"{self.carla_settings.port}") self.client = carla.Client(self.carla_settings.host, self.carla_settings.port) if not self.check_version(client=self.client): self.logger.error( f"Version Mismatch: Client = {self.client.get_client_version()}, " f"Server = {self.client.get_server_version()}. \n" f"HINT: Please change carla_version to either 0.9.9 or 0.9.10 " f"in ROAR_Sim.configurations.carla_version.txt") exit(1) if self.carla_settings.should_visualize_with_pygame is True: self.display = pygame.display.set_mode( (self.carla_settings.width, self.carla_settings.height), pygame.HWSURFACE | pygame.DOUBLEBUF) self.logger.debug(f"Setting HUD") hud = HUD(self.carla_settings.width, self.carla_settings.height) self.logger.debug("Setting up world") self.world = World(carla_world=self.client.get_world(), hud=hud, carla_settings=self.carla_settings, agent_settings=self.agent_settings) if self.carla_settings.should_spawn_npcs: self.spawn_npcs() self.logger.debug(f"Connecting to Keyboard controls") self.controller = KeyboardControl( world=self.world, carla_setting=self.carla_settings) self.logger.debug("All settings done") return self.carla_bridge. \ convert_vehicle_from_source_to_agent(self.world.player) except Exception as e: self.logger.error( f"Unable to initiate the world due to error: {e}") raise e def start_game_loop(self, agent, use_manual_control=False, starting_lap_count=0): """Start running the vehicle and stop when finished running the track""" self.agent = agent lap_count = starting_lap_count has_entered_bbox = False should_restart_lap = False try: self.logger.debug("Initiating game") self.agent.start_module_threads() clock = pygame.time.Clock() self.start_simulation_time = time.time() self.start_vehicle_position = self.agent.vehicle.transform.location.to_array( ) while True: # make sure the program does not run above 60 frames per second # this allow proper synchrony between server and client clock.tick_busy_loop(60) should_continue, carla_control = self.controller.parse_events( client=self.client, world=self.world, clock=clock) self.agent_collision_counter = self.get_num_collision() if self.competition_mode: is_currently_in_bbox = self.is_within_start_finish_bbox( curr_pos=self.agent.vehicle.transform.location. to_array()) if has_entered_bbox is True and is_currently_in_bbox is False: has_entered_bbox = False elif has_entered_bbox is False and is_currently_in_bbox is True: has_entered_bbox = True lap_count += 1 if lap_count > self.lap_count: # if i have reached target number of lap counts, break out of game loop break else: self.logger.info( f"Going onto Lap {lap_count} out of {self.lap_count}" ) if len(self.world.collision_sensor.history) > 0: should_restart_lap = True if should_restart_lap: should_continue = False # check for exiting condition if should_continue is False: break self.world.tick(clock) self.world.render(display=self.display) if self.carla_settings.should_visualize_with_pygame is True: pygame.display.flip() self.fetch_data_async() sensor_data, new_vehicle = self.sensor_data.copy( ), self.vehicle_state.copy() if self.carla_settings.save_semantic_segmentation and self.world.semantic_segmentation_sensor_data: Thread(target=lambda: self.world. semantic_segmentation_sensor_data.save_to_disk( (Path("./data/output_oct_10") / "ss" / f"frame_{self.agent.time_counter}.png"). as_posix(), cc.CityScapesPalette), args=()).start() if self.carla_settings.should_spawn_npcs: self.execute_npcs_step() if self.agent_settings.enable_autopilot: if self.agent is None: raise Exception( "In autopilot mode, but no agent is defined.") agent_control = self.agent.run_step( vehicle=new_vehicle, sensors_data=sensor_data) if not use_manual_control: carla_control = self.carla_bridge. \ convert_control_from_agent_to_source(agent_control) self.world.player.apply_control(carla_control) self.timestep_counter += 1 self.completed_lap_count = lap_count - 1 except Exception as e: self.logger.error(f"Error happened, exiting safely. Error: {e}") finally: if self.competition_mode and should_restart_lap: self.restart_on_lap(agent=agent, use_manual_control=use_manual_control, starting_lap_count=lap_count - 1) else: self.on_finish() def restart_on_lap(self, agent, use_manual_control: bool, starting_lap_count: int): self.logger.info(f"Restarting on Lap {starting_lap_count}") self.on_finish() self.set_carla_world() agent.__init__(vehicle=agent.vehicle, agent_settings=agent.agent_settings) self.start_game_loop(agent=agent, use_manual_control=use_manual_control, starting_lap_count=starting_lap_count) def on_finish(self): self.logger.debug("Ending Game") if self.agent is not None: self.agent.shutdown_module_threads() self.end_vehicle_position = self.agent.vehicle.transform.location.to_array( ) else: self.end_vehicle_position = self.start_vehicle_position if self.world is not None: self.end_simulation_time = time.time() self.world.destroy() self.logger.debug("All actors are destroyed") try: pygame.quit() except Exception as e: self.logger.debug( f"Cannot quit pygame normally, force quitting. Error: {e}") self.logger.debug("Game ended") def is_within_start_finish_bbox(self, curr_pos: np.ndarray) -> bool: min_bounding_box = self.start_bbox[:3] max_bounding_box = self.start_bbox[3:] return all( np.logical_and(min_bounding_box < curr_pos, curr_pos < max_bounding_box)) def fetch_data_async(self): t = Thread(target=self.convert_data, args=()) t.start() def convert_data(self): """ Convert data from source to agent Returns: sensors_data: sensor data for agent new_vehicle: the current player's vehicle state """ try: self.sensor_data: SensorsData = \ self.carla_bridge.convert_sensor_data_from_source_to_agent( { "front_rgb": None if self.world.front_rgb_sensor_data is None else self.world.front_rgb_sensor_data, "rear_rgb": None if self.world.rear_rgb_sensor_data is None else self.world.rear_rgb_sensor_data, "front_depth": None if self.world.front_depth_sensor_data is None else self.world.front_depth_sensor_data, "imu": self.world.imu_sensor } ) if self.world.player.is_alive: self.vehicle_state = self.carla_bridge.convert_vehicle_from_source_to_agent( self.world.player) except Exception as e: print("Error", e) self.logger.error(e) def execute_npcs_step(self): # TODO this can be parallelized try: for agent, actor in self.npc_agents.items(): new_vehicle = self.carla_bridge.convert_vehicle_from_source_to_agent( actor) curr_control: VehicleControl = agent.run_step( sensors_data=SensorsData(), vehicle=new_vehicle) carla_control = self.carla_bridge.convert_control_from_agent_to_source( curr_control) actor.apply_control(carla_control) except Exception as e: self.logger.error(f"Failed to execute step for NPC. " f"Error: {e}") def spawn_npcs(self): # parse npc file npc_config_file_path = Path(self.carla_settings.npc_config_file_path) assert npc_config_file_path.exists( ), f"NPC file path {npc_config_file_path.as_posix()} does not exist" npc_configs = json.load(npc_config_file_path.open('r')) npc_configs: List[AgentConfig] = [ AgentConfig.parse_obj(config) for config in npc_configs ] self.world.spawn_npcs(npc_configs) self.npc_agents = { self.npc_agent_class(vehicle=actor, agent_settings=npc_config): actor for actor, npc_config in self.world.npcs_mapping.values() } def get_num_collision(self): collision_sensor: CollisionSensor = self.world.collision_sensor return len(collision_sensor.history) def check_version(self, client): return ("0.9.9" in client.get_server_version()) == ( "0.9.9" in client.get_client_version())