def test_vehicle_move(beamng): with beamng as bng: bng.set_deterministic() scenario = Scenario('smallgrid', 'move_test') vehicle = Vehicle('test_car', model='etk800') scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0)) scenario.make(bng) bng.load_scenario(scenario) bng.start_scenario() bng.pause() vehicle.control(throttle=1) bng.step(120) vehicle.update_vehicle() assert np.linalg.norm(vehicle.state['pos']) > 1 scenario.delete(beamng)
def test_vehicle_spawn(beamng): scenario = Scenario('smallgrid', 'spawn_test') vehicle = Vehicle('irrelevant', model='pickup') scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0)) scenario.make(beamng) with beamng as bng: bng.load_scenario(scenario) bng.start_scenario() other = Vehicle('relevant', model='etk800') scenario.add_vehicle(other, pos=(10, 10, 0), rot=(0, 0, 0)) other.update_vehicle() assert 'pos' in other.state bng.step(120) scenario.remove_vehicle(other) bng.step(600) assert other.state is None
def collect_data(beamNG_path): bng = BeamNGpy('localhost', 64256, beamNG_path) #bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk') scenario = Scenario('GridMap', 'example') # Create an ETK800 with the licence plate 'PYTHON' vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red') #vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue') electrics = Electrics() vehicle1.attach_sensor('electrics', electrics) pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775) rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936) scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1) # Place files defining our scenario for the simulator to read scenario.make(bng) # Launch BeamNG.research bng.open(launch=True) #SIZE = 50 try: bng.load_scenario(scenario) bng.start_scenario() #vehicle1.ai_set_speed(10.452066507339481,mode = 'set') vehicle1.ai_set_mode('span') #collect data print("\n Position and rotation of car \n ") # for _ in range(200): for _ in range(200): time.sleep(0.1) vehicle1.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors( vehicle1 ) # Polls the data of all sensors attached to the vehicle positions.append(vehicle1.state['pos']) directions.append(vehicle1.state['dir']) print([vehicle1.state['pos'], vehicle1.state['dir']]) #write data into file # print ("position :",positions) # print ("position :",directions) sys_output.print_title( "\n The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \"" ) write_data(FILE_POS, positions) write_data(FILE_ROT, directions) bng.stop_scenario() bng.close() time.sleep(2) finally: bng.close() return (positions, directions)
def run_spawn_for_parts_config( vehicle_model='etk800', loadfile='C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json' ): global base_filename, default_color, default_scenario, setpoint global prev_error global fcam # setup DNN model + weights m = Model() model = m.define_model_BeamNG("BeamNGmodel-5.h5") random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='LOWPRESS', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, 'highway') # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) # bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(100) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # vehicle.load('C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/pristine.json') if loadfile: bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=loadfile) else: bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig='vehicles/hopper/custom.pc') #bng.set_relative_camera() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() bng.set_steps_per_second(100) bng.set_deterministic() totalsecs = 0.0 pitch_traj = [] while totalsecs <= 30: vehicle.update_vehicle() camera_state = bng.poll_sensors(vehicle)['front_cam'] camera_state['roll'] = vehicle.state['roll'] camera_state['pitch'] = vehicle.state['pitch'] camera_state['yaw'] = vehicle.state['yaw'] pitch_traj.append(round(math.degrees(vehicle.state['pitch'][0]), 4)) print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'], vehicle.state['pitch'], vehicle.state['yaw'])) bng.step(100) totalsecs += 1 # print("Camera state:{}".format(camera_state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') #get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up']) img = m.process_image(image) before_prediction = model.predict(img) camera_state["prediction"] = before_prediction plt.imshow(image) plt.pause(0.01) bng.close() return camera_state, vehicle.state, pitch_traj
car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue') scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55)) scenario.make(beamng) bng = beamng.open(launch=True) bng.load_scenario(scenario) bng.start_scenario() vut.ai_set_mode('span') vut.ai_drive_in_lane(True) car_1.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}]) for _ in range(240): sleep(0.1) vut.update_vehicle() sensors = bng.poll_sensors(vut) dmg = sensors['damage'] # Below code snippet is generated form 'detect_obstacle_car' function for car_1 scenario.update() dist_car_1 = np.linalg.norm( np.array(vut.state['pos']) - np.array(car_1.state['pos'])) if dist_car_1 < 8: print('Car Detected') # Below code snippet is generated form 'ai_stopped' function for car_1 scenario.update() if sensors['electrics']['values']['wheelspeed'] == 0 or dmg[ 'damage'] == 0:
import mmap
from beamngpy import BeamNGpy, Vehicle, Scenario, Road from beamngpy.sensors import Camera from getAIScript import getAIScript beamng = BeamNGpy('localhost', 64256, getBeamngDirectory()) scenario = Scenario('smallgrid', 'vehicle_bbox_example') road = Road('track_editor_A_center', rid='main_road') orig = (0, -2, 0) goal = (1150, -22, 0) nodes = [(*orig, 7), (*goal, 7)] road.nodes.extend(nodes) scenario.add_road(road) vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON') overhead = Camera((0, -10, 5), (0, 1, -0.75), 60, (1024, 1024)) vehicle.attach_sensor('overhead', overhead) scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, -90)) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() script = getAIScript() vehicle.ai_set_script(script) while (True): vehicle.update_vehicle() print(vehicle.state['pos']) input() finally: bng.close()
def collection_run(speed=11, risk=0.6, num_samples=10000): global base_filename, training_dir, default_model, setpoint global spawnpoint, steps_per_second f = setup_dir(training_dir) spawn_pt = spawn_point(default_scenario, spawnpoint) random.seed(1703) setup_logging() home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.tech.v0.21.3.0' # beamng = BeamNGpy('localhost', 64256, home=home, user='******') scenario = Scenario(default_scenario, 'research_test') # add barriers and cars to get the ego vehicle to avoid the barriers add_barriers(scenario) # add_barrier_cars(scenario) vehicle = Vehicle('ego_vehicle', model=default_model, licence='DATAMKR', color='White') vehicle = setup_sensors(vehicle) scenario.add_vehicle(vehicle, pos=spawn_pt['pos'], rot=None, rot_quat=spawn_pt['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) # bng.hide_hud() bng.set_nondeterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_second) # # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() bng.switch_vehicle(vehicle) # ai_line, bng = create_ai_line_from_road(spawn_pt, bng) ai_line, bng = create_ai_line_from_road_with_interpolation(spawn_pt, bng) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # bng.resume() start_time = time.time() # Send random inputs to vehicle and advance the simulation 20 steps imagecount = 0 timer = 0 traj = [] steering_inputs = [] timestamps = [] kphs = [] with open(f, 'w') as datafile: datafile.write( 'filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,' 'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature,wheelspeed\n' ) return_str = '' while imagecount < num_samples: sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') full_filename = "{}{}{}.jpg".format(training_dir, base_filename, imagecount) qualified_filename = "{}{}.jpg".format(base_filename, imagecount) steering = sensors['electrics']['steering_input'] dt = sensors['timer']['time'] - timer if dt > 0: throttle = throttle_PID( sensors['electrics']['wheelspeed'] * 3.6, dt) # print("current kph:{} dt:{} throttle:{}".format(sensors['electrics']['wheelspeed'] * 3.6, dt, throttle)) vehicle.update_vehicle() # points=[vehicle.state['front']] # point_colors = [[0, 1, 0, 0.1]] # spheres=[[vehicle.state['front'][0],vehicle.state['front'][1],vehicle.state['front'][1],0.25]] # sphere_colors=[[1, 0, 0, 0.8]] # bng.add_debug_line(points, point_colors, # spheres=spheres, sphere_colors=sphere_colors, # cling=True, offset=0.1) try: steering_setpt = steering_setpoint(vehicle.state, traj) if dt > 0: steering = steering_PID(steering, steering_setpt, dt) vehicle.control(steering=steering, throttle=throttle) timer = sensors['timer']['time'] # traj.append(vehicle.state['pos']) # timestamps.append(timer) # steering_inputs.append(steering) # kphs.append(sensors['electrics']['wheelspeed'] * 3.6) except IndexError as e: print(e, "exiting...") print(e.with_traceback()) plot_trajectory(traj, "Car Behavior using AI Script") plot_inputs(timestamps, steering_inputs, title="Steering Inputs by Time") plot_inputs(timestamps, kphs, title="KPHs by Time") exit(0) if timer > 10: # and abs(steering) >= 0.1: # collect ancillary data datafile.write( '{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format( qualified_filename, str(round(sensors['timer']['time'], 2)), sensors['electrics']['steering_input'], sensors['electrics']['throttle_input'], sensors['electrics']['brake_input'], sensors['electrics']['driveshaft'], sensors['electrics']['engine_load'], sensors['electrics']['fog_lights'], sensors['electrics']['fuel'], sensors['electrics']['lowpressure'], sensors['electrics']['oil'], sensors['electrics']['oil_temperature'], sensors['electrics']['parkingbrake'], sensors['electrics']['rpm'], sensors['electrics']['water_temperature'], sensors['electrics']['wheelspeed'])) # save the image image.save(full_filename) imagecount += 1 print(f"{imagecount=}") if sensors['damage']['damage'] > 0: return_str = "CRASHED at timestep {} speed {}; QUITTING".format( round(sensors['timer']['time'], 2), round(sensors['electrics']['wheelspeed'] * 3.6, 3)) print(return_str) break # if timer > 300: # break bng.step(1, wait=True) plot_trajectory(traj, "Car Behavior using AI Script") plot_inputs(timestamps, steering_inputs, title="Steering Inputs by Time") plot_inputs(timestamps, kphs, title="KPHs by Time") bng.close() plot_trajectory(traj, "Car Behavior using AI Script") return return_str
def main(): global default_model, default_scenario beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') #scenario = Scenario('smallgrid', 'spawn_objects_example') scenario = Scenario(default_scenario, 'research_test', description='Random driving for research') vehicle = Vehicle('ego_vehicle', model=default_model, licence='PYTHON') vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario) scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=spawn['rot'], rot_quat=spawn['rot_quat']) scenario.make(beamng) bng = beamng.open() bng.load_scenario(scenario) bng.start_scenario() vehicle.update_vehicle() d1 = bng.poll_sensors(vehicle) cum_list = [] bound = 0.0 for i in range(3): for _ in range(45): bound = bound + 0.0 # 0.1 # vehicle.save() vehicle.update_vehicle() d2 = bng.poll_sensors(vehicle) throttle = 1.0 #throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1 * bound, bound) brake = 0.0 #random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) pointName = "{}_{}".format(i, _) cum_list.append(pointName) vehicle.saveRecoveryPoint(pointName) bng.step(20) print("SEGMENT #{}: COMPARE DAMAGE".format(i)) damage_diff = compare_damage(d1, d2) d1 = d2 # "Back up" 1 second -- load vehicle at that time in that position. backup_pointName = backup(cum_list, 0.001) print('recovering to {}'.format(pointName)) loadfile = vehicle.loadRecoveryPoint(backup_pointName) print('loadfile is {}'.format(loadfile)) bng.pause() vehicle.update_vehicle() vehicle.load(loadfile) #vehicle.load("vehicles/pickup/vehicle.save.json") bng.resume() #vehicle.startRecovering() #time.sleep(1.5) #vehicle.stopRecovering() vehicle.update_vehicle() bng.pause() time.sleep(2) # vehicle.load("vehicles/pickup/vehicle.save.json") bng.resume() bng.close()
def run_scenario(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config='vehicles/hopper/custom.pc', run_number=0): global base_filename, default_color, default_scenario, default_spawnpoint, steps_per_sec global integral, prev_error, setpoint integral = 0.0 prev_error = 0.0 # setup DNN model + weights sm = DAVE2Model() # steering_model = Model().define_model_BeamNG("BeamNGmodel-racetracksteering8.h5") # throttle_model = Model().define_model_BeamNG("BeamNGmodel-racetrackthrottle8.h5") dual_model = sm.define_dual_model_BeamNG() # dual_model = sm.load_weights("BeamNGmodel-racetrackdualcomparison10K.h5") # dual_model = sm.define_multi_input_model_BeamNG() # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison10K-PIDcontrolset-4.h5") # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison40K-PIDcontrolset-1.h5") # BeamNGmodel-racetrack-multiinput-dualoutput-comparison10K-PIDcontrolset-1.h5 # BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2 dual_model = sm.load_weights( "BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2.h5") # dual_model = sm.load_weights("BeamNGmodel-racetrack-multiinput-dualoutput-comparison103K-PIDcontrolset-1.h5") random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean', user='******') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='EGO', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle( vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) #, partConfig=parts_config) add_barriers(scenario) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 60hz temporal resolution bng.load_scenario(scenario) bng.start_scenario() # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # bng.resume() # perturb vehicle print("vehicle position before deflation via beamstate:{}".format( vehicle.get_object_position())) print("vehicle position before deflation via vehicle state:{}".format( vehicle.state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state)) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) # bng.resume() # vehicle.break_all_breakgroups() # vehicle.break_hinges() wheelspeed = 0.0 throttle = 0.0 prev_error = setpoint damage_prev = None runtime = 0.0 kphs = [] traj = [] pitches = [] rolls = [] steering_inputs = [] throttle_inputs = [] timestamps = [] damage = None final_img = None # Send random inputs to vehice and advance the simulation 20 steps overall_damage = 0.0 total_loops = 0 total_imgs = 0 total_predictions = 0 while overall_damage <= 0: # collect images sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) total_imgs += 1 img = sm.process_image(np.asarray(image)) wheelspeed = sensors['electrics']['wheelspeed'] kph = ms_to_kph(wheelspeed) dual_prediction = dual_model.predict(x=[img, np.array([kph])]) # steering_prediction = steering_model.predict(img) # throttle_prediction = throttle_model.predict(img) dt = sensors['timer']['time'] - runtime runtime = sensors['timer']['time'] # control params brake = 0 # steering = float(steering_prediction[0][0]) #random.uniform(-1.0, 1.0) # throttle = float(throttle_prediction[0][0]) steering = float(dual_prediction[0][0]) #random.uniform(-1.0, 1.0) throttle = float(dual_prediction[0][1]) total_predictions += 1 if abs(steering) > 0.2: setpoint = 20 else: setpoint = 40 # if runtime < 10: throttle = throttle_PID(kph, dt) # if throttle > 1: # throttle = 1 # if setpoint < kph: # brake = 0.0 #throttle / 10000.0 # throttle = 0.0 vehicle.control(throttle=throttle, steering=steering, brake=brake) steering_inputs.append(steering) throttle_inputs.append(throttle) timestamps.append(runtime) steering_state = sensors['electrics']['steering'] steering_input = sensors['electrics']['steering_input'] avg_wheel_av = sensors['electrics']['avg_wheel_av'] damage = sensors['damage'] overall_damage = damage["damage"] new_damage = diff_damage(damage, damage_prev) damage_prev = damage vehicle.update_vehicle() traj.append(vehicle.state['pos']) pitches.append(vehicle.state['pitch'][0]) rolls.append(vehicle.state['roll'][0]) kphs.append(ms_to_kph(wheelspeed)) total_loops += 1 if new_damage > 0.0: final_img = image break bng.step(1, wait=True) if runtime > 300: print("Exited after 5 minutes successful runtime") break if distance( spawn['pos'], vehicle.state['pos']) < 5 and sensors['timer']['time'] > 10: reached_start = True break # print("runtime:{}".format(round(runtime, 2))) # print("time to crash:{}".format(round(runtime, 2))) bng.close() # avg_kph = float(sum(kphs)) / len(kphs) plt.imshow(final_img) plt.savefig("Run-{}-finalimg.png".format(run_number)) plt.pause(0.01) plot_input(timestamps, steering_inputs, "Steering", run_number=run_number) plot_input(timestamps, throttle_inputs, "Throttle", run_number=run_number) plot_input(timestamps, kphs, "KPHs", run_number=run_number) print("Number of steering_inputs:", len(steering_inputs)) print("Number of throttle inputs:", len(throttle_inputs)) results = "Total loops: {} \ntotal images: {} \ntotal predictions: {} \nexpected predictions ={}*{}={}".format( total_loops, total_imgs, total_predictions, round(runtime, 3), steps_per_sec, round(runtime * steps_per_sec, 3)) print(results) results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': final_img, 'total_predictions': total_predictions, 'expected_predictions': round(runtime * steps_per_sec, 3) } return results
def main(): random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') state_cfg = Config() parts_cfg = Config() # Create a scenario in west_coast_usa scenario = Scenario('west_coast_usa', 'research_test', description='Random driving for research') # Set up first vehicle, with two cameras, gforces sensor, lidar, electrical # sensors, and damage sensors vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') vehicle = setup_sensors(vehicle) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795)) # Compile the scenario and place it in BeamNG's map folder #scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) try: #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() pcfg = vehicle.get_part_config() assert vehicle.skt # Send random inputs to vehicle and advance the simulation 20 steps #for _ in range(1024): for _ in range(30): throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1.0, 1.0) brake = random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) # bng.step(20) bng.step(20) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) print('\n{} seconds passed.'.format(sensors['timer']['time'])) print("step in loop {}".format(_)) finally: sensors = bng.poll_sensors(vehicle) for s in sensors.keys(): print("{} : {}".format(s, sensors[s])) damage_dict = sensors['damage'] state_cfg.update(sensors['damage']) state_cfg.update(vehicle.state) state_cfg.save('{}/state_cfg.json'.format(beamng.home)) vehicle.annotate_parts() vehicle.update_vehicle() #part_options = vehicle.get_part_options() parts_cfg_dict = vehicle.get_part_config() parts_cfg.load_values(vehicle.get_part_config()) #parts_cfg.update(vehicle.get_part_config()) parts_cfg.save('{}/parts_cfg.json'.format(beamng.home)) vehicle.save() bng.close() # reload scenario with saved config random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') loaded_config = state_cfg.load("{}/state_cfg.json".format(beamng.home)) scenario = Scenario('west_coast_usa', 'research_test', description='Random driving for research') vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') vehicle = setup_sensors(vehicle) vehicle.set_part_config(parts_cfg) scenario.add_vehicle(vehicle, pos=tuple(state_cfg.pos), rot=None, rot_quat=beamngpy.angle_to_quat(state_cfg.dir)) scenario.make(beamng) bng = beamng.open(launch=True) bng.spawn_vehicle(vehicle, pos=tuple(state_cfg.pos), rot=None, rot_quat=beamngpy.angle_to_quat(state_cfg.dir)) # TODO: debug scenario restart # scenario.restart() try: bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt vehicle.load() # Send random inputs to vehicle and advance the simulation 20 steps #for _ in range(1024): for _ in range(30): throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1.0, 1.0) brake = random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) # bng.step(20) bng.step(20) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) print('\n{} seconds passed.'.format(sensors['timer']['time'])) print("step in loop {}".format(_)) finally: sensors = bng.poll_sensors(vehicle) for s in sensors.keys(): print("{} : {}".format(s, sensors[s])) state_cfg.update(sensors['damage']) state_cfg.update(vehicle.state) state_cfg.save('{}/state_cfg.json'.format(beamng.home)) parts_cfg.update(vehicle.get_part_config()) parts_cfg.save('{}/parts_cfg.json'.format(beamng.home)) bng.close()
def run_scenario_ai_version(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config='vehicles/hopper/custom.pc'): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec, prev_error random.seed(1703) setup_logging() home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.research.v1.7.0.1untouched/BeamNG.research.v1.7.0.1' #'H:/BeamNG.tech.v0.21.3.0' # beamng = BeamNGpy('localhost', 64256, home=home) #, user='******') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='AI', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # create vehicle to be chased # chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red') # bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None, # rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666)) # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() ai_line, bng = create_ai_line_from_road(spawn, bng) # ai_line, bng = create_ai_line_from_centerline(bng) # ai_line, bng = create_ai_line(bng) vehicle.ai_set_script(ai_line, cling=True) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # bng.resume() # vehicle.ai_set_mode('chase') # vehicle.ai_set_target('chase_vehicle') # vehicle.ai_set_mode("traffic") # vehicle.ai_set_speed(12, mode='set') # vehicle.ai_drive_in_lane(True) damage_prev = None runtime = 0.0 traj = [] kphs = [] # with open("ai_lap_data.txt", 'w') as f: for _ in range(1024): sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') damage = sensors['damage'] wheelspeed = sensors['electrics']['wheelspeed'] new_damage = diff_damage(damage, damage_prev) damage_prev = damage runtime = sensors['timer']['time'] vehicle.update_vehicle() traj.append(vehicle.state['pos']) # f.write("{}\n".format(vehicle.state['pos'])) kphs.append(ms_to_kph(wheelspeed)) if new_damage > 0.0: break # if distance(spawn['pos'], vehicle.state['pos']) < 3 and sensors['timer']['time'] > 90: # reached_start = True # plt.imshow(image) # plt.show() # break bng.step(1) bng.close() plot_trajectory(traj, "AI Lap") results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': image } return results
def run_scenario(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config=None): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec global integral, prev_error integral = 0.0 prev_error = 0.0 # setup DNN model + weights # m = Model() # model = m.define_model_BeamNG("BeamNGmodel-racetrack.h5") random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean') scenario = Scenario(default_scenario, 'research_test') # unperturbed_vehicle = Vehicle('unperturbed_vehicle', model=vehicle_model, # licence='SAFE', color='Red') # unperturbed_vehicle = setup_sensors(unperturbed_vehicle) vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='EGO', color=default_color) vehicle = setup_sensors(vehicle) spawn = get_spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) temp = copy.deepcopy(spawn['pos']) temp = [temp[0] + lanewidth, temp[1] + lanewidth, temp[2]] # scenario.add_vehicle(unperturbed_vehicle, pos=temp, rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) bng = beamng.open(launch=True) #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() #bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() # perturb vehicle vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() damage = 0 runtime = 0 while damage <= 0: sensors = bng.poll_sensors(vehicle) headlight_img = sensors['headlight_cam']['colour'].convert('RGB') # sensors[''] # prediction = model.predict() # steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0) # vehicle.control(throttle=throttle, steering=steering, brake=brake) return_str = '\nPERTURBED headlight_cam INFO:' print('\nPERTURBED headlight_cam INFO:') temp = bng.poll_sensors(vehicle)['headlight_cam'] for key in temp: if key == 'rotation': degs = euler_from_quaternion(temp[key][0], temp[key][1], temp[key][2], temp[key][3]) return_str = "{}\nquaternions {}".format(return_str, temp[key]) return_str = "{}\n{} {}".format(return_str, key, [round(i, 3) for i in degs]) print(key, degs) elif key != "colour" and key != "annotation" and key != "depth": return_str = "{}\n{} {}".format(return_str, key, temp[key]) print(key, temp[key]) print('\nPERTURBED rearview_cam INFO:') return_str = "{}\nPERTURBED rearview_cam INFO:".format(return_str) # temp = bng.poll_sensors(vehicle)['rearview_cam'] for key in temp: if key == 'rotation': degs = euler_from_quaternion(temp[key][0], temp[key][1], temp[key][2], temp[key][3]) return_str = "{}\nquaternions {}".format(return_str, temp[key]) return_str = "{}\n{} {}".format(return_str, key, [round(i, 3) for i in degs]) print(key, degs) elif key != "colour" and key != "annotation" and key != "depth": return_str = "{}\n{} {}".format(return_str, key, temp[key]) print(key, temp[key]) # rearview_img = bng.poll_sensors(vehicle)['rearview_cam']['colour'].convert('RGB') headlight_img = sensors['headlight_cam']['colour'].convert('RGB') bng.step(1) damage = sensors['damage']['damage'] runtime = sensors['timer']['time'] # print("runtime:{}".format(round(runtime, 2))) print("time to crash:{} damage:{}".format(round(runtime, 2), damage)) bng.close() # avg_kph = float(sum(kphs)) / len(kphs) # plt.imshow(rearview_img) # plt.pause(0.01) plt.imshow(headlight_img) plt.pause(0.01) # results = {'pitch': round(pitch,3), 'roll':round(roll,3), "z":round(z,3), 'rearview_img':rearview_img, 'headlight_img':headlight_img} return return_str
def run_scenario_ai_version(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config=None): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec global prev_error random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean', user="******") scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='AI', color=default_color) vehicle = setup_sensors(vehicle) spawn = get_spawn_point(default_scenario, default_spawnpoint) # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # create vehicle to be chased chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red') bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None, rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666)) bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt #bng.resume() # perturb vehicle print("vehicle position before deflation via beamstate:{}".format( vehicle.get_object_position())) print("vehicle position before deflation via vehicle state:{}".format( vehicle.state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') plt.imshow(image) plt.pause(0.01) vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state)) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') plt.imshow(image) plt.pause(0.01) bng.resume() vehicle.ai_set_mode('chase') vehicle.ai_set_target('chase_vehicle') vehicle.ai_drive_in_lane(True) damage_prev = None runtime = 0.0 traj = [] kphs = [] for _ in range(650): image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') damage = bng.poll_sensors(vehicle)['damage'] wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed'] new_damage = diff_damage(damage, damage_prev) damage_prev = damage runtime = bng.poll_sensors(vehicle)['timer']['time'] vehicle.update_vehicle() traj.append(vehicle.state['pos']) kphs.append(ms_to_kph(wheelspeed)) if new_damage > 0.0: break bng.step(5) bng.close() results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': image } return results
def test_vehicle_ai(beamng): with beamng as bng: bng.set_deterministic() scenario = Scenario('west_coast_usa', 'ai_test') vehicle = Vehicle('test_car', model='etk800') other = Vehicle('other', model='etk800') pos = [-717.121, 101, 118.675] scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45)) scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45)) scenario.make(bng) bng.load_scenario(scenario) bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_waypoint('Bridge4_B') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='chase') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='flee') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() script = [ { 'x': -735, 'y': 86.7, 'z': 119, 't': 0 }, { 'x': -752, 'y': 70, 'z': 119, 't': 5 }, { 'x': -762, 'y': 60, 'z': 119, 't': 8 }, ] vehicle.ai_set_script(script) bng.step(600, wait=True) vehicle.update_vehicle() ref = [script[1]['x'], script[1]['y'], script[1]['z']] pos = vehicle.state['pos'] ref, pos = np.array(ref), np.array(pos) assert np.linalg.norm(ref - pos) < 2.5 scenario.delete(beamng)
def getOob(oobList, test, port): # Gains to port TORCS actuators to BeamNG # steering_gain = translate_steering() acc_gain = 0.5 # 0.4 brake_gain = 1.0 # BeamNG images are too bright for DeepDrive brightness = 0.4 # Set up first vehicle # ! A vehicle with the name you specify here has to exist in the scenario vehicle = Vehicle('egovehicle') scenario = Scenario('asfault', 'asfault', True) # Set up sensors resolution = (280, 210) # Original Settings #pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below # 0.4 is inside pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below direction = (0, 1, 0) # direction = (180, 0, 180) # FOV 60, MAX_SPEED 100, 20 (3) Hz fails # FOV 60, MAX_SPEED 80, 20 (3) Hz Ok # FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations # FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations # FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower # FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower fov = 60 # MAX_SPEED = 70 MAX_FPS = 60 SIMULATION_STEP = 6 # Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes ! # # WORKING SETTINGS: 20 Freq, 90 FOV. front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) electrics = Electrics() vehicle.attach_sensor('front_cam', front_camera) vehicle.attach_sensor('electrics', electrics) with open(pathToScenarioBeamng + '/asfault.json', 'r') as f: jsonScenario = json.load(f) with open(pathToScenarioBeamng + '/asfault.lua', 'r') as f: luaScenario = f.readlines() with open(pathToScenarioBeamng + '/asfault.prefab', 'r') as f: prefabScenario = f.readlines() with open(pathToScenarioDocuments + '/asfault.json', 'w') as outfile: json.dump(jsonScenario, outfile) with open(pathToScenarioDocuments + '/asfault.lua', 'w') as outfile: outfile.writelines(luaScenario) with open(pathToScenarioDocuments + '/asfault.prefab', 'w') as outfile: outfile.writelines(prefabScenario) deep_drive_engaged = True STATE = "NORMAL" notFinished = True portCount = 1 multiplier = 2 turtleMode = False turtleSpeed = 10 offRoad = False oobSegKey = None oobSearch = False print(oobList) speedList = [] currentSpeed = oobList[0][0][0] for speed in oobList[0][0]: speedList.append(speed) if speed > currentSpeed: currentSpeed = speed currentSpeed = currentSpeed * 3.6 waypointList = [] resultList = [] oobWaypoint = [] newOobWaypointList = [] for key in oobList[0][1]: waypointList.append(key) print(waypointList) for waypoint in waypointList: for key in waypointList: print(key) for k in key: oobWaypoint.append(k) resultList.append(k) while notFinished: # Connect to running beamng setup_logging() beamng = BeamNGpy('localhost', port) bng = beamng.open(launch=True) scenario.make(bng) bng.load_scenario(scenario) try: bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Connect to the existing vehicle (identified by the ID set in the vehicle instance) bng.connect_vehicle(vehicle, port + portCount) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt testDic = RoadTest.to_dict(test) parentage = testDic['network']['parentage'] for parent in parentage: roadKeys = [] for keys in parentage: for key in keys: roadKeys.append(key) nodesDict = testDic['network']['nodes'] nodes = [] for node in nodesDict: nodes.append(node) vehicle.update_vehicle() oobCount = 0 inOob = False vehicle.ai_set_speed(70) if oobSearch: if turtleMode: currentSpeed = turtleSpeed else: currentSpeed = currentSpeed - 10 print(currentSpeed) running = True while running: # Resume the execution # 6 steps correspond to 10 FPS with a resolution of 60FPS # 5 steps 12 FPS # 3 steps correspond to 20 FPS #bng.step(SIMULATION_STEP) # # TODO: Is there a way to query for the speed directly ? #speed = math.sqrt(vehicle.state['vel'][0] * vehicle.state['vel'][0] + vehicle.state['vel'][1] * vehicle.state['vel'][1]) # Speed is M/S ? # print("Speed from BeamNG is: ", speed, speed*3.6) #vehicle.ai_drive_in_lane(True) vehicle.update_vehicle() tup = get_segment(test, vehicle.state) currentSegmentDict = None currentSegKey = None if tup is not None: currentSegmentDict = NetworkNode.to_dict(tup) currentSegKey = currentSegmentDict['key'] if offRoad: if oobSegKey in resultList: index = resultList.index(oobSegKey) lostSeg = resultList.pop(index) print('resList without seg', resultList) print(lostSeg) if oobSegKey not in newOobWaypointList: newOobWaypointList.append(oobSegKey) print('new OOB list', newOobWaypointList) offRoad = False if inOob and off_track(test, vehicle.state): print('off track') offRoad = True oobCount = oobCount + 1 if currentSegKey in oobWaypoint: vehicle.ai_drive_in_lane(True) inOob = True oobSegKey = currentSegKey vehicle.ai_set_speed(currentSpeed) else: inOob = False vehicle.ai_set_speed(70) vehicle.ai_set_waypoint('waypoint_goal') vehicle.ai_drive_in_lane(True) if goal_reached(test, vehicle.state): print(turtleMode) for res in resultList: segOobDict = testDic['execution']['seg_oob_count'] segOobDict[res] = currentSpeed / 3.6 if oobCount == 0: testDic['execution']['oobs'] = 0 print('waypointlist', newOobWaypointList) print('resultlist', resultList) test = RoadTest.from_dict(testDic) return test else: running = False portCount = portCount + 1 multiplier = multiplier + 1 oobSearch = True oobWaypoint = newOobWaypointList newOobWaypointList = [] if currentSpeed < 20: print(currentSpeed) print('NewoobWay: ', newOobWaypointList) print('resultlist: ', resultList) if turtleMode: for res in oobWaypoint: print(res) segOobDict = testDic['execution'][ 'seg_oob_count'] segOobDict[res] = 12 testDic['execution']['oobs'] = 0 test = RoadTest.from_dict(testDic) print(segOobDict) print(resultList) running = False oobCount = 0 return test else: turtleMode = True print('mode on') bng.close() finally: if oobCount == 0: bng.close() else: print('restart')
class BeamNG_ACC_Test(): def __init__(self, **kwargs): keys = kwargs.keys() if "test_controller" in keys: self.controller = kwargs.get("test_controller") else: self.controller = ACC_Test() config = BeamNG_Cruise_Control.Config(KP=CC_P, KI=CC_I, KD=CC_D, target=ACC_CAR_SPEED) config2 = BeamNG_Cruise_Control.Config(KP=CC_P, KI=CC_I, KD=CC_D, target=FRONT_CAR_SPEED) self.PID = BeamNG_Cruise_Control.PID_Controller(config=config) self.PID2 = BeamNG_Cruise_Control.PID_Controller(config=config2) def setup_bngServer(self): # Instantiate BeamNGpy instance running the simulator from the given path, if ('BNG_HOME' in os.environ): self.bng = BeamNGpy('localhost', 64256, home=os.environ['BNG_HOME']) else: print( "WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)" ) self.bng = BeamNGpy( 'localhost', 64256, home='C:\\Users\\Ingars\\Beamng-unlimited\\trunk') def setup_BeamNG(self): # Create a scenario in test map self.scenario = Scenario('cruise-control', 'example') # Create an ETK800 with the licence plate 'PID' self.vehicle = Vehicle('ACC', model='etk800', licence='ACC') electrics = Electrics() self.vehicle.attach_sensor('electrics', electrics) # Cars electric system # Add it to our scenario at this position and rotation self.scenario.add_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 0)) self.vehicle2 = Vehicle('FRONT', model='etk800', licence='FRONT') self.vehicle2.attach_sensor('electrics', electrics) # Cars electric system self.scenario.add_vehicle(self.vehicle2, pos=FRONT_CAR_POS, rot=(0, 0, 180)) # Place files defining our scenario for the simulator to read self.scenario.make(self.bng) # Launch BeamNG.research self.bng.open() # Load and start our scenario self.bng.load_scenario(self.scenario) self.bng.start_scenario() def runTest(self): self.bng.restart_scenario() self.bng.teleport_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 180)) self.run() def run(self): self.setupTest() wanted_t = ACC_CAR_SPEED wanted_d = WANTED_DISTANCE soft_brake_d = wanted_d - (wanted_d / 2) wanted_d_buffer = wanted_d - (wanted_d / 5) # Wanted distance has a buffer of 25%, to compensate holding same constant speed at bigger distance deacc_d = wanted_d_buffer + 10 # +10 meters is buffer for starting to decelerate towards front car speed and wanted distance. # the buffer should be calculated dynamically instead to compensate for different speeds while self.controller.ended == False: start_time = datetime.datetime.now() sensors = self.bng.poll_sensors(self.vehicle) position = self.vehicle.state['pos'] position2 = self.vehicle2.state['pos'] sensors2 = self.bng.poll_sensors( self.vehicle2 ) # replace with self.vehicle.update_vehicle when not using pre-generated coordinates of vehicle to follow wheel_speed = sensors['electrics']['values']['wheelspeed'] distance = self.controller.setDistance(position, position2) front_car_speed = self.controller.getFrontCarSpeed(wheel_speed) curr_target = self.PID.get_target() print("distance: " + str(distance)) #FORMULA USED for deacceleration = front_car_speed^2 = wheel_speed^2 + (distance-20)*2*a #d = (front_car_speed - wheel_speed) / (-0.3 * 2) # 0.3 deacceleration? distance for reducing speed to front cars speed with -0.3 acceleration #print("d: " + str(d)) if distance < 5: # 5 is manually set as last allowed distance between both cars, will not work good if ACC car is driving too fast. Would be better to calculate it as braking distance. print("brake1") self.vehicle.control(brake=1) self.vehicle.control(throttle=0) elif distance < soft_brake_d: print("brake1") self.vehicle.control( brake=0.1 ) #Softness of brake could also be calculated dynamically to compensate different speeds self.vehicle.control(throttle=0) else: if distance <= wanted_d_buffer: print("wanted") if front_car_speed > curr_target + 3.5 or front_car_speed < curr_target - 2: #or front_car_speed < curr_target - 1 // calibrate 3.5 and 2 self.PID.change_target(max(front_car_speed, 0)) elif distance <= deacc_d: a = (front_car_speed - wheel_speed) / ( (distance - wanted_d_buffer) * 2) print("a:" + str(a)) self.PID.change_target(a + wheel_speed) elif curr_target != wanted_t: self.PID.change_target(wanted_t) print("throttle1") value = self.PID.calculate_throttle(wheel_speed) value = min(1, value) self.vehicle.control(brake=0) self.vehicle.control(throttle=value) #PID for front car wheel_speed2 = sensors2['electrics']['values']['wheelspeed'] # print("real front:" + str(wheel_speed2)) value2 = self.PID2.calculate_throttle(wheel_speed2) value2 = min(1, value2) self.vehicle2.control(brake=0) self.vehicle2.control(throttle=value2) elapsed_time = datetime.datetime.now() - start_time while (elapsed_time.total_seconds() * 1000) < 100: elapsed_time = datetime.datetime.now() - start_time elapsed_total = self.controller.calculateElapsed() # Change front car speed after 10 seconds and after 20 seconds if elapsed_total.total_seconds() > float(10): self.PID2.change_target(20) if elapsed_total.total_seconds() > float(20): self.PID2.change_target(10) print("Ending Test") def close(self): self.bng.close() def setupTest(self): self.vehicle.update_vehicle() self.controller.last_position = self.vehicle.state['pos'] self.controller.start()
def GenerateTrack(trackLength, sampleRate, show, startBeamNG): populationSize = 100 maxAcc = 1 times = 20 relNewSize = 0.6 duplicatesThreshold = 0.05 intersectionDelta = 0.01 mutationProb = 0.1 mutationDeviation = 0.01 print("Generating Tracks") pop = initPop(populationSize, trackLength, maxAcc, 20) pop = evolve(pop, times, relNewSize, duplicatesThreshold, intersectionDelta, mutationProb, mutationDeviation, 10) print("eliminating intersecting tracks") pop = elminateIntersecting(pop, intersectionDelta) if show: plotTrack(pop, 100) tracks = [] nr = 0 first = True for track in pop: track = np.vstack((track, completeAcc(track))) poly = polysFromAccs(track) bez = bezFromPolys(poly) smpl = sampleBezSeries(bez, sampleRate).transpose() smpl = scaleTrack(smpl, 100, 100) smpl = np.array(smpl) smpl = np.vstack((smpl, [smpl[0]])) if(first): writeCriteria(smpl) first = False tracks.append(smpl) writeTrack(smpl, nr) nr += 1 if startBeamNG: nodes = [] for p in tracks[0]: nodes.append((p[0], p[1], 0, 7)) beamng = BeamNGpy('localhost', 64256) vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Green') scenario = Scenario('GridMap_v0422', 'track test') scenario.add_vehicle(vehicle, pos=(0, 0, -16.64), rot=(0, 0, 180)) road = Road(material='track_editor_C_center', rid='main_road', texture_length=5, looped=True) road.nodes.extend(nodes) scenario.add_road(road) scenario.make(beamng) beamng.open() beamng.load_scenario(scenario) beamng.start_scenario() vehicle.ai_set_mode('span') while 1: vehicle.update_vehicle() print(vehicle.state['pos']) sleep(1)
def main(): beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN') scenario = Scenario('GridMap', 'road_test') road_a = Road('custom_track_center', looped=False) nodes = [ (0, 0, 0, 4), (0, 400, 0, 4), ] road_a.nodes.extend(nodes) scenario.add_road(road_a) vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black') scenario.add_vehicle(vehicle, pos=(0, 0, 0.163609), rot=(0, 0, 180)) # 0.163609 , scenario.make(beamng) script = list() node0 = { 'pos': (0, 0, 0.163609), 'speed': 20, } node1 = { 'pos': (0, 100, 0.163609), 'speed': 30, } script.append(node0) script.append(node1) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_line(script) # update the state of vehicle at impact. for _ in range(100): time.sleep(0.1) vehicle.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors( vehicle ) # Polls the data of all sensors attached to the vehicle print(vehicle.state['pos']) if vehicle.state['pos'][1] > 99: print('free state') vehicle.control(throttle=0, steering=0, brake=0, parkingbrake=0) vehicle.update_vehicle() bng.stop_scenario() for _ in range(20): time.sleep(0.1) bng.load_scenario(scenario) bng.start_scenario() node0 = { 'pos': (0, 50, 0.163609), 'speed': 20, } node1 = { 'pos': (0, 100, 0.163609), 'speed': 30, } script.append(node0) script.append(node1) vehicle.ai_set_line(script) input('Press enter when done...') finally: bng.close()
def createCrashSimulation(): print("Crash Simulation") beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN') scenario = Scenario('GridMap', 'road_crash_simulation') road_a = Road('track_editor_C_center', looped=False) road_b = Road('track_editor_C_center', looped=False) nodesa = [(30, 0, -4, 4), (0, 0, -4, 4)] nodesb = [(0, 30, -4, 4), (0, 0, -4, 4)] road_a.nodes.extend(nodesa) road_b.nodes.extend(nodesb) # Create an ETK800 with the licence plate 'PYTHON' vehicleA = Vehicle('ego_vehicleA', model='etk800', licence='PYTHON') # Add it to our scenario at this position and rotation damageS = Damage() vehicleA.attach_sensor('damagesS', damageS) scenario.add_vehicle(vehicleA, pos=(30, 0, 0), rot=(0, 0, 90)) # Create an ETK800 with the licence plate 'PYTHON' vehicleB = Vehicle('ego_vehicleB', model='etk800', licence='PYTHON') # Add it to our scenario at this position and rotation damageV = Damage() vehicleB.attach_sensor('damagesV', damageV) scenario.add_vehicle(vehicleB, pos=(0, 30, 0), rot=(0, 0, 0)) scenario.add_road(road_a) scenario.add_road(road_b) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() node0 = { 'pos': (30, 0, 0), 'speed': 0, } node1 = { 'pos': (0, 0, 0), 'speed': 20, } script = list() script.append(node0) script.append(node1) vehicleA.ai_set_line(script) node3 = { 'pos': (0, 30, 0), 'speed': 0, } node4 = { 'pos': (0, 0, 0), 'speed': 20, } script = list() script.append(node3) script.append(node4) vehicleB.ai_set_line(script) time.sleep(12) # input('Press enter when done...') vehicleA.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicleA) damage_striker = sensors['damagesS'] print(sensors['damagesS']) print(vehicleA.state['pos']) vehicleB.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicleB) damage_victim = sensors['damagesV'] print(sensors['damagesV']) print(vehicleB.state['pos']) multiObjectiveFitnessFunction(123456789, damage_striker, vehicleA.state['pos'], (0, 0), damage_victim, vehicleB.state['pos'], (0, 0)) # multiobjective fitness function. bng.stop_scenario() # bng.load_scenario(scenario) # bng.start_scenario() # # print("sleep") # time.sleep(10) # print("wake") # # node0 = { # 'pos': (30, 0, 0), # 'speed': 0, # } # # node1 = { # 'pos': (0, 0, 0), # 'speed': 30, # } # # script = list() # script.append(node0) # script.append(node1) # # vehicleA.ai_set_line(script) # # node0 = { # 'pos': (0, 30, 0), # 'speed': 0, # } # # node1 = { # 'pos': (0, 0, 0), # 'speed': 30, # } # # script = list() # script.append(node0) # script.append(node1) # # vehicleB.ai_set_line(script) # # input('Press enter when done...') finally: bng.close()
def run_spawn_for_deflation(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0]): global base_filename, default_color, default_scenario, setpoint global prev_error global fcam vehicle_loadfile = 'vehicles/hopper/crawler.pc' # setup DNN model + weights m = Model() model = m.define_model_BeamNG("BeamNGmodel-5.h5") random.seed(1703) setup_logging() # beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='LOWPRESS', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, 'highway') # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) # bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # load part config #pc = vehicle.get_part_config() # loadfile = 'C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json' # vehicle.load(loadfile) bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig='vehicles/hopper/custom.pc') #bng.set_relative_camera() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # vehicle.control(throttle=0.0, steering=0.0, brake=1.0) # perturb vehicle # before_state = copy.deepcopy(vehicle.state) before_state = bng.poll_sensors(vehicle)['front_cam'] before_state['vel'] = vehicle.state['vel'] before_state['roll'] = vehicle.state['roll'] before_state['pitch'] = vehicle.state['pitch'] before_state['yaw'] = vehicle.state['yaw'] # print("vehicle position before deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position before deflation via vehicle state:{}".format(vehicle.state)) print( "vehicle position before deflation via camera:{}".format(before_state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # print("camera sensor before deflation: {}".format(bng.poll_sensors(vehicle)['front_cam'])) #get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up']) img = m.process_image(image) before_prediction = model.predict(img) before_state["prediction"] = before_prediction plt.imshow(image) plt.pause(0.01) if deflation_pattern != [0, 0, 0, 0]: print("deflating according to pattern {}".format(deflation_pattern)) vehicle.deflate_tires(deflation_pattern) time.sleep(1) bng.resume() bng.set_steps_per_second(100) bng.set_deterministic() totalsecs = 0.0 deflation_traj = [] while totalsecs <= 30: vehicle.update_vehicle() # vehicle.control(throttle=0.0, steering=0.0, brake=1.0) after_state = bng.poll_sensors(vehicle)['front_cam'] after_state['vel'] = vehicle.state['vel'] after_state['roll'] = vehicle.state['roll'] after_state['pitch'] = vehicle.state['pitch'] after_state['yaw'] = vehicle.state['yaw'] print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'], vehicle.state['pitch'], vehicle.state['yaw'])) deflation_traj.append(round(math.degrees(vehicle.state['pitch'][0]), 4)) bng.step(100) totalsecs += 1 # after_state = copy.deepcopy(vehicle.state) # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') print("vehicle position after deflation via camera:{}".format(after_state)) #print("camera sensor output: {}".format(bng.poll_sensors(vehicle)['front_cam']['rot'])) #print("camera pos output: {}".format(bng.poll_sensors(vehicle)['front_cam']['pos'])) # print("camera rot output: {}".format(bng.poll_sensors(vehicle)['front_cam']['direction'])) # print("fcam.encode_engine_request() = {}".format(fcam.encode_engine_request())) damages = bng.poll_sensors(vehicle)['damage']['deform_group_damage'] d = ["{}={}".format(k, damages[k]['damage']) for k in damages.keys()] print("vehicle pressure after deflation: lowpressure={} damages:".format( bng.poll_sensors(vehicle)['damage']['lowpressure'], d)) img = m.process_image(image) after_state["prediction"] = model.predict(img) plt.imshow(image) plt.pause(0.01) bng.close() return before_state, after_state, deflation_traj
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256, home='D:/Programs/BeamNG/trunk') #scenario = Scenario('west_cost_usa', 'ai_sine') scenario = Scenario('smallgrid', 'ai_sine') vehicle = Vehicle('ego_vehicle', model='etk800', license='AI') #orig = (-769.1, 400.8, 142.8) orig = (-10, -1309, 0.215133) #smallgrid #scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, 180)) scenario.add_vehicle(vehicle, pos=orig, rot=(1, 0, 0)) #smallgrid scenario.make(beamng) #Run simulation without emg_processing_time to obtain target trajectory. bng = beamng.open(launch=True) count = 0 index = 0 vehicle_dynamics = [] try: print('Running simulation with target trajectory.') bng.load_scenario(scenario) bng.start_scenario() #Set throttle to maintain speed near 7 km/h during turn. vehicle.control(throttle=0.04) start_time = time.time() while True: bng.step(1) vehicle.update_vehicle() if vehicle.state['pos'][1] <= -1319 and count == 0: vehicle.control(steering=-1.0) count += 1 if vehicle.state['pos'][1] >= -1319 and count == 1: break #Store vehicle dynamics. vehicle_dynamics.append(','.join( map(str, [ index, time.time() - start_time, math.sqrt(vehicle.state['vel'][0]**2 + vehicle.state['vel'][1]**2), vehicle.state['pos'][0], vehicle.state['pos'][1] ]))) print('Time:' + str(time.time() - start_time) + 'Position: ' + str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str( math.sqrt(vehicle.state['vel'][0]**2 + vehicle.state['vel'][1]**2))) index += 1 finally: bng.close() #Save target trajectory results. heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate'] with open('sim_target_trajectory_left_uturn.txt', "w") as results: heading = map(str, heading) results.write(",".join(heading) + '\n' + "\n".join(str(element) for element in vehicle_dynamics)) results.close() #Read total EMG signal processing time (seconds/feature) per trial from txt files. f = [ line.rstrip('\n') for line in open( 'D:/Research Projects/sEMG_control_for_automobiles/putemg-downloader/putemg_examples/total_processing_time_wrist_flexion.txt' ) ][1:] total_EMG_processing_time = [] for line in f: total_EMG_processing_time.append(float(line)) #Run simulations that delay steering initiation based based on EMG signal processing time from trials. for emg_processing_time in range(len(total_EMG_processing_time)): #for emg_processing_time in range(0,1): print( '\n' + 'Running simulations with EMG signal processing time from trial ' + str(emg_processing_time) + '.') bng = beamng.open(launch=True) count = 0 index = 0 vehicle_dynamics = [] try: bng.load_scenario(scenario) bng.start_scenario() #Set throttle to maintain speed near 7 km/h during turn. vehicle.control(throttle=0.04) start_time = time.time() while True: bng.step(1) vehicle.update_vehicle() if vehicle.state['pos'][1] <= -1319 and count == 0: sleep(total_EMG_processing_time[emg_processing_time]) vehicle.control(steering=-1.0) count += 1 if vehicle.state['pos'][1] >= -1319 and count == 1: break #Store vehicle dynamics. vehicle_dynamics.append(','.join( map(str, [ index, time.time() - start_time, math.sqrt(vehicle.state['vel'][0]**2 + vehicle.state['vel'][1]**2), vehicle.state['pos'][0], vehicle.state['pos'][1] ]))) print('Time:' + str(time.time() - start_time) + 'Position: ' + str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str( math.sqrt(vehicle.state['vel'][0]**2 + vehicle.state['vel'][1]**2))) index += 1 finally: bng.close() heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate'] with open('sim_results_trial_' + str(emg_processing_time) + '.txt', "w") as results: heading = map(str, heading) results.write(",".join(heading) + '\n' + "\n".join( str(element) for element in vehicle_dynamics)) results.close()
node3 = { 'pos': (collision_points[0][0], collision_points[0][1], 0), 'speed': victim_speeds[0], } script = list() script.append(node2) script.append(node3) vehicleVictim.ai_set_line(script) for number in range(60): time.sleep(0.20) vehicleStriker.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors( vehicleStriker ) # Polls the data of all sensors attached to the vehicle #print(vehicleStriker.state['pos']) if vehicleStriker.state['pos'][0] > 306 and vehicleStriker.state[ 'pos'][1] > 39: # print('free state') vehicleStriker.control(throttle=0, steering=0, brake=0, parkingbrake=0) vehicleStriker.update_vehicle() vehicleVictim.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator
def scenario(beamNG_path): if (beamNG_path == ""): beamNG_path = 'D:/BeamNGReasearch/Unlimited_Version/trunk' bng = BeamNGpy('localhost', 64256, beamNG_path) #scenario = Scenario('west_coast_usa', 'example') scenario = Scenario('GridMap', 'example') # Create an ETK800 with the licence plate 'PYTHON' vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red') vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue') electrics = Electrics() vehicle1.attach_sensor('electrics', electrics) pos2 = (-13.04469108581543, -107.0409164428711, 0.202297180891037) pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775) rot2 = (0.0009761620895005763, -0.9999936819076538, -0.0034209610894322395) rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936) scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1) scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2) scenario.make(bng) # Launch BeamNG.research bng.open(launch=True) SIZE = 500 try: bng.load_scenario(scenario) bng.start_scenario() vehicle1.ai_set_speed(10.452066507339481,mode = 'set') vehicle1.ai_set_mode('span') vehicle2.ai_set_mode('chase') #collect data positions = list() directions = list() wheel_speeds = list() for _ in range(100): time.sleep(0.1) vehicle1.update_vehicle() # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicle1) # Polls the data of all sensors attached to the vehicle positions.append(vehicle1.state['pos']) directions.append(vehicle1.state['dir']) wheel_speeds.append(sensors['electrics']['values']['wheelspeed']) print('The Final result - position :') print(positions) print('The Final result - direction :') print(directions) print('The Final result - speed :') print(wheel_speeds) bng.stop_scenario() bng.close() finally: bng.close()
# display(sensors['electrics']['values']['brake']) # print('The Damage is:') # display(sensors['damages']) # display(damage) positions = list() directions = list() wheel_speeds = list() throttles = list() brakes = list() damages = list() for _ in range(150): time.sleep(0.1) vehicle.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors( vehicle) # Polls the data of all sensors attached to the vehicle positions.append(vehicle.state['pos']) directions.append(vehicle.state['dir']) wheel_speeds.append(sensors['electrics']['values']['wheelspeed']) throttles.append(sensors['electrics']['values']['throttle']) brakes.append(sensors['electrics']['values']['brake']) damages.append(sensors['damages']) bng.close() #print(wheel_speeds) # print(directions) # print(damages[-1])
def launch(beamNGPath, car1, car2, speed_car2): crash = False dist_2car = 20 speed_car2 = int(speed_car2) bng = BeamNGpy('localhost', 64256, beamNG_path) #bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk') scenario = Scenario('GridMap', 'example') # Create an ETK800 with the licence plate 'PYTHON' vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red') vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue') electrics1 = Electrics() electrics2 = Electrics() vehicle1.attach_sensor('electrics1', electrics1) vehicle2.attach_sensor('electrics2', electrics2) #position to try drive then teleport #-365.2436828613281, -214.7460479736328, 1.2118444442749023], [0.9762436747550964, 0.20668958127498627, 0.0650215595960617]] #[[-362.4477844238281, -214.16226196289062, 1.32931387424469], [0.9824153780937195, 0.1852567195892334, 0.023236412554979324]] pos2 = (25.75335693359375, -127.78406524658203, 0.2072899490594864) pos1 = (-88.8136978149414, -261.0204162597656, 0.20253072679042816) #pos_tel1 = (53.35311508178711, -139.84017944335938, 0.20729705691337585) #change this #pos_tel2 = (75.94310760498047, -232.62135314941406, 0.20568031072616577) #change this pos_tel1 = car1[0] pos_tel2 = car2[0] rot2 = (0.9298766851425171, -0.36776003241539, 0.009040255099534988) rot1 = (-0.9998571872711182, 0.016821512952446938, -0.0016229493776336312) # rot_tel1= (0.8766672611236572, -0.4810631573200226, 0.005705998744815588) #change this # rot_tel2 = (-0.896364688873291, -0.4433068335056305, -0.0030648468527942896) #change this rot_tel1 = car1[1] rot_tel2 = car2[1] #initial postion of two car. # run 2cars on particular road until they reach particular speed which satisfies the condisiton of the given testcase scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1) scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2) scenario.make(bng) # Launch BeamNG.research bng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() vehicle1.ai_set_speed(10, mode='set') vehicle1.ai_set_mode('span') vehicle2.ai_set_speed(speed_car2, mode='set') ##change this param of speed vehicle2.ai_set_mode('chase') sys_output.print_sub_tit("Initial Position and rotation of car") print("\nInitial Position and rotation of car1 %s %s " % (pos1, rot1)) print("\nInitial Position and rotation of car2 %s %s " % (pos2, rot2)) sys_output.print_sub_tit( "\n when speed of car 1 rearch 10 and speed car 2 reach %s. Two cars are teleport to new locations." % (speed_car2)) for _ in range(450): time.sleep(0.1) vehicle1.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator vehicle2.update_vehicle() sensors1 = bng.poll_sensors( vehicle1 ) # Polls the data of all sensors attached to the vehicle sensors2 = bng.poll_sensors(vehicle2) speed1 = sensors1['electrics1']['values']['wheelspeed'] speed2 = sensors2['electrics2']['values']['wheelspeed'] print("speed of car 1", speed1) print("speed of car 2", speed2) #print([vehicle1.state['pos'],vehicle1.state['dir']]) if speed1 > 9 and speed2 > speed_car2 - 1: #change speed here bng.teleport_vehicle(vehicle1, pos_tel1, rot_tel1) bng.teleport_vehicle(vehicle2, pos_tel2, rot_tel2) sys_output.print_sub_tit("Teleport 2 car to new location") print("\n Car1 : " + str(pos_tel1) + ", " + str(rot_tel1)) print("\n Car2 : " + str(pos_tel2) + ", " + str(rot_tel2)) print("\n Distance between two cars: " + str(distance.euclidean(pos_tel1, pos_tel2))) break # if speed > 19: # bng.teleport_vehicle(vehicle1,pos_tel,rot_tel ) # break for _ in range(100): #pos1 = [] time.sleep(0.1) vehicle1.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator vehicle2.update_vehicle() sensors1 = bng.poll_sensors( vehicle1 ) # Polls the data of all sensors attached to the vehicle sensors2 = bng.poll_sensors(vehicle2) speed1 = sensors1['electrics1']['values']['wheelspeed'] speed2 = sensors2['electrics2']['values']['wheelspeed'] #pos1.append(vehicle1.state['pos']) #pos2.append(vehicle2.state['pos']) dist_2car = distance.euclidean(vehicle1.state['pos'], vehicle2.state['pos']) if dist_2car < 5: #or int(speed2)== 0 : crash = True print( "\n Failed because distance of two cars are less than 5") break print("\n speed1 %s, speed2: %s, distance: %s" % (speed1, speed2, dist_2car)) if int(speed2) == 0: print("\n Pass because car2 stoped") break bng.stop_scenario() bng.close() finally: bng.close() return crash