├── README.md ├── configs └── config_ds_1.yaml ├── corpus └── corpus.py ├── data └── ds_1.json ├── main.py ├── mutation ├── genetic_algorithm.py ├── local_genetic_algorithm.py ├── restart.py └── tools.py └── simulation ├── dreamview.py ├── ego_check.py ├── liability.py ├── run_parse.py ├── simulator.py └── utils.py /README.md: -------------------------------------------------------------------------------- 1 | # AV-Fuzzer 2 | Reimplementation of AV-FUZZER: Finding Safety Violations in Autonomous Driving Systems (2020 ISSRE [Reference](https://github.com/cclinus/AV-Fuzzer)) 3 | 4 | ## Requirements 5 | ``` 6 | apollo 6.0 7 | lgsvl 2021.3 8 | apollo_map: see maps/apollo_map, copy maps/apollo_map/SanFrancisco to /apollo/modules/map/data/SanFrancisco 9 | lgsvl_map: SanFrancisco_correct link:https://wise.svlsimulator.com/maps/profile/12da60a7-2fc9-474d-a62a-5cc08cb97fe8 10 | ``` 11 | 12 | ## LGSVL Config 13 | ``` 14 | Step1: Add map SanFrancisco_correct from Store to Library. Map link:https://wise.svlsimulator.com/maps/profile/12da60a7-2fc9-474d-a62a-5cc08cb97fe8 15 | 16 | Step2: Add an API Only simulation. 17 | ``` 18 | 19 | ## Steps: 20 | ``` 21 | Step1: Define your scenario config, like configs/config_ds_1.yaml 22 | 23 | Step2: python main.py --config [your defined config.yaml] 24 | ``` 25 | -------------------------------------------------------------------------------- /configs/config_ds_1.yaml: -------------------------------------------------------------------------------- 1 | scenario_env_json: data/ds_1.json 2 | output_path: outputs/ds_1_v2 # absolute path 3 | lgsvl_map: SanFrancisco_correct 4 | apollo_map: SanFrancisco 5 | 6 | pop_size: 4 7 | npc_size: 2 8 | time_size: 10 9 | bounds: [[0, 30], [0, 3]] 10 | total_sim_time: 30 11 | p_mutation: 0.4 12 | p_crossover: 0.4 13 | max_gen: 100 14 | selection: roulette 15 | 16 | # apollo reltaed path config 17 | default_record_folder: /home/cmf/apollo/apollo6.0/apollo/data/bag # False means ignoring records -------------------------------------------------------------------------------- /corpus/corpus.py: -------------------------------------------------------------------------------- 1 | class CorpusElement(object): 2 | """Class representing a single element of a corpus.""" 3 | 4 | def __init__(self, scenario_id, scenario, fitness): 5 | 6 | self.scenario = scenario # input data 7 | self.fitness = fitness # simulator output - fitness score or others 8 | self.parent = None 9 | self.scenario_id = scenario_id 10 | 11 | def set_parent(self, parent): 12 | self.parent = parent 13 | 14 | def oldest_ancestor(self): 15 | """Returns the least recently created ancestor of this corpus item.""" 16 | current_element = self 17 | generations = 0 18 | while current_element.parent is not None: 19 | current_element = current_element.parent 20 | generations += 1 21 | return current_element, generations -------------------------------------------------------------------------------- /data/ds_1.json: -------------------------------------------------------------------------------- 1 | {"agents": { 2 | "ego": { 3 | "position": { 4 | "x": -0.6, 5 | "y": 10.2, 6 | "z": 0.5 7 | }, 8 | "destination": { 9 | "method": "forward_right", 10 | "value": { 11 | "v1": 225, 12 | "v2": -1, 13 | "v3": 0 14 | } 15 | } 16 | }, 17 | "npcs": [ 18 | { 19 | "type": "Sedan", 20 | "goal": "mutated", 21 | "position": { 22 | "x": 11.7, 23 | "y": 10.2, 24 | "z": -11.2 25 | } 26 | }, 27 | { 28 | "type": "SUV", 29 | "goal": "mutated", 30 | "position": { 31 | "x": 10.1, 32 | "y": 10.2, 33 | "z": -4.4 34 | } 35 | } 36 | ]}, 37 | "lines" : { 38 | "yellow_lines" : [ 39 | [[10.0, -12.6], [-164.2, 154.0]] 40 | ], 41 | "cross_lines" : [ 42 | [[12.5, -9.3], [-161.3, 156.7]] 43 | ], 44 | "edge_lines" : [ 45 | [[15.5, -6.7], [-158.6, 159.7]] 46 | ] 47 | } 48 | } -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import yaml 4 | import argparse 5 | 6 | from loguru import logger 7 | from datetime import datetime 8 | from mutation.genetic_algorithm import GeneticMutator 9 | from simulation.run_parse import Runner 10 | 11 | level = "INFO" 12 | logger.configure(handlers=[{"sink": sys.stderr, "level": level}]) # TODO: fix file output 13 | 14 | class Fuzzer(object): 15 | 16 | def __init__(self, cfgs): 17 | now = datetime.now() 18 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 19 | 20 | self.cfgs = cfgs 21 | cfgs['output_path'] = cfgs['output_path'] + '-at-' + date_time 22 | self.output_path = cfgs['output_path'] 23 | self.scenario_name = os.path.basename(cfgs['scenario_env_json']).split('.')[0] 24 | 25 | if not os.path.exists(self.output_path): 26 | os.makedirs(self.output_path) 27 | log_file = os.path.join(self.output_path, 'logs/system.log') 28 | if os.path.exists(log_file): 29 | os.remove(log_file) 30 | logger.add(log_file, level=level) 31 | self.record_cfgs() 32 | 33 | self.runner = Runner(cfgs['scenario_env_json'], 34 | self.output_path, 35 | self.cfgs['total_sim_time'], 36 | self.cfgs['default_record_folder'], 37 | self.cfgs['lgsvl_map'], 38 | self.cfgs['apollo_map']) 39 | self.mutation_runner = GeneticMutator(self.runner, self.cfgs['selection'], self.output_path, self.scenario_name, cfgs['bounds'], cfgs['p_mutation'], cfgs['p_crossover'], cfgs['pop_size'], cfgs['npc_size'], cfgs['time_size'], cfgs['max_gen']) 40 | self.mutation_runner.init_pop() 41 | logger.info('Initilized Genetic Mutator.') 42 | 43 | def loop(self): 44 | self.mutation_runner.process() 45 | 46 | def record_cfgs(self): 47 | logger.info('Record fuzzer configs:') 48 | for k, v in self.cfgs.items(): 49 | logger.info(str(k) + ' : ' + str(v)) 50 | 51 | if __name__ == '__main__': 52 | parser = argparse.ArgumentParser(description='Apollo AV-Fuzzer Testing.') 53 | parser.add_argument('--config', type=str, help='Test config yaml file.', default='configs/config_ds_1.yaml') 54 | args = parser.parse_args() 55 | 56 | yaml_file = args.config 57 | with open(yaml_file, 'r') as f: 58 | params = yaml.safe_load(f) 59 | 60 | fuzzer = Fuzzer(params) 61 | fuzzer.loop() -------------------------------------------------------------------------------- /mutation/genetic_algorithm.py: -------------------------------------------------------------------------------- 1 | import os 2 | import copy 3 | import random 4 | import pickle 5 | import shutil 6 | 7 | from datetime import datetime 8 | from loguru import logger 9 | 10 | from corpus.corpus import CorpusElement 11 | from mutation.local_genetic_algorithm import LocalGeneticMutator 12 | from mutation import restart 13 | 14 | class GeneticMutator(object): 15 | def __init__(self, runner, selection, output_path, scenario_name, bounds, pm, pc, pop_size, NPC_size, time_size, max_gen): 16 | self.pop = [] 17 | self.bounds = bounds # The value ranges of the inner most elements 18 | self.pm = pm 19 | self.pc = pc 20 | self.pop_size = pop_size # Number of scenarios in the population 21 | self.NPC_size = NPC_size # Number of NPC in each scenario 22 | self.time_size = time_size # Number of time slides in each NPC 23 | self.max_gen = max_gen 24 | self.bests = [0] * max_gen 25 | self.bestIndex = 0 26 | self.g_best = None 27 | self.touched_chs = [] # Record which chromosomes have been touched in each generation 28 | 29 | self.minLisGen = 2 # Min gen to start LIS 30 | self.numOfGenInLis = 5 # Number of gens in LIS 31 | self.hasRestarted = False 32 | self.lastRestartGen = 0 33 | self.bestYAfterRestart = 0 34 | 35 | self.runner = runner 36 | self.selection = selection 37 | self.scenario_name = scenario_name 38 | self.output_path = output_path 39 | self.ga_checkpoints_path = os.path.join(self.output_path, 'logs/checkpoints_ga') 40 | 41 | if os.path.exists(self.ga_checkpoints_path): 42 | shutil.rmtree(self.ga_checkpoints_path) 43 | os.makedirs(self.ga_checkpoints_path) 44 | 45 | # TODO: add inner log 46 | self.ga_log = os.path.join(self.output_path, 'logs/ga.log') 47 | if os.path.exists(self.ga_log): 48 | os.remove(self.ga_log) 49 | 50 | self.progress_log = os.path.join(self.output_path, 'logs/progress.log') 51 | if os.path.exists(self.progress_log): 52 | os.remove(self.progress_log) 53 | 54 | def take_checkpoint(self, obj, ck_name): 55 | ck_file = os.path.join(self.ga_checkpoints_path, ck_name) 56 | with open(ck_file, 'wb') as ck_f: 57 | pickle.dump(obj, ck_f) 58 | 59 | def cross(self): 60 | # Implementation of random crossover 61 | 62 | for i in range(int(self.pop_size / 2.0)): 63 | # Check crossover probability 64 | if self.pc > random.random(): 65 | # randomly select 2 chromosomes(scenarios) in pops 66 | i = 0 67 | j = 0 68 | while i == j: 69 | i = random.randint(0, self.pop_size-1) 70 | j = random.randint(0, self.pop_size-1) 71 | pop_i = self.pop[i] 72 | pop_j = self.pop[j] 73 | 74 | # Record which chromosomes have been touched 75 | self.touched_chs.append(i) 76 | self.touched_chs.append(j) 77 | 78 | # Every time we only switch one NPC between scenarios 79 | # select cross index 80 | swap_index = random.randint(0, self.NPC_size - 1) 81 | 82 | temp = copy.deepcopy(pop_j.scenario[swap_index]) 83 | pop_j.scenario[swap_index] = copy.deepcopy(pop_i.scenario[swap_index]) 84 | pop_i.scenario[swap_index] = temp 85 | # cross: generate new elements 86 | 87 | def mutation(self, ga_iter): 88 | 89 | i = 0 90 | while i < len(self.pop) : 91 | eachChs = self.pop[i] 92 | 93 | if self.pm >= random.random(): 94 | 95 | # select mutation index 96 | npc_index = random.randint(0, self.NPC_size - 1) 97 | time_index = random.randint(0, self.time_size - 1) 98 | 99 | # Record which chromosomes have been touched 100 | self.touched_chs.append(i) 101 | actionIndex = random.randint(0, 1) 102 | 103 | if actionIndex == 0: 104 | # Change Speed 105 | eachChs.scenario[npc_index][time_index][0] = random.uniform(self.bounds[0][0], self.bounds[0][1]) 106 | elif actionIndex == 1: 107 | # Change direction 108 | eachChs.scenario[npc_index][time_index][1] = random.randrange(self.bounds[1][0], self.bounds[1][1]) 109 | 110 | i = i + 1 111 | # Only run simulation for the chromosomes that are touched in this generation 112 | self.touched_chs = set(self.touched_chs) 113 | logger.info('Generate ' + str(len(self.touched_chs)) + ' mutated scenarios') 114 | 115 | for i in self.touched_chs: 116 | eachChs = self.pop[i] 117 | before_fitness = eachChs.fitness 118 | # TODO: fixed 119 | # 1. run simulator for each modified elements 120 | fitness, scenario_id = self.runner.run(eachChs.scenario) 121 | # 2. creat new elements or update fitness_score and coverage feat 122 | eachChs.fitness = fitness 123 | eachChs.scenario_id = scenario_id 124 | after_fitness = eachChs.fitness 125 | 126 | with open(self.ga_log, 'a') as f: 127 | f.write('global_' + str(ga_iter)) 128 | f.write(',') 129 | f.write(scenario_id) 130 | f.write(',') 131 | f.write('before run:' + str(before_fitness)) 132 | f.write(',') 133 | f.write('after run:' + str(after_fitness)) 134 | f.write('\n') 135 | 136 | def select_roulette(self): 137 | 138 | sum_f = 0 139 | for i in range(0, self.pop_size): 140 | if self.pop[i].fitness == 0: 141 | self.pop[i].fitness = 0.001 142 | 143 | ############################################################ 144 | min_fitness = self.pop[0].fitness 145 | for k in range(0, self.pop_size): 146 | if self.pop[k].fitness < min_fitness: 147 | min_fitness = self.pop[k].fitness 148 | if min_fitness < 0: 149 | for l in range(0, self.pop_size): 150 | self.pop[l].fitness = self.pop[l].fitness + (-1) * min_fitness 151 | 152 | # roulette 153 | for i in range(0, self.pop_size): 154 | sum_f += self.pop[i].fitness 155 | p = [0] * self.pop_size 156 | for i in range(0, self.pop_size): 157 | if sum_f == 0: 158 | sum_f = 1 159 | p[i] = self.pop[i].fitness / sum_f 160 | q = [0] * self.pop_size 161 | q[0] = 0 162 | for i in range(0, self.pop_size): 163 | s = 0 164 | for j in range(0, i+1): 165 | s += p[j] 166 | q[i] = s 167 | 168 | # start roulette 169 | v = [] 170 | for i in range(0, self.pop_size): 171 | r = random.random() 172 | if r < q[0]: 173 | v.append(copy.deepcopy(self.pop[0])) 174 | for j in range(1, self.pop_size): 175 | if q[j - 1] < r <= q[j]: 176 | v.append(copy.deepcopy(self.pop[j])) 177 | self.pop = copy.deepcopy(v) 178 | 179 | def select_top2(self): 180 | maxFitness = 0 181 | v = [] 182 | for i in range(0, self.pop_size): 183 | if self.pop[i].fitness > maxFitness: 184 | maxFitness = self.pop[i].fitness 185 | 186 | for i in range(0, self.pop_size): 187 | if self.pop[i].fitness == maxFitness: 188 | for j in range(int(self.pop_size / 2.0)): 189 | v.append(copy.deepcopy(self.pop[i])) 190 | break 191 | 192 | max2Fitness = 0 193 | for i in range(0, self.pop_size): 194 | if self.pop[i].fitness > max2Fitness and self.pop[i].fitness != maxFitness: 195 | max2Fitness = self.pop[i].fitness 196 | 197 | for i in range(0, self.pop_size): 198 | if self.pop[i].fitness == max2Fitness: 199 | for j in range(int(self.pop_size / 2.0)): 200 | v.append(copy.deepcopy(self.pop[i])) 201 | break 202 | 203 | self.pop = copy.deepcopy(v) 204 | 205 | def find_best(self): 206 | logger.debug(self.pop) 207 | best = copy.deepcopy(self.pop[0]) # element object 208 | bestIndex = 0 209 | for i in range(self.pop_size): 210 | if best.fitness < self.pop[i].fitness: 211 | best = copy.deepcopy(self.pop[i]) 212 | bestIndex = i 213 | return best, bestIndex 214 | 215 | def init_pop(self): 216 | for i in range(self.pop_size): 217 | # 1. init scenario data (data) 218 | scenario_data = [[[] for _ in range(self.time_size)] for _ in range(self.NPC_size)] 219 | 220 | for n_s in range(self.NPC_size): 221 | for t_s in range(self.time_size): 222 | v = random.uniform(self.bounds[0][0], self.bounds[0][1]) # velocity 223 | a = random.randrange(self.bounds[1][0], self.bounds[1][1]) # action 224 | scenario_data[n_s][t_s].append(v) 225 | scenario_data[n_s][t_s].append(a) 226 | 227 | # 2. run simulator -> get outputs 228 | fitness_score, scenario_id = self.runner.run(scenario_data) 229 | # 3. generate new elements 230 | new_element = CorpusElement(scenario_id, scenario_data, fitness_score) 231 | self.pop.append(new_element) 232 | with open(self.ga_log, 'a') as f: 233 | f.write('init_' + str(i)) 234 | f.write(',') 235 | f.write(scenario_id) 236 | f.write('\n') 237 | 238 | def process(self): 239 | 240 | best, bestIndex = self.find_best() 241 | self.g_best = copy.deepcopy(best) 242 | 243 | with open(self.progress_log, 'a') as f: 244 | f.write('name' + " " + "best_fitness" + " " + "global_best_fitness" + " " + "similarity" + " " + "datatime" + "\n") 245 | 246 | now = datetime.now() 247 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 248 | with open(self.progress_log, 'a') as f: 249 | f.write('initialization' + " " + str(best.fitness) + " " + str(self.g_best.fitness) + " " + "0000" + " " + date_time + "\n") 250 | 251 | 252 | # Start evolution 253 | for i in range(self.max_gen): # i th generation. 254 | # util.print_debug(" \n\n*** " + str(i) + "th generation ***") 255 | logger.info(" *** " + str(i) + "th generation *** ") 256 | 257 | # Make sure we clear touched_chs history book every gen 258 | # TODO: this process has bug 259 | self.touched_chs = [] 260 | self.cross() 261 | self.mutation(i) 262 | if self.selection == 'top': 263 | self.select_top2() 264 | elif self.selection == 'roulette': 265 | self.select_roulette() 266 | else: 267 | raise RuntimeError('Selection methods require: top or roulette.') 268 | 269 | best, bestIndex = self.find_best() # Find the scenario with the best fitness score in current generation 270 | self.bests[i] = best # Record the scenario with the best fitness score in i th generation 271 | 272 | ########### Update noprogressCounter ######### 273 | noprogress = False 274 | ave = 0 275 | if i >= self.lastRestartGen + 5: 276 | for j in range(i - 5, i): 277 | ave += self.bests[j].fitness 278 | ave /= 5 279 | if ave >= best.fitness: 280 | self.lastRestarGen = i 281 | noprogress = True 282 | 283 | if self.g_best.fitness < best.fitness: # Record the best fitness score across all generations 284 | self.g_best = copy.deepcopy(best) 285 | 286 | N_generation = self.pop 287 | N_b = self.g_best # Record the scenario with the best score over all generations 288 | 289 | # Update the checkpoint of the best scenario so far 290 | self.take_checkpoint(N_b, 'best_scenario.obj') 291 | 292 | # Checkpoint this generation 293 | self.take_checkpoint(N_generation, 'last_gen.obj') 294 | 295 | # Checkpoint every generation 296 | now = datetime.now() 297 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 298 | self.take_checkpoint(N_generation, 'generation-' + str(i) + '-at-' + date_time) 299 | 300 | #################### Start the Restart Process ################### 301 | if noprogress: 302 | logger.info(" ### Restart Based on Generation: " + str(i) + " ### ") 303 | oldCkName = self.ga_checkpoints_path 304 | newPop = restart.generate_restart_scenarios(self.runner, self.ga_log, i, oldCkName, 1000, self.bounds) 305 | self.pop = copy.deepcopy(newPop) 306 | self.hasRestarted = True 307 | best, self.bestIndex = self.find_best() 308 | self.bestYAfterRestart = best.fitness 309 | self.lastRestartGen = i 310 | # Log fitness etc 311 | with open(self.progress_log, 'a') as f: 312 | f.write('global_' + str(i) + '_restart' + " " + str(best.fitness) + " " + str(self.g_best.fitness) + " " + str(simiSum/float(self.pop_size)) + " " + date_time + "\n") 313 | 314 | #################### End the Restart Process ################### 315 | 316 | if os.path.exists(self.ga_checkpoints_path) == True: 317 | prePopPool = restart.get_all_checkpoints(self.ga_checkpoints_path) 318 | simiSum = 0 319 | for eachChs in self.pop: 320 | eachSimilarity = restart.get_similarity_scenario_vs_pre_pop(eachChs, prePopPool) 321 | simiSum += eachSimilarity 322 | logger.debug(" ==== Similarity compared with all prior generations: " + str(simiSum/float(self.pop_size))) 323 | 324 | # Log fitness etc 325 | with open(self.progress_log, 'a') as f: 326 | f.write('global_' + str(i) + " " + str(best.fitness) + " " + str(self.g_best.fitness) + " " + str(simiSum/float(self.pop_size)) + " " + date_time + "\n") 327 | 328 | if best.fitness > self.bestYAfterRestart: 329 | self.bestYAfterRestart = best.fitness 330 | if i > (self.lastRestartGen + self.minLisGen): # Only allow one level of recursion 331 | ################## Start LIS ################# 332 | logger.debug(" === Start of Local Iterative Search === ") 333 | # Increase mutation rate a little bit to jump out of local maxima 334 | local_output_path = os.path.join(self.output_path, 'local_ga', 'local_' + str(i)) 335 | lis = LocalGeneticMutator(self.runner, self.selection, local_output_path, i, self.ga_log, self.progress_log, self.scenario_name, self.bounds, self.pm * 1.5, self.pc, self.pop_size, self.NPC_size, self.time_size, self.numOfGenInLis) 336 | lis.setLisPop(self.g_best) 337 | lisBestChs = lis.process(i) 338 | logger.debug(" --- Best fitness in LIS: " + str(lisBestChs.fitness)) 339 | if lisBestChs.fitness > self.g_best.fitness: 340 | # Let's replace this 341 | self.pop[bestIndex] = copy.deepcopy(lisBestChs) 342 | logger.info(" --- Find better scenario in LIS: LIS->" + str(lisBestChs.fitness) + ", original->" + str(self.g_best.fitness)) 343 | else: 344 | logger.debug(" --- LIS does not find any better scenarios") 345 | logger.info(' === End of Local Iterative Search === ') 346 | 347 | return self.g_best 348 | -------------------------------------------------------------------------------- /mutation/local_genetic_algorithm.py: -------------------------------------------------------------------------------- 1 | import os 2 | import copy 3 | import random 4 | import pickle 5 | import shutil 6 | 7 | from datetime import datetime 8 | from loguru import logger 9 | from mutation import restart 10 | 11 | class LocalGeneticMutator(object): 12 | def __init__(self, runner, selection, output_path, global_iter, ga_logger, progress_logger, scenario_name, bounds, pm, pc, pop_size, NPC_size, time_size, max_gen): 13 | self.pop = [] 14 | self.bounds = bounds # The value ranges of the inner most elements 15 | self.pm = pm 16 | self.pc = pc 17 | self.pop_size = pop_size # Number of scenarios in the population 18 | self.NPC_size = NPC_size # Number of NPC in each scenario 19 | self.time_size = time_size # Number of time slides in each NPC 20 | self.max_gen = max_gen 21 | self.bests = [0] * max_gen 22 | self.bestIndex = 0 23 | self.g_best = None 24 | self.touched_chs = [] # Record which chromosomes have been touched in each generation 25 | 26 | self.minLisGen = 2 # Min gen to start LIS 27 | self.numOfGenInLis = 5 # Number of gens in LIS 28 | self.hasRestarted = False 29 | self.lastRestartGen = 0 30 | self.bestYAfterRestart = 0 31 | 32 | self.runner = runner 33 | self.selection = selection 34 | self.scenario_name = scenario_name 35 | self.output_path = output_path 36 | self.ga_checkpoints_path = os.path.join(self.output_path, 'logs/checkpoints_ga') 37 | 38 | if os.path.exists(self.ga_checkpoints_path): 39 | shutil.rmtree(self.ga_checkpoints_path) 40 | os.makedirs(self.ga_checkpoints_path) 41 | 42 | # TODO: add inner log 43 | self.ga_log = ga_logger 44 | self.progress_log = progress_logger 45 | self.global_iter = global_iter 46 | 47 | 48 | def take_checkpoint(self, obj, ck_name): 49 | ck_file = os.path.join(self.ga_checkpoints_path, ck_name) 50 | with open(ck_file, 'wb') as ck_f: 51 | pickle.dump(obj, ck_f) 52 | 53 | def setLisPop(self, singleChs): 54 | for i in range(self.pop_size): 55 | self.pop.append(copy.deepcopy(singleChs)) 56 | 57 | # Add some entropy 58 | tempPm = self.pm 59 | self.pm = 1 60 | self.mutation(0) 61 | self.pm = tempPm 62 | self.g_best, bestIndex = self.find_best() 63 | 64 | def cross(self): 65 | # Implementation of random crossover 66 | 67 | for i in range(int(self.pop_size / 2.0)): 68 | # Check crossover probability 69 | if self.pc > random.random(): 70 | # randomly select 2 chromosomes(scenarios) in pops 71 | i = 0 72 | j = 0 73 | while i == j: 74 | i = random.randint(0, self.pop_size-1) 75 | j = random.randint(0, self.pop_size-1) 76 | pop_i = self.pop[i] 77 | pop_j = self.pop[j] 78 | 79 | # Record which chromosomes have been touched 80 | self.touched_chs.append(i) 81 | self.touched_chs.append(j) 82 | 83 | # Every time we only switch one NPC between scenarios 84 | # select cross index 85 | swap_index = random.randint(0, self.NPC_size - 1) 86 | 87 | temp = copy.deepcopy(pop_j.scenario[swap_index]) 88 | pop_j.scenario[swap_index] = copy.deepcopy(pop_i.scenario[swap_index]) 89 | pop_i.scenario[swap_index] = temp 90 | # cross: generate new elements 91 | 92 | def mutation(self, ga_iter): 93 | 94 | i = 0 95 | while i < len(self.pop) : 96 | eachChs = self.pop[i] 97 | 98 | if self.pm >= random.random(): 99 | 100 | beforeMutation = copy.deepcopy(eachChs) 101 | # select mutation index 102 | npc_index = random.randint(0, self.NPC_size - 1) 103 | time_index = random.randint(0, self.time_size - 1) 104 | 105 | # Record which chromosomes have been touched 106 | self.touched_chs.append(i) 107 | actionIndex = random.randint(0, 1) 108 | 109 | if actionIndex == 0: 110 | # Change Speed 111 | eachChs.scenario[npc_index][time_index][0] = random.uniform(self.bounds[0][0], self.bounds[0][1]) 112 | elif actionIndex == 1: 113 | # Change direction 114 | eachChs.scenario[npc_index][time_index][1] = random.randrange(self.bounds[1][0], self.bounds[1][1]) 115 | i += 1 116 | 117 | logger.info('Generate ' + str(len(self.touched_chs)) + ' mutated scenarios') 118 | # Only run simulation for the chromosomes that are touched in this generation 119 | self.touched_chs = set(self.touched_chs) 120 | for i in self.touched_chs: 121 | before_fitness = eachChs.fitness 122 | eachChs = self.pop[i] 123 | fitness, scenario_id = self.runner.run(eachChs.scenario) 124 | # 2. creat new elements or update fitness_score and coverage feat 125 | eachChs.fitness = fitness 126 | eachChs.scenario_id = scenario_id 127 | after_fitness = eachChs.fitness 128 | 129 | with open(self.ga_log, 'a') as f: 130 | f.write('global_' + str(self.global_iter) + '_local_' + str(ga_iter)) 131 | f.write(',') 132 | f.write(scenario_id) 133 | f.write(',') 134 | f.write('before run:' + str(before_fitness)) 135 | f.write(',') 136 | f.write('after run:' + str(after_fitness)) 137 | f.write('\n') 138 | 139 | def select_roulette(self): 140 | 141 | sum_f = 0 142 | for i in range(0, self.pop_size): 143 | if self.pop[i].fitness == 0: 144 | self.pop[i].fitness = 0.001 145 | 146 | ############################################################ 147 | min_fitness = self.pop[0].fitness 148 | for k in range(0, self.pop_size): 149 | if self.pop[k].fitness < min_fitness: 150 | min_fitness = self.pop[k].fitness 151 | if min_fitness < 0: 152 | for l in range(0, self.pop_size): 153 | self.pop[l].fitness = self.pop[l].fitness + (-1) * min_fitness 154 | 155 | # roulette 156 | for i in range(0, self.pop_size): 157 | sum_f += self.pop[i].fitness 158 | p = [0] * self.pop_size 159 | for i in range(0, self.pop_size): 160 | if sum_f == 0: 161 | sum_f = 1 162 | p[i] = self.pop[i].fitness / sum_f 163 | q = [0] * self.pop_size 164 | q[0] = 0 165 | for i in range(0, self.pop_size): 166 | s = 0 167 | for j in range(0, i+1): 168 | s += p[j] 169 | q[i] = s 170 | 171 | # start roulette 172 | v = [] 173 | for i in range(0, self.pop_size): 174 | r = random.random() 175 | if r < q[0]: 176 | v.append(copy.deepcopy(self.pop[0])) 177 | for j in range(1, self.pop_size): 178 | if q[j - 1] < r <= q[j]: 179 | v.append(copy.deepcopy(self.pop[j])) 180 | self.pop = copy.deepcopy(v) 181 | 182 | def select_top2(self): 183 | maxFitness = 0 184 | v = [] 185 | for i in range(0, self.pop_size): 186 | if self.pop[i].fitness > maxFitness: 187 | maxFitness = self.pop[i].fitness 188 | 189 | for i in range(0, self.pop_size): 190 | if self.pop[i].fitness == maxFitness: 191 | for j in range(int(self.pop_size / 2.0)): 192 | v.append(copy.deepcopy(self.pop[i])) 193 | break 194 | 195 | max2Fitness = 0 196 | for i in range(0, self.pop_size): 197 | if self.pop[i].fitness > max2Fitness and self.pop[i].fitness != maxFitness: 198 | max2Fitness = self.pop[i].fitness 199 | 200 | for i in range(0, self.pop_size): 201 | if self.pop[i].fitness == max2Fitness: 202 | for j in range(int(self.pop_size / 2.0)): 203 | v.append(copy.deepcopy(self.pop[i])) 204 | break 205 | 206 | self.pop = copy.deepcopy(v) 207 | 208 | def find_best(self): 209 | best = copy.deepcopy(self.pop[0]) # element object 210 | bestIndex = 0 211 | for i in range(self.pop_size): 212 | if best.fitness < self.pop[i].fitness: 213 | best = copy.deepcopy(self.pop[i]) 214 | bestIndex = i 215 | return best, bestIndex 216 | 217 | def process(self, global_generation_id): 218 | 219 | best, bestIndex = self.find_best() 220 | self.g_best = copy.deepcopy(best) 221 | 222 | # Start evolution 223 | for i in range(self.max_gen): # i th generation. 224 | 225 | logger.info("Local Generation of " + str(global_generation_id) + ": * " + str(i) + "th generation *") 226 | 227 | # Make sure we clear touched_chs history book every gen 228 | self.touched_chs = [] 229 | self.cross() 230 | self.mutation(i) 231 | if self.selection == 'top': 232 | self.select_top2() 233 | elif self.selection == 'roulette': 234 | self.select_roulette() 235 | else: 236 | raise RuntimeError('Selection methods require: top or roulette.') 237 | 238 | best, bestIndex = self.find_best() # Find the scenario with the best fitness score in current generation 239 | self.bests[i] = best # Record the scenario with the best fitness score in i th generation 240 | 241 | if self.g_best.fitness < best.fitness: # Record the best fitness score across all generations 242 | self.g_best = copy.deepcopy(best) 243 | 244 | N_generation = self.pop 245 | N_b = self.g_best # Record the scenario with the best score over all generations 246 | 247 | # Update the checkpoint of the best scenario so far 248 | self.take_checkpoint(N_b, 'best_scenario.obj') 249 | 250 | # Checkpoint this generation 251 | self.take_checkpoint(N_generation, 'last_gen.obj') 252 | 253 | # Checkpoint every generation 254 | now = datetime.now() 255 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 256 | self.take_checkpoint(N_generation, 'generation-' + str(i) + '-at-' + date_time) 257 | 258 | if os.path.exists(self.ga_checkpoints_path) == True: 259 | prePopPool = restart.get_all_checkpoints(self.ga_checkpoints_path) 260 | simiSum = 0 261 | for eachChs in self.pop: 262 | eachSimilarity = restart.get_similarity_scenario_vs_pre_pop(eachChs, prePopPool) 263 | simiSum += eachSimilarity 264 | # util.print_debug(" ==== Similarity compared with all prior generations: " + str(simiSum/float(self.pop_size))) 265 | 266 | # Log fitness etc 267 | with open(self.progress_log, 'a') as f: 268 | f.write('global_' + str(self.global_iter) + '_local_' + str(i) + " " + str(best.fitness) + " " + str(self.g_best.fitness) + " " + str(simiSum/float(self.pop_size)) + " " + date_time + "\n") 269 | 270 | return self.g_best 271 | -------------------------------------------------------------------------------- /mutation/restart.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | import os 3 | import collections 4 | import pickle 5 | import random 6 | 7 | from corpus.corpus import CorpusElement 8 | from mutation import tools 9 | 10 | def get_all_checkpoints(ck_path): 11 | # ga_checkpoints_path 12 | only_files = os.listdir(ck_path) 13 | 14 | pre_pop_pool = [] 15 | 16 | for i in range(len(only_files)): 17 | with open(ck_path+'/'+only_files[i], "rb") as f: 18 | if "generation" not in only_files[i]: 19 | continue 20 | try: 21 | pre_pop = pickle.load(f) 22 | pre_pop_pool.append(pre_pop) 23 | except Exception: 24 | pass 25 | 26 | return pre_pop_pool 27 | 28 | def generate_restart_scenarios(runner, ga_logger, global_iter, ck_path, scenario_num, bounds): 29 | 30 | pre_pop_pool = get_all_checkpoints(ck_path) 31 | 32 | new_pop_candidate = [] 33 | new_scenario_list = [] 34 | pop_size = len(pre_pop_pool[0]) 35 | npc_size = len(pre_pop_pool[0][0].scenario) 36 | time_size = len(pre_pop_pool[0][0].scenario[0]) 37 | scenario_size = len(pre_pop_pool[0]) 38 | pop_pool_size = len(pre_pop_pool) 39 | scenario_dict = {} 40 | 41 | for i in range(scenario_num): 42 | # 1. init scenario data (data) 43 | scenario_data = [[[] for _ in range(time_size)] for _ in range(npc_size)] 44 | 45 | for n_s in range(npc_size): 46 | for t_s in range(time_size): 47 | v = random.uniform(bounds[0][0], bounds[0][1]) # velocity 48 | a = random.randrange(bounds[1][0], bounds[1][1]) # action 49 | scenario_data[n_s][t_s].append(v) 50 | scenario_data[n_s][t_s].append(a) 51 | 52 | new_pop_candidate.append(scenario_data) 53 | 54 | # Go through every scenario 55 | 56 | for i in range(scenario_num): 57 | similarity = 0 58 | for j in range(pop_pool_size): 59 | simi_pop = 0 60 | for k in range(scenario_size): 61 | # TODO 62 | scenario1 = new_pop_candidate[i] 63 | scenario2 = pre_pop_pool[j][k].scenario 64 | simi = tools.get_similarity_between_scenarios(scenario1, scenario2) 65 | simi_pop += simi 66 | 67 | simi_pop /= scenario_size + 0.0 68 | similarity += simi_pop 69 | similarity /= pop_pool_size + 0.0 70 | scenario_dict[i] = similarity 71 | 72 | sorted_x = sorted(scenario_dict.items(), key=lambda kv: kv[1], reverse=True) 73 | sorted_dict = collections.OrderedDict(sorted_x) 74 | 75 | index = sorted_dict.keys() 76 | 77 | j = 0 78 | 79 | for i in index: 80 | if j == pop_size: 81 | break 82 | # run pop 83 | fitness, scenario_id = runner.run(new_pop_candidate[i]) 84 | 85 | new_element = CorpusElement(scenario_id, new_pop_candidate[i], fitness) 86 | 87 | new_scenario_list.append(new_element) 88 | 89 | with open(ga_logger, 'a') as f: 90 | f.write('global_' + str(global_iter) + '_restart_' + str(j)) 91 | f.write(',') 92 | f.write(scenario_id) 93 | f.write('\n') 94 | 95 | j += 1 96 | 97 | return new_scenario_list 98 | 99 | def get_similarity_scenario_vs_pre_pop(scenario, pre_pop_pool): 100 | 101 | similarity = 0 102 | for i in pre_pop_pool: 103 | pop_similarity = 0 104 | for j in i: 105 | simi = tools.get_similarity_between_scenarios(j.scenario, scenario.scenario) 106 | pop_similarity += simi 107 | pop_similarity /= len(i) 108 | similarity += pop_similarity + 0.0 109 | similarity /= len(pre_pop_pool) + 0.0 110 | 111 | return similarity -------------------------------------------------------------------------------- /mutation/tools.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | def get_similarity_between_npcs(npc1, npc2): 4 | accumD = 0.0 5 | horD1 = 0.0 6 | horD2 = 0.0 7 | v1 = 0.0 8 | v2 = 0.0 9 | 10 | x = [] 11 | y = [] 12 | x1 = [] 13 | y1 = [] 14 | 15 | for i in range(len(npc1)): 16 | v1 += npc1[i][0] 17 | v2 += npc2[i][0] 18 | a1 = npc1[i][1] 19 | a2 = npc2[i][1] 20 | 21 | if a1 == 1: 22 | horD1 += -34.0 23 | elif a1 == 2: 24 | horD1 += 34.0 25 | if a2 == 1: 26 | horD2 += -34.0 27 | elif a2 == 2: 28 | horD2 += 34.0 29 | x.append(horD1) 30 | y.append(v1) 31 | x1.append(horD2) 32 | y1.append(v2) 33 | 34 | curED = math.sqrt(math.pow(v1 - v2, 2) + math.pow(horD1 - horD2, 2)) 35 | accumD += curED 36 | 37 | return accumD 38 | 39 | def get_similarity_between_scenarios(scenario1, scenario2): 40 | 41 | npc_size = len(scenario1) 42 | #time_size = len(scenario1[0]) 43 | 44 | scenario_sim_total = 0.0 45 | 46 | for i in range(npc_size): 47 | npc1 = scenario1[i] 48 | npc2 = scenario2[i] 49 | npc_simi = get_similarity_between_npcs(npc1, npc2) 50 | scenario_sim_total += npc_simi 51 | 52 | return scenario_sim_total / npc_size + 0.0 53 | 54 | ###################################################################################### 55 | 56 | 57 | -------------------------------------------------------------------------------- /simulation/dreamview.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2019-2020 LG Electronics, Inc. 3 | # 4 | # This software contains code licensed as described in LICENSE. 5 | # 6 | 7 | from websocket import create_connection 8 | from enum import Enum 9 | import json 10 | import lgsvl 11 | import math 12 | import logging 13 | import sys 14 | import os 15 | 16 | log = logging.getLogger(__name__) 17 | 18 | 19 | class CoordType(Enum): 20 | Unity = 1 21 | Northing = 2 22 | Latitude = 3 23 | 24 | 25 | class Connection: 26 | def __init__(self, simulator, ip=os.environ.get("LGSVL__AUTOPILOT_0_HOST", "localhost"), port="8888"): 27 | """ 28 | simulator: is an lgsvl.Simulator object 29 | ego_agent: an lgsvl.EgoVehicle object, this is intended to be used with a vehicle equipped with Apollo 5.0 30 | ip: address of the machine where the Apollo stack is running 31 | port: the port number for Dreamview 32 | """ 33 | self.url = "ws://" + ip + ":" + port + "/websocket" 34 | self.sim = simulator 35 | self.ego = None 36 | self.ws = create_connection(self.url) 37 | self.gps_offset = lgsvl.Vector() 38 | 39 | def set_ego(self, ego_agent): 40 | self.ego = ego_agent 41 | 42 | def set_destination(self, x_long_east, z_lat_north, y=0, coord_type=CoordType.Unity): 43 | """ 44 | This function can accept a variety of Coordinate systems 45 | 46 | If using Unity World Coordinate System: 47 | x_long_east = x 48 | z_lat_north = z 49 | y = y 50 | 51 | If using Latitude/Longitude: 52 | x_long_east = Longitude 53 | z_lat_north = Latitude 54 | 55 | If using Easting/Northing: 56 | x_long_east = Easting 57 | z_lat_north = Northing 58 | """ 59 | current_pos = self.ego.state.transform 60 | current_gps = self.sim.map_to_gps(current_pos) 61 | heading = math.radians(current_gps.orientation) 62 | 63 | # Start position should be the position of the GPS 64 | # Unity returns the position of the center of the vehicle so adjustment is required 65 | northing_adjustment = ( 66 | math.sin(heading) * self.gps_offset.z - math.cos(heading) * self.gps_offset.x 67 | ) 68 | easting_adjustment = ( 69 | math.cos(heading) * self.gps_offset.z + math.sin(heading) * self.gps_offset.x 70 | ) 71 | 72 | if coord_type == CoordType.Unity: 73 | transform = lgsvl.Transform( 74 | lgsvl.Vector(x_long_east, y, z_lat_north), lgsvl.Vector(0, 0, 0) 75 | ) 76 | gps = self.sim.map_to_gps(transform) 77 | dest_x = gps.easting 78 | dest_y = gps.northing 79 | 80 | elif coord_type == CoordType.Northing: 81 | dest_x = x_long_east 82 | dest_y = z_lat_north 83 | 84 | elif coord_type == CoordType.Latitude: 85 | transform = self.sim.map_from_gps(longitude=x_long_east, latitude=z_lat_north) 86 | gps = self.sim.map_to_gps(transform) 87 | dest_x = gps.easting 88 | dest_y = gps.northing 89 | 90 | else: 91 | log.error( 92 | "Please input a valid Coordinate Type: Unity position, Easting/Northing, or Longitude/Latitude" 93 | ) 94 | return 95 | 96 | self.ws.send( 97 | json.dumps( 98 | { 99 | "type": "SendRoutingRequest", 100 | "start": { 101 | "x": current_gps.easting + easting_adjustment, 102 | "y": current_gps.northing + northing_adjustment, 103 | "z": 0, 104 | "heading": heading, 105 | }, 106 | "end": {"x": dest_x, "y": dest_y, "z": 0}, 107 | "waypoint": "[]", 108 | } 109 | ) 110 | ) 111 | 112 | return 113 | 114 | def enable_module(self, module): 115 | """ 116 | module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview 117 | """ 118 | self.ws.send( 119 | json.dumps({"type": "HMIAction", "action": "START_MODULE", "value": module}) 120 | ) 121 | return 122 | 123 | def disable_module(self, module): 124 | """ 125 | module is the name of the Apollo 5.0 module as seen in the "Module Controller" tab of Dreamview 126 | """ 127 | self.ws.send( 128 | json.dumps({"type": "HMIAction", "action": "STOP_MODULE", "value": module}) 129 | ) 130 | return 131 | 132 | def set_hd_map(self, hd_map): 133 | """ 134 | Folders in /apollo/modules/map/data/ are the available HD maps 135 | Map options in Dreamview are the folder names with the following changes: 136 | - underscores (_) are replaced with spaces 137 | - the first letter of each word is capitalized 138 | 139 | hd_map parameter is the modified folder name. 140 | hd_map should match one of the options in the right-most drop down in the top-right corner of Dreamview. 141 | """ 142 | 143 | word_list = [] 144 | for s in hd_map.split('_'): 145 | word_list.append(s[0].upper() + s[1:]) 146 | 147 | mapped_map = ' '.join(word_list) 148 | 149 | self.ws.send( 150 | json.dumps({"type": "HMIAction", "action": "CHANGE_MAP", "value": mapped_map}) 151 | ) 152 | 153 | if not self.get_current_map() == mapped_map: 154 | folder_name = hd_map.replace(" ", "_") 155 | error_message = ( 156 | "HD Map {0} was not set. Verify the files exist in " 157 | "/apollo/modules/map/data/{1} and restart Dreamview -- Aborting..." 158 | ) 159 | log.error( 160 | error_message.format( 161 | mapped_map, folder_name 162 | ) 163 | ) 164 | sys.exit(1) 165 | return 166 | 167 | def set_vehicle(self, vehicle, gps_offset_x=0.0, gps_offset_y=0.0, gps_offset_z=-1.348): 168 | # Lincoln2017MKZ from LGSVL has a GPS offset of 1.348m behind the center of the vehicle, lgsvl.Vector(0.0, 0.0, -1.348) 169 | """ 170 | Folders in /apollo/modules/calibration/data/ are the available vehicle calibrations 171 | Vehicle options in Dreamview are the folder names with the following changes: 172 | - underscores (_) are replaced with spaces 173 | - the first letter of each word is capitalized 174 | 175 | vehicle parameter is the modified folder name. 176 | vehicle should match one of the options in the middle drop down in the top-right corner of Dreamview. 177 | """ 178 | 179 | word_list = [] 180 | for s in vehicle.split('_'): 181 | word_list.append(s[0].upper() + s[1:]) 182 | 183 | mapped_vehicle = ' '.join(word_list) 184 | 185 | self.ws.send( 186 | json.dumps( 187 | {"type": "HMIAction", "action": "CHANGE_VEHICLE", "value": mapped_vehicle} 188 | ) 189 | ) 190 | 191 | self.gps_offset = lgsvl.Vector(gps_offset_x, gps_offset_y, gps_offset_z) 192 | 193 | if not self.get_current_vehicle() == mapped_vehicle: 194 | folder_name = vehicle.replace(" ", "_") 195 | error_message = ( 196 | "Vehicle calibration {0} was not set. Verify the files exist in " 197 | "/apollo/modules/calibration/data/{1} and restart Dreamview -- Aborting..." 198 | ) 199 | log.error( 200 | error_message.format( 201 | mapped_vehicle, folder_name 202 | ) 203 | ) 204 | sys.exit(1) 205 | return 206 | 207 | def set_setup_mode(self, mode): 208 | """ 209 | mode is the name of the Apollo 5.0 mode as seen in the left-most drop down in the top-right corner of Dreamview 210 | """ 211 | self.ws.send( 212 | json.dumps({"type": "HMIAction", "action": "CHANGE_MODE", "value": mode}) 213 | ) 214 | return 215 | 216 | def get_module_status(self): 217 | """ 218 | Returns a dict where the key is the name of the module and value is a bool based on the module's current status 219 | """ 220 | self.reconnect() 221 | data = json.loads( 222 | self.ws.recv() 223 | ) # This first recv() call returns the SimControlStatus in the form '{"enabled":false,"type":"SimControlStatus"}' 224 | while data["type"] != "HMIStatus": 225 | data = json.loads(self.ws.recv()) 226 | 227 | # The second recv() call also contains other information: 228 | # the current map, vehicle, and mode: 229 | # data["data"]["currentMap"], data["data"]["currentVehicle"], data["data"]["currentMode"] 230 | # 231 | # the available maps, vehicles, and modes: 232 | # data["data"]["maps"], data["data"]["vehicles"], data["data"]["modes"] 233 | # 234 | # the status of monitors components: 235 | # data["data"]["monitoredComponents"] 236 | 237 | return data["data"]["modules"] 238 | 239 | def get_current_map(self): 240 | """ 241 | Returns the current HD Map loaded in Dreamview 242 | """ 243 | try: 244 | self.reconnect() 245 | except ConnectionRefusedError as e: 246 | log.error("Not able to get the current HD map loaded in Dreamview.") 247 | log.error("Original exception: " + str(e)) 248 | return None 249 | 250 | data = json.loads(self.ws.recv()) 251 | while data["type"] != "HMIStatus": 252 | data = json.loads(self.ws.recv()) 253 | return data["data"]["currentMap"] 254 | 255 | def get_current_vehicle(self): 256 | """ 257 | Returns the current Vehicle configuration loaded in Dreamview 258 | """ 259 | try: 260 | self.reconnect() 261 | except ConnectionRefusedError as e: 262 | log.error("Not able to get the current vehicle configuration loaded in Dreamview.") 263 | log.error("Original exception: " + str(e)) 264 | return None 265 | 266 | data = json.loads(self.ws.recv()) 267 | while data["type"] != "HMIStatus": 268 | data = json.loads(self.ws.recv()) 269 | return data["data"]["currentVehicle"] 270 | 271 | def reconnect(self): 272 | """ 273 | Closes the websocket connection and re-creates it so that data can be received again 274 | """ 275 | self.ws.close() 276 | self.ws = create_connection(self.url) 277 | return 278 | 279 | def disconnect(self): 280 | self.ws.close() 281 | return 282 | 283 | def enable_apollo(self, dest_x, dest_z, modules, coord_type=CoordType.Unity): 284 | """ 285 | Enables a list of modules and then sets the destination 286 | """ 287 | for mod in modules: 288 | log.info("Starting {} module...".format(mod)) 289 | self.enable_module(mod) 290 | 291 | self.set_destination(dest_x, dest_z, coord_type=coord_type) 292 | 293 | def disable_apollo(self): 294 | """ 295 | Disables all Apollo modules 296 | """ 297 | module_status = self.get_module_status() 298 | for module in module_status.keys(): 299 | self.disable_module(module) 300 | 301 | def check_module_status(self, modules): 302 | """ 303 | Checks if all modules in a provided list are enabled 304 | """ 305 | module_status = self.get_module_status() 306 | for module, status in module_status.items(): 307 | if not status and module in modules: 308 | log.warning( 309 | "Warning: Apollo module {} is not running!!!".format(module) 310 | ) 311 | 312 | def setup_apollo(self, dest_x, dest_z, modules, default_timeout=60.0, coord_type=CoordType.Unity): 313 | """ 314 | Starts a list of Apollo modules and sets the destination. Will wait for Control module to send a message before returning. 315 | Control sending a message indicates that all modules are working and Apollo is ready to continue. 316 | """ 317 | initial_state = self.ego.state 318 | mod_status = self.get_module_status() 319 | 320 | # If any module is not enabled, Control may still send a message even though Apollo is not ready 321 | if not all(mod_status[mod] for mod in modules): 322 | self.disable_apollo() 323 | 324 | self.enable_apollo(dest_x, dest_z, modules, coord_type=coord_type) 325 | self.ego.is_control_received = False 326 | 327 | def on_control_received(agent, kind, context): 328 | if kind == "checkControl": 329 | agent.is_control_received = True 330 | log.info("Control message received") 331 | 332 | self.ego.on_custom(on_control_received) 333 | 334 | try: 335 | timeout = float(os.environ.get("LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS", default_timeout)) 336 | except Exception: 337 | timeout = default_timeout 338 | log.warning("Invalid LGSVL__DREAMVIEW__CONTROL_MESSAGE_TIMEOUT_SECS, using default {0}s".format(default_timeout)) 339 | 340 | run_time = 2 341 | elapsed = 0 342 | while timeout <= 0.0 or float(elapsed) < timeout: 343 | self.sim.run(run_time) 344 | 345 | if self.ego.is_control_received: 346 | break 347 | 348 | if elapsed > 0 and elapsed % (run_time * 5) == 0: 349 | self.check_module_status(modules) 350 | log.info("{} seconds have passed but Ego hasn't received any control messages.".format(elapsed)) 351 | log.info("Please also check if your route has been set correctly in Dreamview.") 352 | 353 | elapsed += run_time 354 | else: 355 | log.error("No control message from Apollo within {} seconds. Aborting...".format(timeout)) 356 | self.disable_apollo() 357 | raise WaitApolloError() 358 | 359 | self.ego.state = initial_state 360 | 361 | 362 | class WaitApolloError(Exception): 363 | """ 364 | Raised when Apollo control message is not received in time 365 | """ 366 | 367 | pass 368 | -------------------------------------------------------------------------------- /simulation/ego_check.py: -------------------------------------------------------------------------------- 1 | import math 2 | 3 | from shapely.geometry import Polygon, LineString 4 | 5 | def right_rotation(coord, theta): 6 | """ 7 | theta : degree 8 | """ 9 | theta = math.radians(theta) 10 | x = coord[0] 11 | y = coord[1] 12 | x1 = x * math.cos(theta) - y * math.sin(theta) 13 | y1 = x * math.cos(theta) + y * math.cos(theta) 14 | return [x1, y1] 15 | 16 | def get_bbox(agent): 17 | agent_theta = agent.transform.rotation.y 18 | agent_bbox = agent.bounding_box # min max (x_min, y_min, z_min) (x_max, y_max, z_max) 19 | global_x = agent.transform.position.x 20 | global_z = agent.transform.position.z 21 | x_min = agent_bbox.min.x 22 | x_max = agent_bbox.max.x 23 | z_min = agent_bbox.min.z 24 | z_max = agent_bbox.max.z 25 | coords = [[x_min, z_min], [x_max, z_min], [x_max, z_max], [x_min, z_max]] 26 | for i in range(len(coords)): 27 | coord_i = coords[i] 28 | new_coord_i = right_rotation(coord_i, agent_theta) 29 | new_coord_i[0] += global_x 30 | new_coord_i[1] += global_z 31 | coords[i] = new_coord_i 32 | p1, p2, p3, p4 = coords[0], coords[1], coords[2], coords[3] 33 | agent_poly = Polygon((p1, p2, p3, p4)) 34 | if agent_poly.area <= 0: 35 | print(agent_poly.area) 36 | exit(-1) 37 | return agent_poly 38 | 39 | def get_distance_ego_npc(ego, npc): 40 | ego_bbox = get_bbox(ego) 41 | npc_bbox = get_bbox(npc) 42 | d = ego_bbox.distance(npc_bbox) 43 | return d 44 | 45 | def get_distance_ego_line(ego, line_start, line_end): 46 | """ 47 | lines: support many edges 48 | line_start: [x_start, z_start] 49 | line_end: [x_end, z_end] 50 | """ 51 | line = LineString([line_start, line_end]) 52 | ego_bbox = get_bbox(ego) 53 | d = ego_bbox.distance(line) 54 | return d 55 | 56 | def is_hit_line(ego, lane_line): 57 | """ 58 | lane_line: single line, each line contains many points 59 | """ 60 | pass 61 | 62 | -------------------------------------------------------------------------------- /simulation/liability.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | from loguru import logger 5 | from shapely.geometry import Polygon, LineString 6 | 7 | def calc_abc_from_line_2d(x0, y0, x1, y1): 8 | a = y0 - y1 9 | b = x1 - x0 10 | c = x0 * y1 - x1 * y0 11 | return a, b, c 12 | 13 | def get_line_cross_point(line1, line2): 14 | a0, b0, c0 = calc_abc_from_line_2d(*line1) 15 | a1, b1, c1 = calc_abc_from_line_2d(*line2) 16 | D = a0 * b1 - a1 * b0 17 | if D == 0: 18 | return None 19 | x = (b0 * c1 - b1 * c0) / D 20 | y = (a1 * c0 - a0 * c1) / D 21 | return x, y 22 | 23 | def right_rotation(coord, theta): 24 | """ 25 | theta : degree 26 | """ 27 | theta = math.radians(theta) 28 | x = coord[1] 29 | y = coord[0] 30 | x1 = x * math.cos(theta) - y * math.sin(theta) 31 | y1 = x * math.sin(theta) + y * math.cos(theta) 32 | return [y1, x1] 33 | 34 | def get_bbox(agent): 35 | agent_theta = agent['state'].transform.rotation.y 36 | agent_bbox = agent['bbox'] # min max (x_min, y_min, z_min) (x_max, y_max, z_max) 37 | 38 | global_x = agent['state'].transform.position.x 39 | global_z = agent['state'].transform.position.z 40 | x_min = agent_bbox.min.x + 0.1 41 | x_max = agent_bbox.max.x - 0.1 42 | z_min = agent_bbox.min.z + 0.1 43 | z_max = agent_bbox.max.z - 0.1 44 | 45 | line1 = [x_min, z_min, x_max, z_max] 46 | line2 = [x_min, z_max, x_max, z_min] 47 | x_center, z_center = get_line_cross_point(line1, line2) 48 | 49 | coords = [[x_min, z_min], [x_max, z_min], [x_max, z_max], [x_min, z_max]] 50 | new_coords = [] 51 | for i in range(len(coords)): 52 | coord_i = coords[i] 53 | coord_i[0] = coord_i[0] - x_center 54 | coord_i[1] = coord_i[1] - z_center 55 | new_coord_i = right_rotation(coord_i, agent_theta) 56 | new_coord_i[0] += global_x 57 | new_coord_i[1] += global_z 58 | new_coords.append(new_coord_i) 59 | p1, p2, p3, p4 = new_coords[0], new_coords[1], new_coords[2], new_coords[3] 60 | 61 | agent_poly = Polygon((p1, p2, p3, p4)) 62 | if agent_poly.area <= 0: 63 | print(agent_poly.area) 64 | exit(-1) 65 | return agent_poly 66 | 67 | def get_distance_ego_npc(ego, npc): 68 | ego_bbox = get_bbox(ego) 69 | npc_bbox = get_bbox(npc) 70 | d = ego_bbox.distance(npc_bbox) 71 | return d 72 | 73 | def get_distance_ego_line(ego, line_points): 74 | """ 75 | line_points: [start_point, ..., end_point] 76 | point (x, y) 77 | """ 78 | line = LineString(line_points) 79 | ego_bbox = get_bbox(ego) 80 | d = ego_bbox.distance(line) 81 | return d 82 | 83 | def is_hit_edge(ego, edge_lines): 84 | """ 85 | edge_lines : [line1, line2, line3] 86 | """ 87 | ego_bbox = get_bbox(ego) 88 | for l_i in edge_lines: 89 | l = LineString(l_i) 90 | d_i = ego_bbox.distance(l) 91 | if d_i == 0: 92 | return True 93 | return False 94 | 95 | def ego_npc_direction(ego, npc): 96 | ego_x = ego['state'].transform.position.x 97 | ego_z = ego['state'].transform.position.z 98 | npc_x = npc['state'].transform.position.x 99 | npc_z = npc['state'].transform.position.z 100 | 101 | ego_rotation = ego['state'].transform.rotation.y 102 | unit_direction = [0, 1] 103 | ego_direction = right_rotation(unit_direction, ego_rotation) 104 | 105 | dist_direction = [npc_x - ego_x, npc_z - ego_z] 106 | 107 | d = ego_direction[0] * dist_direction[0] + ego_direction[1] * dist_direction[1] 108 | 109 | return d 110 | 111 | def ego_npc_lateral(ego, npc): 112 | ego_x = ego['state'].transform.position.x 113 | ego_z = ego['state'].transform.position.z 114 | npc_x = npc['state'].transform.position.x 115 | npc_z = npc['state'].transform.position.z 116 | 117 | ego_rotation = ego['state'].transform.rotation.y 118 | unit_direction = [1, 0] 119 | ego_direction = right_rotation(unit_direction, ego_rotation) 120 | 121 | dist_direction = [npc_x - ego_x, npc_z - ego_z] 122 | 123 | d = abs(ego_direction[0] * dist_direction[0] + ego_direction[1] * dist_direction[1]) 124 | 125 | return d 126 | 127 | def ego_is_straight(ego, sim): 128 | ego_rotation = ego['state'].transform.rotation.y 129 | lane_center = sim.map_point_on_lane(ego['state'].transform.position) 130 | lane_rotation = lane_center.rotation.y 131 | if abs(ego_rotation - lane_rotation) > 8: 132 | return False 133 | else: 134 | return True 135 | 136 | 137 | def ego_yellow_line_fault(ego, yellow_line_points): 138 | ego_bbox = get_bbox(ego) 139 | 140 | yellow_line = LineString(yellow_line_points) 141 | distance_yellow = ego_bbox.distance(yellow_line) 142 | if distance_yellow <= 0: 143 | return True 144 | else: 145 | return False 146 | 147 | def ego_edge_line_fault(ego, edge_line_points): 148 | # 2. hit edge line 149 | ego_bbox = get_bbox(ego) 150 | edge_line = LineString(edge_line_points) 151 | distance_edge = ego_bbox.distance(edge_line) 152 | 153 | if distance_edge <= 0: 154 | return True 155 | else: 156 | return False 157 | 158 | def ego_cross_line(ego, cross_lines): 159 | ego_bbox = get_bbox(ego) 160 | for cross_line_points in cross_lines: 161 | cross_line = LineString(cross_line_points) 162 | cross_line_distance = ego_bbox.distance(cross_line) 163 | if cross_line_distance <= 0: 164 | return True 165 | return False 166 | 167 | def ego_collision_fault(ego, npc, cross_lines): 168 | """ 169 | coarse filter 170 | """ 171 | ego_speed = np.linalg.norm(np.array([ego['state'].velocity.x, ego['state'].velocity.y, ego['state'].velocity.z])) 172 | if ego_speed <= 0.1: 173 | return False 174 | 175 | is_ego_cross_line = ego_cross_line(ego, cross_lines) 176 | direct_ego_npc = ego_npc_direction(ego, npc) 177 | if is_ego_cross_line: 178 | logger.debug('Ego cross, seen as the ego fault') 179 | return True 180 | else: 181 | # zhui wei & jia sai 182 | logger.debug('Ego stay in line') 183 | if direct_ego_npc <= 0: 184 | logger.error('NPC fault') 185 | return False 186 | else: 187 | logger.error('Ego fault') 188 | return True 189 | 190 | def compute_danger_fitness(ego, npc, collision=False): 191 | if collision: 192 | ego_speed = np.array([ego['state'].velocity.x, ego['state'].velocity.y, ego['state'].velocity.z]) 193 | npc_speed = np.array([npc['state'].velocity.x, npc['state'].velocity.y, npc['state'].velocity.z]) 194 | fitness = np.linalg.norm(ego_speed - npc_speed) 195 | fitness = fitness + 100 196 | else: 197 | ego_speed = np.array([ego['state'].velocity.x, ego['state'].velocity.y, ego['state'].velocity.z]) 198 | npc_speed = np.array([npc['state'].velocity.x, npc['state'].velocity.y, npc['state'].velocity.z]) 199 | speed_norm = np.linalg.norm(ego_speed - npc_speed) 200 | location_norm = (get_distance_ego_npc(ego, npc) + 1) ** 2 201 | if location_norm <= 0: 202 | logger.warning('No collision, but distance norm <= 0') 203 | location_norm = 1.0 204 | fitness = speed_norm / location_norm 205 | 206 | return fitness 207 | 208 | -------------------------------------------------------------------------------- /simulation/run_parse.py: -------------------------------------------------------------------------------- 1 | import os 2 | import math 3 | import pickle 4 | import shutil 5 | 6 | #TODO: how to extract this file from apollo or is that neccessary? 7 | from loguru import logger 8 | from simulation.simulator import Simulator 9 | 10 | def isNaN(i_): 11 | return math.isnan(i_) 12 | 13 | def rotation(position, heading): 14 | cos_h = math.cos(heading) 15 | sin_h = math.sin(heading) 16 | 17 | x0, y0 = position[0], position[1] 18 | x1, y1 = x0 * cos_h + y0 * sin_h, y0 * cos_h - x0 * sin_h 19 | 20 | return (x1, y1) 21 | 22 | def clear_and_create(folder_path): 23 | if os.path.exists(folder_path): 24 | shutil.rmtree(folder_path) 25 | os.makedirs(folder_path) 26 | 27 | class Runner(object): 28 | 29 | def __init__(self, scenario_env_json, output_path, total_sim_time, default_record_folder, lgsvl_map = 'SanFrancisco_correct', apollo_map = 'SanFrancisco'): 30 | self.global_id = 0 31 | self.scenario_env_json = scenario_env_json 32 | self.scenario_name = os.path.basename(scenario_env_json).split('.')[0] 33 | 34 | self.SCENARIO_FOLDER = 'scenarios' 35 | self.RESULT_FOLDER = 'results' 36 | self.RECORD_FOLDER = 'records' 37 | 38 | self.default_record_folder = default_record_folder 39 | logger.info('Default record path: ' + str(self.default_record_folder)) 40 | 41 | self.scenario_path = os.path.join(output_path, 'simulation', self.SCENARIO_FOLDER) 42 | self.result_path = os.path.join(output_path, 'simulation', self.RESULT_FOLDER) 43 | self.record_path = os.path.join(output_path, 'simulation', self.RECORD_FOLDER) 44 | 45 | clear_and_create(self.scenario_path) 46 | clear_and_create(self.result_path) 47 | clear_and_create(self.record_path) 48 | 49 | self.sim = Simulator(self.default_record_folder, self.record_path, total_sim_time, lgsvl_map, apollo_map) # save record to records/scenario_name/scenario_id 50 | 51 | self.runner_log = os.path.join(output_path, 'logs/case_states.log') 52 | if os.path.exists(self.runner_log): 53 | os.remove(self.runner_log) 54 | 55 | def run(self, scenario_data): 56 | scenario_id = 'scenario_' + str(self.global_id) 57 | sim_result = self._run_scenario(scenario_id, scenario_data) 58 | if sim_result is None: 59 | print('sim_result is None, ERROR') 60 | exit(-1) 61 | # TODO: add test log, to record test results. 62 | 63 | sim_fault = sim_result['fault'] 64 | with open(self.runner_log, 'a') as f: 65 | f.write(str(scenario_id)) 66 | for item in sim_fault: 67 | f.write(' ') 68 | f.write(str(item)) 69 | f.write('\n') 70 | 71 | 72 | self.global_id += 1 73 | logger.info(' === Simulation Result: ' + str(sim_result)) 74 | logger.info(' === Record ' + scenario_id + ' to ' + self.runner_log) 75 | return float(sim_result['fitness']), scenario_id 76 | 77 | def _run_scenario(self, scenario_id, scenario_data): 78 | """ 79 | run elements: 80 | save - recording, json config 81 | save scenario config to scenario_name/scenarios/scenario_id 82 | save results to scenario_name/results/scenario_id 83 | """ 84 | scenario_file = os.path.join(self.scenario_path, scenario_id + '.obj') 85 | with open(scenario_file, 'wb') as s_f: 86 | pickle.dump(scenario_data, s_f) 87 | 88 | result_file = os.path.join(self.result_path, scenario_id + '.obj') 89 | 90 | if os.path.isfile(result_file): 91 | os.remove(result_file) 92 | 93 | # replace simulator codes 94 | resultDic = {} 95 | try: 96 | resultDic = self.sim.runSimulation(scenario_data, self.scenario_env_json, scenario_id) 97 | except Exception as e: 98 | logger.debug(e.message) 99 | #resultDic['fitness'] = '' 100 | resultDic['fault'] = '' 101 | 102 | # Send fitness score int object back to ge 103 | if os.path.isfile(result_file): 104 | os.system("rm " + result_file) 105 | with open(result_file, 'wb') as f_f: 106 | pickle.dump(resultDic, f_f) 107 | 108 | return resultDic 109 | -------------------------------------------------------------------------------- /simulation/simulator.py: -------------------------------------------------------------------------------- 1 | import os 2 | import lgsvl 3 | import time 4 | import json 5 | 6 | import simulation.utils as util 7 | import simulation.liability as liability 8 | 9 | from datetime import datetime 10 | from loguru import logger 11 | 12 | EGO_VEHICLE_ID = '2e966a70-4a19-44b5-a5e7-64e00a7bc5de' 13 | 14 | class Simulator(object): 15 | 16 | def __init__(self, default_record_folder, target_record_folder, total_sim_time, lgsvl_map = 'SanFrancisco_correct', apollo_map = 'SanFrancisco'): 17 | 18 | self.default_record_folder = default_record_folder 19 | self.target_record_folder = target_record_folder 20 | ################################################################ 21 | self.total_sim_time = total_sim_time 22 | self.destination = None 23 | ################################################################ 24 | self.sim = None 25 | self.data_prime = None 26 | self.dv = None 27 | self.lgsvl_map = lgsvl_map 28 | self.apollo_map = apollo_map 29 | self.ego = None 30 | self.mutated_npc_list = [] # The list contains all the npc added 31 | self.fixed_npc_list = [] 32 | self.yellow_lines = None 33 | self.cross_lines = None 34 | self.edge_lines = None 35 | 36 | self.connect_lgsvl() 37 | self.load_map(self.lgsvl_map) 38 | self.isEgoFault = False 39 | self.isHit = False 40 | self.maxint = 130 41 | self.egoFaultDeltaD = 0 42 | 43 | self.modules = [ 44 | 'Localization', 45 | 'Transform', 46 | 'Routing', 47 | 'Prediction', 48 | 'Planning', 49 | 'Control', 50 | 'Storytelling', 51 | ] 52 | self.dy_modules = [ 53 | 'Recorder', 54 | ] 55 | 56 | def connect_lgsvl(self): 57 | try: 58 | sim = lgsvl.Simulator(os.environ.get("SIMULATOR_HOST", "127.0.0.1"), 8181) 59 | self.sim = sim 60 | except Exception as e: 61 | logger.error('Connect LGSVL wrong: ' + '127.0.0.1:8181') 62 | logger.error(e.message) 63 | logger.info('Connected LGSVL 127.0.0.1:8181') 64 | 65 | def load_map(self, mapName="SanFrancisco_correct"): 66 | if self.sim.current_scene == mapName: 67 | self.sim.reset() 68 | else: 69 | self.sim.load(mapName) 70 | logger.info('Loaded map: ' + mapName) 71 | 72 | def load_json(self, json_file): 73 | self.data_prime = json.load(open(json_file)) 74 | if not self.data_prime.get('environment'): 75 | self.data_prime['environment'] = dict() 76 | self.data_prime['environment'] = { 77 | 'rain': 0, 78 | 'fog': 0, 79 | 'wetness': 0, 80 | 'cloudiness': 0, 81 | 'damage': 0, 82 | 'time':0, 83 | } 84 | 85 | def init_environment(self, json_file): 86 | """ 87 | 88 | Args: 89 | json_file: contains env configs 90 | must: car position 91 | ego - destination (forward, right) or (x, y, z) 92 | Returns: 93 | 94 | """ 95 | 96 | self.isEgoFault = False 97 | self.isHit = False 98 | self.mutated_npc_list = [] 99 | self.fixed_npc_list = [] 100 | 101 | self.load_json(json_file) 102 | 103 | # load ego car 104 | ego_data = self.data_prime['agents']['ego'] 105 | ego_position = ego_data['position'] 106 | ego_pos_vector = lgsvl.Vector(x=ego_position['x'], y=ego_position['y'], z=ego_position['z']) 107 | ego_state = lgsvl.AgentState() 108 | ego_state.transform = self.sim.map_point_on_lane(ego_pos_vector) 109 | self.ego = self.sim.add_agent(EGO_VEHICLE_ID, lgsvl.AgentType.EGO, ego_state) 110 | ## ego destination 111 | des_method = ego_data['destination']['method'] 112 | if des_method == 'forward_right': 113 | des_forward = ego_data['destination']['value']['v1'] 114 | des_right = ego_data['destination']['value']['v2'] 115 | forward = lgsvl.utils.transform_to_forward(ego_state.transform) 116 | right = lgsvl.utils.transform_to_right(ego_state.transform) 117 | self.destination = ego_state.position + des_forward * forward + des_right * right 118 | elif des_method == 'xyz': 119 | x = ego_data['destination']['value']['v1'] 120 | y = ego_data['destination']['value']['v2'] 121 | z = ego_data['destination']['value']['v3'] 122 | self.destination = lgsvl.Vector(x, y, z) 123 | else: 124 | raise RuntimeError('Unmatched destination method') 125 | 126 | # load mutated npc 127 | npcs = self.data_prime['agents']['npcs'] 128 | for m_npc in npcs: 129 | npc_type = m_npc['type'] 130 | npc_goal = m_npc['goal'] 131 | npc_pos_x = m_npc['position']['x'] 132 | npc_pos_y = m_npc['position']['y'] 133 | npc_pos_z = m_npc['position']['z'] 134 | npc_pos = lgsvl.Vector(x=npc_pos_x, y=npc_pos_y, z=npc_pos_z) 135 | npc_state = lgsvl.AgentState() 136 | npc_state.transform = self.sim.map_point_on_lane(npc_pos) 137 | npc = self.sim.add_agent(npc_type, lgsvl.AgentType.NPC, npc_state) 138 | if npc_goal == 'fixed': 139 | self.fixed_npc_list.append(npc) 140 | elif npc_goal == 'mutated': 141 | self.mutated_npc_list.append(npc) 142 | else: 143 | raise RuntimeError('Wrong npc goal. Only support fixed or mutated.') 144 | 145 | # load environments 146 | self.sim.weather = lgsvl.WeatherState( 147 | rain=self.data_prime['environment']['rain'], 148 | fog=self.data_prime['environment']['fog'], 149 | wetness=self.data_prime['environment']['wetness'], 150 | cloudiness=self.data_prime['environment']['cloudiness'], 151 | damage=self.data_prime['environment']['damage'] 152 | ) 153 | self.sim.set_time_of_day(self.data_prime['environment']['time']) 154 | 155 | # load lines 156 | # yellow line 157 | self.yellow_lines = self.data_prime['lines']['yellow_lines'] 158 | self.cross_lines = self.data_prime['lines']['cross_lines'] 159 | self.edge_lines = self.data_prime['lines']['edge_lines'] 160 | 161 | def runSimulation(self, scenario_obj, json_file, case_id): 162 | 163 | #exit_handler() 164 | now = datetime.now() 165 | date_time = now.strftime("%m-%d-%Y-%H-%M-%S") 166 | logger.info(' === Simulation Start: [' + date_time + '] ===') 167 | 168 | self.sim.reset() 169 | self.init_environment(json_file) 170 | 171 | time_slice_size = len(scenario_obj[0]) 172 | mutated_npc_num = len(scenario_obj) 173 | 174 | assert mutated_npc_num == len(self.mutated_npc_list) 175 | 176 | # simulation info 177 | simulation_recording = { 178 | 'bbox': { 179 | 'ego' : self.ego.bounding_box 180 | }, 181 | 'frames': { 182 | 183 | } 184 | } 185 | for npc_i in range(mutated_npc_num): 186 | simulation_recording['bbox']['npc_' + str(npc_i)] = self.mutated_npc_list[npc_i].bounding_box 187 | 188 | global collision_info 189 | global accident_happen 190 | global time_index 191 | 192 | collision_info = None 193 | accident_happen = False 194 | 195 | def on_collision(agent1, agent2, contact): 196 | global accident_happen 197 | global collision_info 198 | global time_index 199 | 200 | accident_happen = True 201 | collision_info = {} 202 | 203 | name1 = "STATIC OBSTACLE" if agent1 is None else agent1.name 204 | name2 = "STATIC OBSTACLE" if agent2 is None else agent2.name 205 | logger.error(str(name1) + " collided with " + str(name2) + " at " + str(contact)) 206 | 207 | agent1_info = [agent1.state, agent1.bounding_box] 208 | 209 | if not agent2: 210 | agent2_info = [None, None] 211 | else: 212 | agent2_info = [agent2.state, agent2.bounding_box] 213 | 214 | if contact: 215 | contact_loc = [contact.x, contact.y, contact.z] 216 | 217 | collision_info['time'] = time_index 218 | collision_info['ego'] = agent1_info 219 | collision_info['npc'] = agent2_info 220 | collision_info['contact'] = contact_loc 221 | 222 | self.sim.stop() 223 | 224 | # INIT apollo 225 | self.ego.connect_bridge(address='127.0.0.1', port=9090) #address, port 226 | self.ego.on_collision(on_collision) 227 | 228 | times = 0 229 | success = False 230 | while times < 3: 231 | try: 232 | dv = lgsvl.dreamview.Connection(self.sim, self.ego, os.environ.get("BRIDGE_HOST", "127.0.0.1")) 233 | dv.set_hd_map(self.apollo_map) 234 | dv.set_vehicle('Lincoln2017MKZ_LGSVL') 235 | dv.setup_apollo(self.destination.x, self.destination.z, self.modules, default_timeout=30) 236 | success = True 237 | break 238 | except: 239 | logger.warning('Fail to spin up apollo, try again!') 240 | times += 1 241 | if not success: 242 | raise RuntimeError('Fail to spin up apollo') 243 | 244 | if self.default_record_folder: 245 | util.disnable_modules(dv, self.dy_modules) 246 | time.sleep(1) 247 | util.enable_modules(dv, self.dy_modules) 248 | 249 | dv.set_destination(self.destination.x, self.destination.z) 250 | logger.info(' --- destination: ' + str(self.destination.x) + ',' + str(self.destination.z)) 251 | 252 | delay_t = 5 253 | time.sleep(delay_t) 254 | 255 | for npc in self.mutated_npc_list: 256 | npc.follow_closest_lane(True, 0) 257 | 258 | for npc in self.fixed_npc_list: 259 | npc.follow_closest_lane(True, 13.4) 260 | 261 | # Frequency of action change of NPCs 262 | total_sim_time = self.total_sim_time 263 | action_change_freq = total_sim_time / time_slice_size 264 | time_index = 0 265 | 266 | # record start 267 | simulation_recording['frames'][time_index] = { 268 | 'ego': self.ego.state 269 | } 270 | 271 | for npc_i in range(mutated_npc_num): 272 | simulation_recording['frames'][time_index]['npc_' + str(npc_i)] = self.mutated_npc_list[npc_i].state 273 | 274 | for t in range(0, int(time_slice_size)): 275 | # check module states 276 | 277 | 278 | 279 | # actionChangeFreq seconds 280 | # For every npc 281 | i = 0 282 | for npc in self.mutated_npc_list: 283 | npc.follow_closest_lane(True, scenario_obj[i][t][0]) 284 | turn_command = scenario_obj[i][t][1] 285 | 286 | #<0: no turn; 1: left; 2: right> 287 | if turn_command == 1: 288 | #direction = "LEFT" 289 | npc.change_lane(True) 290 | 291 | elif turn_command == 2: 292 | #direction = "RIGHT" 293 | npc.change_lane(False) 294 | 295 | i += 1 296 | 297 | for j in range(0, int(action_change_freq) * 10): 298 | module_status_mark = True 299 | while module_status_mark: 300 | module_status_mark = False 301 | module_status = dv.get_module_status() 302 | for module, status in module_status.items(): 303 | if (not status) and (module in self.modules): 304 | logger.warning('$$Simulator$$ Module is closed: ' + module + ' ==> restart') 305 | dv.enable_module(module) 306 | time.sleep(0.5) 307 | module_status_mark = True 308 | time_index += 1 309 | 310 | self.sim.run(0.1) 311 | 312 | simulation_recording['frames'][time_index] = { 313 | 'ego': self.ego.state 314 | } 315 | 316 | for npc_i in range(len(self.mutated_npc_list)): 317 | simulation_recording['frames'][time_index]['npc_' + str(npc_i)] = self.mutated_npc_list[npc_i].state 318 | 319 | 320 | if self.default_record_folder: 321 | util.disnable_modules(dv, self.dy_modules) 322 | time.sleep(0.5) 323 | 324 | # check new folder and move -> save folder 325 | if self.default_record_folder: 326 | util.check_rename_record(self.default_record_folder, self.target_record_folder, case_id) 327 | 328 | 329 | # compute fitness score & check other bugs such as line cross or else 330 | ''' 331 | 332 | 333 | global collision_info 334 | global accident_happen 335 | 336 | collision_info = None 337 | accident_happen = False 338 | 339 | ''' 340 | # Step 1 obtain time 341 | simulation_slices = max(simulation_recording['frames'].keys()) 342 | 343 | ''' 344 | simulation_recording[time_index] = { 345 | 'ego': self.ego.transform, 346 | 'npc': [] 347 | } 348 | ''' 349 | fault = [] 350 | max_fitness = -1111 351 | # Step 2 compute distance and check line error and filter npc_fault 352 | for t in range(simulation_slices): 353 | simulation_frame = simulation_recording['frames'][t] 354 | ego_info = { 355 | 'state': simulation_frame['ego'], 356 | 'bbox': simulation_recording['bbox']['ego'] 357 | } 358 | # compute distance 359 | for npc_i in range(len(self.mutated_npc_list)): 360 | npc_id = 'npc_' + str(npc_i) 361 | npc_info = { 362 | 'state': simulation_frame[npc_id], 363 | 'bbox': simulation_recording['bbox'][npc_id] 364 | } 365 | 366 | npc_ego_fitness = liability.compute_danger_fitness(ego_info, npc_info, False) 367 | 368 | if npc_ego_fitness > max_fitness: 369 | max_fitness = npc_ego_fitness 370 | 371 | # check line 372 | for yellow_line in self.yellow_lines: 373 | hit_yellow_line = liability.ego_yellow_line_fault(ego_info, yellow_line) 374 | if hit_yellow_line: 375 | fault.append('hit_yellow_line') 376 | 377 | for edge_line in self.edge_lines: 378 | hit_edge_line = liability.ego_edge_line_fault(ego_info, edge_line) 379 | if hit_edge_line: 380 | fault.append('hit_edge_line') 381 | 382 | # Step 3 if collision, check is npc fault 383 | ''' 384 | agent1_info = [agent1.transform, agent1.state] 385 | 386 | if not agent2: 387 | agent2_info = [None, None] 388 | else: 389 | agent2_info = [agent2.transform, agent2.state] 390 | 391 | if contact: 392 | contact_loc = [contact.x, contact.y, contact.z] 393 | 394 | collision_info['time'] = time_index 395 | collision_info['ego'] = agent1_info 396 | collision_info['npc'] = agent2_info 397 | collision_info['contact'] = contact_loc 398 | 399 | ''' 400 | if collision_info is not None: 401 | ego_info = { 402 | 'state': collision_info['ego'][0], 403 | 'bbox': collision_info['ego'][1] 404 | } 405 | 406 | npc_info = { 407 | 'state': collision_info['npc'][0], 408 | 'bbox': collision_info['npc'][1] 409 | } 410 | 411 | ego_fault = liability.ego_collision_fault(ego_info, npc_info, self.cross_lines) 412 | if ego_fault: 413 | fault.append('ego_fault') 414 | else: 415 | fault.append('npc_fault') 416 | 417 | fitness = liability.compute_danger_fitness(ego_info, npc_info, True) 418 | if fitness <= max_fitness: 419 | logger.error('Please increase K in liability.compute_danger_fitness: Collision - ' + str(fitness) + 'No Collision - ' + str(max_fitness)) 420 | raise RuntimeError('liability.compute_danger_fitness parameter setting is not right.') 421 | else: 422 | max_fitness = fitness 423 | 424 | if len(fault) == 0: 425 | fault.append('normal') 426 | 427 | #fitness_score = self.findFitness(deltaDList, dList, self.isHit, hit_time) 428 | 429 | result_dict = {} 430 | result_dict['fitness'] = max_fitness 431 | #(fitness_score + self.maxint) / float(len(self.mutated_npc_list) - 1 ) # Try to make sure it is positive 432 | result_dict['fault'] = fault 433 | 434 | logger.info(' === Simulation End === ') 435 | 436 | return result_dict -------------------------------------------------------------------------------- /simulation/utils.py: -------------------------------------------------------------------------------- 1 | import os 2 | import shutil 3 | import time 4 | 5 | from loguru import logger 6 | 7 | def check_rename_record(default_path, target_path, case_id): 8 | if not os.path.exists(target_path): 9 | os.makedirs(target_path) 10 | 11 | folders = os.listdir(default_path) 12 | folders = sorted(folders) 13 | if len(folders) > 0: 14 | original_folder = folders[-1] 15 | original_fpath = os.path.join(default_path, original_folder) 16 | target_fpath = os.path.join(target_path, case_id) 17 | shutil.move(original_fpath, target_fpath) 18 | logger.info(' --- Move: ' + original_fpath + ' ==> ' + target_fpath) 19 | 20 | def enable_modules(dv, modules): 21 | # try 5 times 22 | not_all = True 23 | while not_all: 24 | not_all = False 25 | module_status = dv.get_module_status() 26 | for module, status in module_status.items(): 27 | if (not status) and (module in modules): 28 | dv.enable_module(module) 29 | not_all = True 30 | time.sleep(1) 31 | 32 | def disnable_modules(dv, modules): 33 | not_all = True 34 | while not_all: 35 | not_all = False 36 | module_status = dv.get_module_status() 37 | for module, status in module_status.items(): 38 | if status and (module in modules): 39 | dv.disable_module(module) 40 | not_all = True 41 | time.sleep(1) 42 | --------------------------------------------------------------------------------