class CarlaEnv(gym.Env): def __init__(self, config=ENV_CONFIG): self.config = config # print('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>'*200) self.command = { "stop": 1, "lane_keep": 2, "turn_right": 3, "turn_left": 4, } # change action space self.action_space = Box(-1.0, 1.0, shape=(ENV_CONFIG["action_dim"], ), dtype=np.float32) if ENV_CONFIG["image_mode"] == "encode": framestack = 7 elif ENV_CONFIG["image_mode"] == "stack": framestack = 3 else: framestack = 3 image_space = Box(0, 255, shape=(config["y_res"], config["x_res"], framestack), dtype=np.float32) self.observation_space = image_space # environment config self._spec = lambda: None self._spec.id = "Carla_v0" # experiment config self.num_steps = 0 self.total_reward = 0 self.episode_id = None self.measurements_file = None self.weather = None self.feature_map = None # actors self.actor_list = [ ] # save actor list for destroying them after finish self.vehicle = None self.collision_sensor = None self.camera_rgb1 = None self.camera_rgb2 = None self.invasion_sensor = None # states and data self._history_info = [] # info history self._history_collision = [] # collision history self._history_invasion = [] # invasion history self._image_rgb1 = [] # save a list of rgb image self._image_rgb2 = [] # save a list of rgb image self._history_waypoint = [] self._obs_collect = [] self._global_step = 0 # # self._d_collect = [] # # initialize our world # self._carla_server = ServerManagerBinary() # self.server_port = random.randint(1000, 60000) # self.world = None # # # start a new carla service # self._carla_server.reset(self.config["host"], self.server_port) # self._carla_server.wait_until_ready() self.server_process = None self.server_port = None self.world = None # self.init_server() self._error_rest_test = 0 def __del__(self): cleanup() def init_server(self): print("Initializing new Carla server...") # Create a new server process and start the client. self.server_port = 2000 self.server_process = subprocess.Popen([ "/home/sdc/Desktop/carla94/CarlaUE4.sh", "/Game/Carla/Maps/Town01", "-benchmark", '-fps=20' "-ResX=1024", "-ResY=768", "-host = 127.0.0.1", "-carla-port=2000", ], preexec_fn=os.setsid, stdout=open(os.devnull, "w")) live_carla_processes.add(self.server_process.pid) # print(live_carla_processes) # live_carla_processes.add(os.getpgid(self.server_process.pid)) try: pre_pid = np.loadtxt(PID_FILE_NAME, ndmin=1) pre_pid = pre_pid.astype(int) if len(pre_pid) > 5: pre_pid = np.delete(pre_pid, range(0, len(pre_pid - 5))) except: pre_pid = [] pid = np.array([x for x in live_carla_processes]) np.savetxt(PID_FILE_NAME, np.concatenate([pre_pid, pid]), fmt='%d') # with open('/tmp/_carla_pid.txt', 'w') as f: # f.write(str(self.server_process.pid)) # f.write(str(os.getpgid(self.server_process.pid))) # write carla server pid into file time.sleep(20) # wait for world get ready # @set_timeout(10) def _restart(self): """restart world and add sensors""" # self.init_server() connect_fail_times = 0 self.world = None while self.world is None: try: self.client = carla.Client(self.config["host"], self.server_port) self.client.set_timeout(2.0) self.world = self.client.get_world() self.map = self.world.get_map() except Exception as e: connect_fail_times += 1 print("Error connecting: {}, attempt {}".format( e, connect_fail_times)) time.sleep(2) if connect_fail_times > 5: break world = self.world self._global_step = 0 # actors self.actor_list = [ ] # save actor list for destroying them after finish self.vehicle = None self.collision_sensor = None self.invasion_sensor = None self._history_info = [] # info history self._history_collision = [] # collision history self._history_invasion = [] # invasion history self._image_rgb1 = [] # save a list of rgb image self._image_rgb2 = [] self._history_waypoint = [] self.other_actors_list = [] #save other actors for a in self.world.get_actors().filter('vehicle.*'): # print(a) try: a.destroy() time.sleep(2) except: pass # for b in self.world.get_actors().filter('sensor.*'): # try: # b.destroy() # time.sleep(2) # except: # pass try: #setup ego_vehicle & other_actor self.scenario_name = 'VehicleTurningLeft' #the name of scenario bp_library = self.world.get_blueprint_library() # setup vehicle scenario_config_file = find_scenario_config(self.scenario_name) self.scenario_configurations = parse_scenario_configuration( scenario_config_file, self.scenario_name) for config in self.scenario_configurations: print(config) # Prepare scenario print("Preparing scenario: " + config.name) scenario_class = ScenarioRunner.get_scenario_class_or_fail( config.type) try: # init ego_vehicle # spawn_point of ego_vehicle spawn_point = config.ego_vehicle.transform # model of ego_vehicle bp_vehicle = config.ego_vehicle.model blueprint = random.choice(bp_library.filter(bp_vehicle)) blueprint.set_attribute('role_name', 'hero') # set ego_vehicle self.vehicle = self.world.try_spawn_actor( blueprint, spawn_point) self.actor_list.append(self.vehicle) # init other_actor for actor in config.other_actors: # spawn_point of other actor actor_point = actor.transform # model of other actor actor_bp_vehicle = actor.model actor_blueprint = random.choice( bp_library.filter(actor_bp_vehicle)) actor_blueprint.set_attribute('role_name', 'scenario') self.other_actor = self.world.try_spawn_actor( actor_blueprint, actor_point) #set other actor self.other_actors_list.append(self.other_actor) self.actor_list.append(self.other_actors_list) #input parameter of scenario self.scenario = scenario_class(self.world, self.vehicle, self.other_actors_list, config.town, False, False) except Exception as exception: print("The scenario cannot be loaded") traceback.print_exc() print(exception) self.cleanup() #setup sensor camera_transform = carla.Transform(carla.Location(x=1, y=0, z=2)) camera_rgb1 = bp_library.find('sensor.camera.rgb') camera_rgb1.set_attribute('fov', '120') camera_rgb1.set_attribute('image_size_x', str(ENV_CONFIG["x_res"])) camera_rgb1.set_attribute('image_size_y', str(ENV_CONFIG["y_res"])) # here is the bug, carla will throw bad_weak_ptr() and block the process self.camera_rgb1 = world.spawn_actor( camera_rgb1, camera_transform, attach_to=self.vehicle) # 32 !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! self.actor_list.append(self.camera_rgb1) bp = bp_library.find('sensor.other.collision') self.collision_sensor = world.try_spawn_actor( bp, carla.Transform(), attach_to=self.vehicle) self.actor_list.append(self.collision_sensor) bp = bp_library.find('sensor.other.lane_detector') self.invasion_sensor = world.try_spawn_actor( bp, carla.Transform(), attach_to=self.vehicle) self.actor_list.append( self.invasion_sensor ) # 39 steps for first time 42 steps for reset except Exception as e: print("spawn fail, sad news", e) def reset(self): error = None for _ in range(100): try: if len(live_carla_processes) == 0: self.init_server() self._restart() # bugggggggggg!!!!!!!!!!!!!!!!!!!!!!!! obs = self._reset() return obs except Exception as e: with open( "/home/gu/error_log %s.txt" % str(datetime.datetime.now()), "w") as f: f.write('============Error====================, %s' % str(e)) print("<<<<<<<<<<Error during reset in env>>>>>>>>>>") cleanup() self.init_server() error = e raise error # @set_timeout(10) def _reset(self): # self._error_rest_test += 1 # if self._error_rest_test < 3: # print(1/0) # else: # print("+++++++++++++++++++++++++++++++++++++++++++++++") weak_self = weakref.ref(self) # set invasion sensor self.invasion_sensor.listen( lambda event: self._parse_invasion(weak_self, event)) # set collision sensor self.collision_sensor.listen( lambda event: self._parse_collision(weak_self, event)) # set rgb camera sensor self.camera_rgb1.listen( lambda image: self._parse_image1(weak_self, image, cc.Raw, 'rgb')) while len(self._image_rgb1) < 4: print("resetting rgb") time.sleep(0.001) if ENV_CONFIG[ "image_mode"] == "encode": # stack gray depth segmentation obs = np.concatenate([ self._image_rgb1[-1], self._image_rgb1[-2], np.zeros([ENV_CONFIG['x_res'], ENV_CONFIG['y_res'], 1]) ], axis=2) else: obs = self._image_rgb1[-1] t = self.vehicle.get_transform() v = self.vehicle.get_velocity() c = self.vehicle.get_control() acceleration = self.vehicle.get_acceleration() if len(self._history_invasion) > 0: invasion = self._history_invasion[-1] else: invasion = [] self.planner() distance = ((self._history_waypoint[-1].transform.location.x - self.vehicle.get_location().x)**2 + (self._history_waypoint[-1].transform.location.y - self.vehicle.get_location().y)**2)**0.5 info = { "speed": math.sqrt(v.x**2 + v.y**2 + v.z**2), # m/s "acceleration": math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2), "location_x": t.location.x, "location_y": t.location.y, "Throttle": c.throttle, "Steer": c.steer, "Brake": c.brake, "command": self.planner(), "distance": distance, "lane_invasion": invasion, "traffic_light": str(self.vehicle.get_traffic_light_state() ), # Red Yellow Green Off Unknown "is_at_traffic_light": self.vehicle.is_at_traffic_light(), # True False "collision": len(self._history_collision) } self._history_info.append(info) self._obs_collect.append(obs[:, :, 0:3]) if len(self._obs_collect) > 32: self._obs_collect.pop(0) mask = self._compute_mask() # define how many channel we want play with if ENV_CONFIG["attention_mode"] == "soft": obs[:, :, 0:ENV_CONFIG["attention_channel"]] = obs[:, :, 0:ENV_CONFIG[ "attention_channel"]] + mask else: obs[:, :, 0:ENV_CONFIG["attention_channel"]] = obs[:, :, 0:ENV_CONFIG[ "attention_channel"]] * mask self._obs_collect.append(np.clip(obs, 0, 255)) # clip in case we want render if len(self._obs_collect) > 32: self._obs_collect.pop(0) return self._obs_collect[-1] @staticmethod def _generate_point_list(): """ generate the Cartesian coordinates for every pixel in the picture, because attention point is represented in Cartesian coordinates(e.g. (-48, -48) (0, 0) (48, 48)) but the position of pixel is represented by index(e.g. [95, 0] [47, 47] [0 95]) :return: Cartesian coordinates for pixels """ r = int(ENV_CONFIG["x_res"] / 2) point_list = [] for i in range(r, -r, -1): for j in range(-r, r, 1): point_list.append((j, i)) return point_list @staticmethod def _compute_distance_transform(d, action=np.zeros(ENV_CONFIG["action_dim"])): """compute the variance for attention mask when we adding noise if we specify attention mode to soft we will use this function """ if ENV_CONFIG["action_dim"] == 5: # in care our poor agent see nothing we set threshold equal to 5 # in other word if action[4] = 0 then action[4] will be set to 5 # action[4] belong to range(-1, 1) we project it to [0, 70] r = 35 * (1 + action[4]) if 35 * (1 + action[4]) > 5 else 5 else: r = 25 if ENV_CONFIG["attention_mode"] == "soft": # d is the threshold of distance between attention point # if the distance is greater then d we add noise on image # the strength of noise is linear to distance d = 0 if d < r else 2 * d elif ENV_CONFIG["attention_mode"] == "hard": # it behave like mask(i.e. 0 for totally dark) d = 1 if d < r else (r / d)**2.5 # d = -24 + 2*d return d def _compute_mask(self, action=np.zeros(ENV_CONFIG["action_dim"])): """"compute mask for attention""" if ENV_CONFIG["action_dim"] == 4 or ENV_CONFIG["action_dim"] == 5: mu_1 = int(ENV_CONFIG["x_res"] * action[2] * 0.5) mu_2 = int(ENV_CONFIG["y_res"] * action[3] * 0.5) elif ENV_CONFIG["action_dim"] == 2: mu_1 = 0 mu_2 = 0 d_list = [] point_list = self._generate_point_list() for p in point_list: d = np.sqrt((mu_1 - p[0])**2 + (mu_2 - p[1])**2) if ENV_CONFIG["attention_mode"] == "soft": # self._d_collect.append(d) p_mask = float( self._compute_distance_transform(d, action) * np.random.randn(1)) elif ENV_CONFIG["attention_mode"] == "hard": p_mask = float(self._compute_distance_transform(d, action)) else: # if we want use raw rgb p_mask = 1 d_list.append(p_mask) mask = np.reshape(d_list, [ENV_CONFIG["x_res"], ENV_CONFIG["y_res"]]) return mask[:, :, np.newaxis] @staticmethod def _parse_image1(weak_self, image, cc, use): """convert BGRA to RGB""" self = weak_self() if not self: return def convert(cc): image.convert(cc) array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (image.height, image.width, 4)) array = array[:, :, -2:-5:-1] array = array.astype(np.float32) return array if use == 'rgb': array = convert(cc) self._image_rgb1.append(array) if len(self._image_rgb1) > 32: self._image_rgb1.pop(0) @staticmethod def _parse_image2(weak_self, image, cc, use): """convert BGRA to RGB""" self = weak_self() if not self: return def convert(cc): image.convert(cc) array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) array = np.reshape(array, (image.height, image.width, 4)) array = array[:, :, -2:-5:-1] array = array.astype(np.float32) return array if use == 'rgb': array = convert(cc) self._image_rgb2.append(array) if len(self._image_rgb2) > 32: self._image_rgb2.pop(0) @staticmethod def _parse_collision(weak_self, event): self = weak_self() if not self: return impulse = event.normal_impulse intensity = math.sqrt(impulse.x**2 + impulse.y**2 + impulse.z**2) self._history_collision.append((event.frame_number, intensity)) if len(self._history_collision) > 32: self._history_collision.pop(0) @staticmethod def _parse_invasion(weak_self, event): self = weak_self() if not self: return # print(str(event.crossed_lane_markings)) [carla.libcarla.LaneMarking.Solid] text = [ '%r' % str(x).split()[-1] for x in set(event.crossed_lane_markings) ] # S for Solid B for Broken self._history_invasion.append(text[0][1]) if len(self._history_invasion) > 32: self._history_invasion.pop(0) def step(self, action): try: obs = self._step(action) return obs except Exception as e: print("Error during step, terminating episode early") print(e) return self._obs_collect[-1], 0, True, self._history_info[-1] # @set_timeout(10) def _step(self, action): self._global_step += 1 def compute_reward(info, prev_info): reward = 0.0 reward += np.clip(info["speed"], 0, 15) / 3 reward += info['distance'] if info["collision"] == 1: reward -= 70 elif 2 <= info["collision"] < 5: reward -= info['speed'] * 2 elif info["collision"] > 5: reward -= info['speed'] * 1 print(self._global_step, "current speed", info["speed"], "collision", info['collision']) new_invasion = list( set(info["lane_invasion"]) - set(prev_info["lane_invasion"])) if 'S' in new_invasion: # go across solid lane reward -= info["speed"] elif 'B' in new_invasion: # go across broken lane reward -= 0.4 * info["speed"] return reward throttle = float(np.clip(action[0], 0, 1)) brake = float(np.abs(np.clip(action[0], -1, 0))) steer = float(np.clip(action[1], -1, 1)) distance_before_act = ( (self._history_waypoint[-1].transform.location.x - self.vehicle.get_location().x)**2 + (self._history_waypoint[-1].transform.location.y - self.vehicle.get_location().y)**2)**0.5 self.vehicle.apply_control( carla.VehicleControl(throttle=throttle, brake=brake, steer=steer)) # sleep a little waiting for the responding from simulator if ENV_CONFIG[ "attention_mode"] == "None": # or ENV_CONFIG["attention_mode"] == "hard": time.sleep(0.04) t = self.vehicle.get_transform() v = self.vehicle.get_velocity() c = self.vehicle.get_control() acceleration = self.vehicle.get_acceleration() if len(self._history_invasion) > 0: invasion = self._history_invasion[-1] else: invasion = [] command = self.planner() distance_after_act = ( (self._history_waypoint[-2].transform.location.x - self.vehicle.get_location().x)**2 + (self._history_waypoint[-2].transform.location.y - self.vehicle.get_location().y)**2)**0.5 info = { "speed": math.sqrt(v.x**2 + v.y**2 + v.z**2), # m/s "acceleration": math.sqrt(acceleration.x**2 + acceleration.y**2 + acceleration.z**2), "location_x": t.location.x, "location_y": t.location.y, "Throttle": c.throttle, "Steer": c.steer, "Brake": c.brake, "command": command, "distance": distance_before_act - distance_after_act, # distance to waypoint "lane_invasion": invasion, "traffic_light": str(self.vehicle.get_traffic_light_state() ), # Red Yellow Green Off Unknown "is_at_traffic_light": self.vehicle.is_at_traffic_light(), # True False "collision": len(self._history_collision) } self._history_info.append(info) reward = compute_reward(self._history_info[-1], self._history_info[-2]) # print(self._history_info[-1]["speed"], self._history_info[-1]["collision"]) # early stop done = False if ENV_CONFIG["early_stop"]: if len(self._history_collision) > 0 and self._global_step > 60: # print("collisin length", len(self._history_collision)) done = True # self.destroy_actor() # elif reward < -100: # done = True if ENV_CONFIG[ "image_mode"] == "encode": # stack gray depth segmentation obs = np.concatenate([ self._image_rgb1[-1], self._image_rgb1[-2], self.encode_measurement(info) ], axis=2) else: obs = self._image_rgb1[-1] mask = self._compute_mask(action) if ENV_CONFIG["attention_mode"] == "soft": obs[:, :, 0:ENV_CONFIG["attention_channel"]] = obs[:, :, 0:ENV_CONFIG[ "attention_channel"]] + mask else: obs[:, :, 0:ENV_CONFIG["attention_channel"]] = obs[:, :, 0:ENV_CONFIG[ "attention_channel"]] * mask self._obs_collect.append(np.clip(obs, 0, 255)) # clip in case we want render if len(self._obs_collect) > 32: self._obs_collect.pop(0) return self._obs_collect[-1], reward, done, self._history_info[-1] def render(self): display = pygame.display.set_mode( (ENV_CONFIG["x_res"], ENV_CONFIG["y_res"]), pygame.HWSURFACE | pygame.DOUBLEBUF) # surface = pygame.surfarray.make_surface(env._image_rgb1[-1].swapaxes(0, 1)) surface = pygame.surfarray.make_surface( env._obs_collect[-1][:, :, 0:3].swapaxes(0, 1)) display.blit(surface, (0, 0)) time.sleep(0.01) pygame.display.flip() def planner(self): waypoint = self.map.get_waypoint(self.vehicle.get_location()) waypoint = random.choice(waypoint.next(12.0)) self._history_waypoint.append(waypoint) yaw = waypoint.transform.rotation.yaw if yaw > -90 or yaw < 60: command = "turn_right" elif yaw > 60 and yaw < 120: command = "lane_keep" elif yaw > 120 or yaw < -90: command = "turn_left" return self.command[command] @staticmethod def encode_measurement(py_measurements): """encode measurements into another channel""" feature_map = np.zeros([4, 4]) feature_map[0, :] = (py_measurements["command"]) * 60.0 feature_map[1, :] = (py_measurements["speed"]) * 4.0 feature_map[2, :] = (py_measurements["command"]) * 60.0 feature_map[3, :] = (py_measurements["Steer"] + 1) * 120.0 stack = int(ENV_CONFIG["x_res"] / 4) feature_map = np.tile(feature_map, (stack, stack)) feature_map = feature_map.astype(np.float32) return feature_map[:, :, np.newaxis] # load the scenario def load_scenario(self): self.manager = ScenarioManager(self.world, False) self.manager.load_scenario(self.scenario) # run the scenario def run_scenario(self): self.manager.run_scenario() # analyse the result of env in the scenario def analyse_scenario(self): for config in self.scenario_configurations: current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) junit_filename = None if False: junit_filename = config.name + current_time + ".xml" filename = None if False: filename = config.name + current_time + ".txt" if not self.manager.analyze_scenario(False, filename, junit_filename): print( "==================================================Success!==================================================\n\n" ) self.score = 'success' self.final_score(self.score) time.sleep(1) else: print( "==================================================Failure!==================================================\n\n" ) self.score = 'failure' self.final_score(self.score) time.sleep(1) # input the final score def final_score(self, score): self.total_score = [] self.total_score.append(self.score) return self.total_score # write the result into local txt def write_result(self, times): current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) filename = self.scenario_name + '_training_result' + ".txt" if times == 1: with open(filename, "a+") as fd: fd.write('========== Start tarning ! ' + current_time + '==========\n') fd.write(str(times) + ': ' + str(self.total_score) + '\n') else: with open(filename, "a+") as fd: fd.write(str(times) + ': ' + str(self.total_score) + '\n')
class ScenarioRunner(object): """ This is the core scenario runner module. It is responsible for running (and repeating) a single scenario or a list of scenarios. Usage: scenario_runner = ScenarioRunner(args) scenario_runner.run() del scenario_runner """ ego_vehicles = [] # Tunable parameters client_timeout = 10.0 # in seconds wait_for_world = 20.0 # in seconds frame_rate = 10.0 # in Hz # CARLA world and scenario handlers world = None manager = None additional_scenario_module = None agent_instance = None module_agent = None def __init__(self, args): """ Setup CARLA client and world Setup ScenarioManager """ self._args = args if args.timeout: self.client_timeout = float(args.timeout) # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. self.client = carla.Client(args.host, int(args.port)) self.client.set_timeout(self.client_timeout) self.traffic_manager = self.client.get_trafficmanager(int(self._args.trafficManagerPort)) dist = pkg_resources.get_distribution("carla") if LooseVersion(dist.version) < LooseVersion('0.9.10'): raise ImportError("CARLA version 0.9.10 or newer required. CARLA version found: {}".format(dist)) # Load agent if requested via command line args # If something goes wrong an exception will be thrown by importlib (ok here) if self._args.agent is not None: module_name = os.path.basename(args.agent).split('.')[0] sys.path.insert(0, os.path.dirname(args.agent)) self.module_agent = importlib.import_module(module_name) # Create the ScenarioManager self.manager = ScenarioManager(self._args.debug, self._args.sync, self._args.timeout) # Create signal handler for SIGINT self._shutdown_requested = False if sys.platform != 'win32': signal.signal(signal.SIGHUP, self._signal_handler) signal.signal(signal.SIGINT, self._signal_handler) signal.signal(signal.SIGTERM, self._signal_handler) self._start_wall_time = datetime.now() def destroy(self): """ Cleanup and delete actors, ScenarioManager and CARLA world """ self._cleanup() if self.manager is not None: del self.manager if self.world is not None: del self.world if self.client is not None: del self.client def _signal_handler(self, signum, frame): """ Terminate scenario ticking when receiving a signal interrupt """ self._shutdown_requested = True if self.manager: self.manager.stop_scenario() self._cleanup() if not self.manager.get_running_status(): raise RuntimeError("Timeout occured during scenario execution") def _get_scenario_class_or_fail(self, scenario): """ Get scenario class by scenario name If scenario is not supported or not found, exit script """ # Path of all scenario at "srunner/scenarios" folder + the path of the additional scenario argument scenarios_list = glob.glob("{}/srunner/scenarios/*.py".format(os.getenv('SCENARIO_RUNNER_ROOT', "./"))) scenarios_list.append(self._args.additionalScenario) for scenario_file in scenarios_list: # Get their module module_name = os.path.basename(scenario_file).split('.')[0] sys.path.insert(0, os.path.dirname(scenario_file)) scenario_module = importlib.import_module(module_name) # And their members of type class for member in inspect.getmembers(scenario_module, inspect.isclass): if scenario in member: return member[1] # Remove unused Python paths sys.path.pop(0) print("Scenario '{}' not supported ... Exiting".format(scenario)) sys.exit(-1) def _cleanup(self): """ Remove and destroy all actors """ # Simulation still running and in synchronous mode? if self.world is not None and self._args.sync: try: # Reset to asynchronous mode settings = self.world.get_settings() settings.synchronous_mode = False settings.fixed_delta_seconds = None self.world.apply_settings(settings) except RuntimeError: sys.exit(-1) self.manager.cleanup() CarlaDataProvider.cleanup() for i, _ in enumerate(self.ego_vehicles): if self.ego_vehicles[i]: if not self._args.waitForEgo: print("Destroying ego vehicle {}".format(self.ego_vehicles[i].id)) self.ego_vehicles[i].destroy() self.ego_vehicles[i] = None self.ego_vehicles = [] if self.agent_instance: self.agent_instance.destroy() self.agent_instance = None def _prepare_ego_vehicles(self, ego_vehicles): """ Spawn or update the ego vehicles """ if not self._args.waitForEgo: for vehicle in ego_vehicles: self.ego_vehicles.append(CarlaDataProvider.request_new_actor(vehicle.model, vehicle.transform, vehicle.rolename, color=vehicle.color, actor_category=vehicle.category)) else: ego_vehicle_missing = True while ego_vehicle_missing: self.ego_vehicles = [] ego_vehicle_missing = False for ego_vehicle in ego_vehicles: ego_vehicle_found = False carla_vehicles = CarlaDataProvider.get_world().get_actors().filter('vehicle.*') for carla_vehicle in carla_vehicles: if carla_vehicle.attributes['role_name'] == ego_vehicle.rolename: ego_vehicle_found = True self.ego_vehicles.append(carla_vehicle) break if not ego_vehicle_found: ego_vehicle_missing = True break for i, _ in enumerate(self.ego_vehicles): self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) CarlaDataProvider.register_actor(self.ego_vehicles[i]) # sync state if CarlaDataProvider.is_sync_mode(): self.world.tick() else: self.world.wait_for_tick() def _analyze_scenario(self, config): """ Provide feedback about success/failure of a scenario """ # Create the filename current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) junit_filename = None json_filename = None config_name = config.name if self._args.outputDir != '': config_name = os.path.join(self._args.outputDir, config_name) if self._args.junit: junit_filename = config_name + current_time + ".xml" if self._args.json: json_filename = config_name + current_time + ".json" filename = None if self._args.file: filename = config_name + current_time + ".txt" if not self.manager.analyze_scenario(self._args.output, filename, junit_filename, json_filename): print("All scenario tests were passed successfully!") else: print("Not all scenario tests were successful") if not (self._args.output or filename or junit_filename): print("Please run with --output for further information") def _record_criteria(self, criteria, name): """ Filter the JSON serializable attributes of the criterias and dumps them into a file. This will be used by the metrics manager, in case the user wants specific information about the criterias. """ file_name = name[:-4] + ".json" # Filter the attributes that aren't JSON serializable with open('temp.json', 'w') as fp: criteria_dict = {} for criterion in criteria: criterion_dict = criterion.__dict__ criteria_dict[criterion.name] = {} for key in criterion_dict: if key != "name": try: key_dict = {key: criterion_dict[key]} json.dump(key_dict, fp, sort_keys=False, indent=4) criteria_dict[criterion.name].update(key_dict) except TypeError: pass os.remove('temp.json') # Save the criteria dictionary into a .json file with open(file_name, 'w') as fp: json.dump(criteria_dict, fp, sort_keys=False, indent=4) def _load_and_wait_for_world(self, town, ego_vehicles=None): """ Load a new CARLA world and provide data to CarlaDataProvider """ if self._args.reloadWorld: self.world = self.client.load_world(town) else: # if the world should not be reloaded, wait at least until all ego vehicles are ready ego_vehicle_found = False if self._args.waitForEgo: while not ego_vehicle_found and not self._shutdown_requested: vehicles = self.client.get_world().get_actors().filter('vehicle.*') for ego_vehicle in ego_vehicles: ego_vehicle_found = False for vehicle in vehicles: if vehicle.attributes['role_name'] == ego_vehicle.rolename: ego_vehicle_found = True break if not ego_vehicle_found: print("Not all ego vehicles ready. Waiting ... ") time.sleep(1) break self.world = self.client.get_world() if self._args.sync: settings = self.world.get_settings() settings.synchronous_mode = True settings.fixed_delta_seconds = 1.0 / self.frame_rate self.world.apply_settings(settings) self.traffic_manager.set_synchronous_mode(True) self.traffic_manager.set_random_device_seed(int(self._args.trafficManagerSeed)) CarlaDataProvider.set_client(self.client) CarlaDataProvider.set_world(self.world) CarlaDataProvider.set_traffic_manager_port(int(self._args.trafficManagerPort)) # Wait for the world to be ready if CarlaDataProvider.is_sync_mode(): self.world.tick() else: self.world.wait_for_tick() if CarlaDataProvider.get_map().name != town and CarlaDataProvider.get_map().name != "OpenDriveMap": print("The CARLA server uses the wrong map: {}".format(CarlaDataProvider.get_map().name)) print("This scenario requires to use map: {}".format(town)) return False return True def _load_and_run_scenario(self, config): """ Load and run the scenario given by config """ result = False if not self._load_and_wait_for_world(config.town, config.ego_vehicles): self._cleanup() return False if self._args.agent: agent_class_name = self.module_agent.__name__.title().replace('_', '') try: self.agent_instance = getattr(self.module_agent, agent_class_name)(self._args.agentConfig) config.agent = self.agent_instance except Exception as e: # pylint: disable=broad-except traceback.print_exc() print("Could not setup required agent due to {}".format(e)) self._cleanup() return False # Prepare scenario print("Preparing scenario: " + config.name) try: self._prepare_ego_vehicles(config.ego_vehicles) if self._args.openscenario: scenario = OpenScenario(world=self.world, ego_vehicles=self.ego_vehicles, config=config, config_file=self._args.openscenario, timeout=100000) elif self._args.route: scenario = RouteScenario(world=self.world, config=config, debug_mode=self._args.debug) else: scenario_class = self._get_scenario_class_or_fail(config.type) scenario = scenario_class(self.world, self.ego_vehicles, config, self._args.randomize, self._args.debug) except Exception as exception: # pylint: disable=broad-except print("The scenario cannot be loaded") traceback.print_exc() print(exception) self._cleanup() return False try: if self._args.record: recorder_name = "{}/{}/{}.log".format( os.getenv('SCENARIO_RUNNER_ROOT', "./"), self._args.record, config.name) self.client.start_recorder(recorder_name, True) # Load scenario and run it self.manager.load_scenario(scenario, self.agent_instance) self.manager.run_scenario() # Provide outputs if required self._analyze_scenario(config) # Remove all actors, stop the recorder and save all criterias (if needed) scenario.remove_all_actors() if self._args.record: self.client.stop_recorder() self._record_criteria(self.manager.scenario.get_criteria(), recorder_name) result = True except Exception as e: # pylint: disable=broad-except traceback.print_exc() print(e) result = False self._cleanup() return result def _run_scenarios(self): """ Run conventional scenarios (e.g. implemented using the Python API of ScenarioRunner) """ result = False # Load the scenario configurations provided in the config file scenario_configurations = ScenarioConfigurationParser.parse_scenario_configuration( self._args.scenario, self._args.configFile) if not scenario_configurations: print("Configuration for scenario {} cannot be found!".format(self._args.scenario)) return result # Execute each configuration for config in scenario_configurations: for _ in range(self._args.repetitions): result = self._load_and_run_scenario(config) self._cleanup() return result def _run_route(self): """ Run the route scenario """ result = False if self._args.route: routes = self._args.route[0] scenario_file = self._args.route[1] single_route = None if len(self._args.route) > 2: single_route = self._args.route[2] # retrieve routes route_configurations = RouteParser.parse_routes_file(routes, scenario_file, single_route) for config in route_configurations: for _ in range(self._args.repetitions): result = self._load_and_run_scenario(config) self._cleanup() return result def _run_openscenario(self): """ Run a scenario based on OpenSCENARIO """ # Load the scenario configurations provided in the config file if not os.path.isfile(self._args.openscenario): print("File does not exist") self._cleanup() return False config = OpenScenarioConfiguration(self._args.openscenario, self.client) result = self._load_and_run_scenario(config) self._cleanup() return result def run(self): """ Run all scenarios according to provided commandline args """ result = True if self._args.openscenario: result = self._run_openscenario() elif self._args.route: result = self._run_route() else: result = self._run_scenarios() print("No more scenarios .... Exiting") return result
class ScenarioRunner(object): """ This is the core scenario runner module. It is responsible for running (and repeating) a single scenario or a list of scenarios. Usage: scenario_runner = ScenarioRunner(args) scenario_runner.run() del scenario_runner """ ego_vehicles = [] # Tunable parameters client_timeout = 10.0 # in seconds wait_for_world = 20.0 # in seconds frame_rate = 20.0 # in Hz # CARLA world and scenario handlers world = None manager = None additional_scenario_module = None agent_instance = None module_agent = None def __init__(self, args): """ Setup CARLA client and world Setup ScenarioManager """ self._args = args if args.timeout: self.client_timeout = float(args.timeout) # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. self.client = carla.Client(args.host, int(args.port)) self.client.set_timeout(self.client_timeout) dist = pkg_resources.get_distribution("carla") if LooseVersion(dist.version) < LooseVersion('0.9.8'): raise ImportError( "CARLA version 0.9.8 or newer required. CARLA version found: {}" .format(dist)) # Load additional scenario definitions, if there are any # If something goes wrong an exception will be thrown by importlib (ok here) if self._args.additionalScenario != '': module_name = os.path.basename( args.additionalScenario).split('.')[0] sys.path.insert(0, os.path.dirname(args.additionalScenario)) self.additional_scenario_module = importlib.import_module( module_name) # Load agent if requested via command line args # If something goes wrong an exception will be thrown by importlib (ok here) if self._args.agent is not None: module_name = os.path.basename(args.agent).split('.')[0] sys.path.insert(0, os.path.dirname(args.agent)) self.module_agent = importlib.import_module(module_name) # Create the ScenarioManager self.manager = ScenarioManager(self._args.debug, self._args.challenge, self._args.timeout) # Create signal handler for SIGINT self._shutdown_requested = False signal.signal(signal.SIGHUP, self._signal_handler) signal.signal(signal.SIGINT, self._signal_handler) signal.signal(signal.SIGTERM, self._signal_handler) self._start_wall_time = datetime.now() def destroy(self): """ Cleanup and delete actors, ScenarioManager and CARLA world """ self._cleanup() if self.manager is not None: del self.manager if self.world is not None: del self.world if self.client is not None: del self.client def _signal_handler(self, signum, frame): """ Terminate scenario ticking when receiving a signal interrupt """ self._shutdown_requested = True if self.manager: self.manager.stop_scenario() self._cleanup() if not self.manager.get_running_status(): raise RuntimeError("Timeout occured during scenario execution") def _within_available_time(self): """ Check if the elapsed runtime is within the remaining user time budget Only relevant when running in challenge mode """ current_time = datetime.now() elapsed_seconds = (current_time - self._start_wall_time).seconds return elapsed_seconds < os.getenv('CHALLENGE_TIME_AVAILABLE', '1080000') def _get_scenario_class_or_fail(self, scenario): """ Get scenario class by scenario name If scenario is not supported or not found, exit script """ if scenario in globals(): return globals()[scenario] for member in inspect.getmembers(self.additional_scenario_module): if scenario in member and inspect.isclass(member[1]): return member[1] print("Scenario '{}' not supported ... Exiting".format(scenario)) sys.exit(-1) def _cleanup(self): """ Remove and destroy all actors """ if self.world is not None and self.manager is not None \ and self._args.agent and self.manager.get_running_status(): # Reset to asynchronous mode settings = self.world.get_settings() settings.synchronous_mode = False settings.fixed_delta_seconds = None self.world.apply_settings(settings) self.client.stop_recorder() self.manager.cleanup() CarlaDataProvider.cleanup() CarlaActorPool.cleanup() for i, _ in enumerate(self.ego_vehicles): if self.ego_vehicles[i]: if not self._args.waitForEgo: print("Destroying ego vehicle {}".format( self.ego_vehicles[i].id)) self.ego_vehicles[i].destroy() self.ego_vehicles[i] = None self.ego_vehicles = [] if self.agent_instance: self.agent_instance.destroy() self.agent_instance = None def _prepare_ego_vehicles(self, ego_vehicles): """ Spawn or update the ego vehicles """ if not self._args.waitForEgo: for vehicle in ego_vehicles: self.ego_vehicles.append( CarlaActorPool.setup_actor( vehicle.model, vehicle.transform, vehicle.rolename, True, color=vehicle.color, actor_category=vehicle.category)) else: ego_vehicle_missing = True while ego_vehicle_missing: self.ego_vehicles = [] ego_vehicle_missing = False for ego_vehicle in ego_vehicles: ego_vehicle_found = False carla_vehicles = CarlaDataProvider.get_world().get_actors( ).filter('vehicle.*') for carla_vehicle in carla_vehicles: if carla_vehicle.attributes[ 'role_name'] == ego_vehicle.rolename: ego_vehicle_found = True self.ego_vehicles.append(carla_vehicle) break if not ego_vehicle_found: ego_vehicle_missing = True break for i, _ in enumerate(self.ego_vehicles): self.ego_vehicles[i].set_transform(ego_vehicles[i].transform) # sync state CarlaDataProvider.perform_carla_tick() def _analyze_scenario(self, config): """ Provide feedback about success/failure of a scenario """ current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) junit_filename = None config_name = config.name if self._args.outputDir != '': config_name = os.path.join(self._args.outputDir, config_name) if self._args.junit: junit_filename = config_name + current_time + ".xml" filename = None if self._args.file: filename = config_name + current_time + ".txt" if not self.manager.analyze_scenario(self._args.output, filename, junit_filename): print("All scenario tests were passed successfully!") else: print("Not all scenario tests were successful") if not (self._args.output or filename or junit_filename): print("Please run with --output for further information") def _load_and_wait_for_world(self, town, ego_vehicles=None): """ Load a new CARLA world and provide data to CarlaActorPool and CarlaDataProvider """ if self._args.reloadWorld: self.world = self.client.load_world(town) else: # if the world should not be reloaded, wait at least until all ego vehicles are ready ego_vehicle_found = False if self._args.waitForEgo: while not ego_vehicle_found and not self._shutdown_requested: vehicles = self.client.get_world().get_actors().filter( 'vehicle.*') for ego_vehicle in ego_vehicles: ego_vehicle_found = False for vehicle in vehicles: if vehicle.attributes[ 'role_name'] == ego_vehicle.rolename: ego_vehicle_found = True break if not ego_vehicle_found: print("Not all ego vehicles ready. Waiting ... ") time.sleep(1) break self.world = self.client.get_world() CarlaActorPool.set_client(self.client) CarlaActorPool.set_world(self.world) CarlaDataProvider.set_world(self.world) settings = self.world.get_settings() settings.fixed_delta_seconds = 1.0 / self.frame_rate self.world.apply_settings(settings) if self._args.agent: settings = self.world.get_settings() settings.synchronous_mode = True self.world.apply_settings(settings) # Wait for the world to be ready if self.world.get_settings().synchronous_mode: CarlaDataProvider.perform_carla_tick() else: self.world.wait_for_tick() if CarlaDataProvider.get_map( ).name != town and CarlaDataProvider.get_map().name != "OpenDriveMap": print("The CARLA server uses the wrong map: {}".format( CarlaDataProvider.get_map().name)) print("This scenario requires to use map: {}".format(town)) return False return True def _load_and_run_scenario(self, config): """ Load and run the scenario given by config """ result = False if not self._load_and_wait_for_world(config.town, config.ego_vehicles): self._cleanup() return False if self._args.agent: agent_class_name = self.module_agent.__name__.title().replace( '_', '') try: self.agent_instance = getattr(self.module_agent, agent_class_name)( self._args.agentConfig) config.agent = self.agent_instance except Exception as e: # pylint: disable=broad-except traceback.print_exc() print("Could not setup required agent due to {}".format(e)) self._cleanup() return False # Prepare scenario print("Preparing scenario: " + config.name) try: self._prepare_ego_vehicles(config.ego_vehicles) if self._args.openscenario: scenario = OpenScenario(world=self.world, ego_vehicles=self.ego_vehicles, config=config, config_file=self._args.openscenario, timeout=100000) elif self._args.route: scenario = RouteScenario(world=self.world, config=config, debug_mode=self._args.debug) else: scenario_class = self._get_scenario_class_or_fail(config.type) scenario = scenario_class(self.world, self.ego_vehicles, config, self._args.randomize, self._args.debug) except Exception as exception: # pylint: disable=broad-except print("The scenario cannot be loaded") traceback.print_exc() print(exception) self._cleanup() return False # Set the appropriate weather conditions self.world.set_weather(config.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) try: # Load scenario and run it if self._args.record: self.client.start_recorder("{}/{}.log".format( os.getenv('ROOT_SCENARIO_RUNNER', "./"), config.name)) self.manager.load_scenario(scenario, self.agent_instance) self.manager.run_scenario() # Provide outputs if required self._analyze_scenario(config) # Remove all actors scenario.remove_all_actors() result = True except SensorConfigurationInvalid as e: self._cleanup() ChallengeStatisticsManager.record_fatal_error(e) sys.exit(-1) except Exception as e: # pylint: disable=broad-except traceback.print_exc() if self._args.challenge: ChallengeStatisticsManager.set_error_message( traceback.format_exc()) print(e) result = False self._cleanup() return result def _run_scenarios(self): """ Run conventional scenarios (e.g. implemented using the Python API of ScenarioRunner) """ result = False # Setup and run the scenarios for repetition times for _ in range(int(self._args.repetitions)): # Load the scenario configurations provided in the config file scenario_configurations = None scenario_config_file = ScenarioConfigurationParser.find_scenario_config( self._args.scenario, self._args.configFile) if scenario_config_file is None: print("Configuration for scenario {} cannot be found!".format( self._args.scenario)) continue scenario_configurations = ScenarioConfigurationParser.parse_scenario_configuration( scenario_config_file, self._args.scenario) # Execute each configuration for config in scenario_configurations: result = self._load_and_run_scenario(config) self._cleanup() return result def _run_challenge(self): """ Run the challenge mode """ result = False phase_codename = os.getenv('CHALLENGE_PHASE_CODENAME', 'dev_track_3') phase = phase_codename.split("_")[0] repetitions = self._args.repetitions if self._args.challenge: weather_profiles = CarlaDataProvider.find_weather_presets() scenario_runner_root = os.getenv('ROOT_SCENARIO_RUNNER', "./") if phase == 'dev': routes = '{}/srunner/challenge/routes_devtest.xml'.format( scenario_runner_root) repetitions = 1 elif phase == 'validation': routes = '{}/srunner/challenge/routes_testprep.xml'.format( scenario_runner_root) repetitions = 3 elif phase == 'test': routes = '{}/srunner/challenge/routes_testchallenge.xml'.format( scenario_runner_root) repetitions = 3 else: # debug mode routes = '{}/srunner/challenge/routes_debug.xml'.format( scenario_runner_root) repetitions = 1 if self._args.route: routes = self._args.route[0] scenario_file = self._args.route[1] single_route = None if len(self._args.route) > 2: single_route = self._args.route[2] # retrieve routes route_descriptions_list = RouteParser.parse_routes_file( routes, single_route) # find and filter potential scenarios for each of the evaluated routes # For each of the routes and corresponding possible scenarios to be evaluated. if self._args.challenge: n_routes = len(route_descriptions_list) * repetitions ChallengeStatisticsManager.set_number_of_scenarios(n_routes) for _, route_description in enumerate(route_descriptions_list): for repetition in range(repetitions): if self._args.challenge and not self._within_available_time(): error_message = 'Not enough simulation time available to continue' print(error_message) ChallengeStatisticsManager.record_fatal_error( error_message) self._cleanup() return False config = RouteScenarioConfiguration(route_description, scenario_file) if self._args.challenge: profile = weather_profiles[repetition % len(weather_profiles)] config.weather = profile[0] config.weather.sun_azimuth_angle = -1 config.weather.sun_altitude_angle = -1 result = self._load_and_run_scenario(config) self._cleanup() return result def _run_openscenario(self): """ Run a scenario based on OpenSCENARIO """ # Load the scenario configurations provided in the config file if not os.path.isfile(self._args.openscenario): print("File does not exist") self._cleanup() return False config = OpenScenarioConfiguration(self._args.openscenario, self.client) result = self._load_and_run_scenario(config) self._cleanup() return result def run(self): """ Run all scenarios according to provided commandline args """ result = True if self._args.openscenario: result = self._run_openscenario() elif self._args.route or self._args.challenge: result = self._run_challenge() else: result = self._run_scenarios() print("No more scenarios .... Exiting") return result
class ScenarioRunner(object): """ This is the core scenario runner module. It is responsible for running (and repeating) a single scenario or a list of scenarios. Usage: scenario_runner = ScenarioRunner(args) scenario_runner.run(args) del scenario_runner """ ego_vehicle = None actors = [] # Tunable parameters client_timeout = 10.0 # in seconds wait_for_world = 10.0 # in seconds # CARLA world and scenario handlers world = None manager = None def __init__(self, args): """ Setup CARLA client and world Setup ScenarioManager """ # First of all, we need to create the client that will send the requests # to the simulator. Here we'll assume the simulator is accepting # requests in the localhost at port 2000. client = carla.Client(args.host, int(args.port)) client.set_timeout(self.client_timeout) # Once we have a client we can retrieve the world that is currently # running. self.world = client.get_world() # Wait for the world to be ready self.world.wait_for_tick(self.wait_for_world) # Create scenario manager self.manager = ScenarioManager(self.world, args.debug) def __del__(self): """ Cleanup and delete actors, ScenarioManager and CARLA world """ self.cleanup(True) if self.manager is not None: del self.manager if self.world is not None: del self.world @staticmethod def get_scenario_class_or_fail(scenario): """ Get scenario class by scenario name If scenario is not supported or not found, exit script """ for scenarios in SCENARIOS.values(): if scenario in scenarios: if scenario in globals(): return globals()[scenario] print("Scenario '{}' not supported ... Exiting".format(scenario)) sys.exit(-1) def cleanup(self, ego=False): """ Remove and destroy all actors """ # We need enumerate here, otherwise the actors are not properly removed for i, _ in enumerate(self.actors): if self.actors[i] is not None: self.actors[i].destroy() self.actors[i] = None self.actors = [] if ego and self.ego_vehicle is not None: self.ego_vehicle.destroy() self.ego_vehicle = None def setup_vehicle(self, model, spawn_point, hero=False): """ Function to setup the most relevant vehicle parameters, incl. spawn point and vehicle model. """ blueprint_library = self.world.get_blueprint_library() # Get vehicle by model blueprint = random.choice(blueprint_library.filter(model)) if hero: blueprint.set_attribute('role_name', 'hero') else: blueprint.set_attribute('role_name', 'scenario') vehicle = self.world.try_spawn_actor(blueprint, spawn_point) if vehicle is None: raise Exception( "Error: Unable to spawn vehicle {} at {}".format(model, spawn_point)) else: # Let's deactivate the autopilot of the vehicle vehicle.set_autopilot(False) return vehicle def prepare_actors(self, config): """ Spawn or update all scenario actors according to their parameters provided in config """ # If ego_vehicle already exists, just update location # Otherwise spawn ego vehicle if self.ego_vehicle is None: self.ego_vehicle = self.setup_vehicle(config.ego_vehicle.model, config.ego_vehicle.transform, True) else: self.ego_vehicle.set_transform(config.ego_vehicle.transform) # spawn all other actors for actor in config.other_actors: new_actor = self.setup_vehicle(actor.model, actor.transform) self.actors.append(new_actor) def analyze_scenario(self, args, config): """ Provide feedback about success/failure of a scenario """ current_time = str(datetime.now().strftime('%Y-%m-%d-%H-%M-%S')) junit_filename = None if args.junit: junit_filename = config.name + current_time + ".xml" filename = None if args.file: filename = config.name + current_time + ".txt" if not self.manager.analyze_scenario(args.output, filename, junit_filename): print("Success!") else: print("Failure!") def run(self, args): """ Run all scenarios according to provided commandline args """ # Setup and run the scenarios for repetition times for _ in range(int(args.repetitions)): # Load the scenario configurations provided in the config file scenario_configurations = None if args.scenario.startswith("group:"): scenario_configurations = parse_scenario_configuration(args.scenario, args.scenario) else: scenario_config_file = find_scenario_config(args.scenario) if scenario_config_file is None: print("Configuration for scenario {} cannot be found!".format(args.scenario)) continue scenario_configurations = parse_scenario_configuration(scenario_config_file, args.scenario) # Execute each configuration for config in scenario_configurations: # Prepare scenario print("Preparing scenario: " + config.name) scenario_class = ScenarioRunner.get_scenario_class_or_fail(config.type) try: self.prepare_actors(config) scenario = scenario_class(self.world, self.ego_vehicle, self.actors, config.town, args.randomize, args.debug) except Exception as exception: print("The scenario cannot be loaded") traceback.print_exc() print(exception) self.cleanup() continue # Load scenario and run it self.manager.load_scenario(scenario) self.manager.run_scenario() # Provide outputs if required self.analyze_scenario(args, config) # Stop scenario and cleanup self.manager.stop_scenario() del scenario self.cleanup() print("No more scenarios .... Exiting")