def main(): random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256) scenario = Scenario('west_coast_usa', 'lidar_demo', description='Spanning the map with a lidar sensor') vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') lidar = Lidar(offset=(0, 0, 1.6)) vehicle.attach_sensor('lidar', lidar) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=(0, 0, 45)) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution bng.load_scenario(scenario) bng.hide_hud() bng.start_scenario() vehicle.ai_set_mode('span') print('Driving around for 60 seconds...') sleep(60) finally: bng.close()
def setup_beamng(vehicle_model='etk800', camera_pos=(-0.5, 0.38, 1.3), camera_direction=(0, 1.0, 0), pitch_euler=0.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 # version==keras # sm = DAVE2Model() # model = sm.define_dual_model_BeamNG() # model = sm.load_weights("BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2.h5") # model = sm.load_weights("BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-3-sanitycheckretraining-model.h5") # version==pytorch # model = torch.load("C:/Users/merie/Downloads/dave/test_model_20.pt", map_location=torch.device('cpu')).eval() # model = torch.load("F:/MODELS-FOR-MERIEL/models/model-100000D-1000B-100E-1p000000e-04LR-100.pt.pt", map_location=torch.device('cpu')).eval() # model = torch.load("F:/MODELS-FOR-MERIEL/models/model-292757D-1000B-100E-1p000000e-04LR-100.pt", map_location=torch.device('cpu')).eval() model = torch.load( "H:/GitHub/DAVE2-Keras/test7-trad-50epochs-64batch-1e4lr-ORIGDATASET-singleoutput-model.pt", map_location=torch.device('cpu')).eval() 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, pos=camera_pos, direction=camera_direction) 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) add_qr_cubes(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() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # bng.resume() # find_width_of_road(bng) return vehicle, bng, model
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') #beamng.change_setting('research', True) scenario = Scenario('west_coast_usa', 'lidar_tour', description='Tour through the west coast gathering ' 'Lidar data') vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR') lidar = Lidar() vehicle.attach_sensor('lidar', lidar) #beamng.open_lidar('lidar', vehicle, 'shmem', 8000) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795)) scenario.make(beamng) bng = beamng.open(launch=True) #bng.open_lidar('lidar', vehicle, 'shmem', 8000) #lidar.connect(bng, vehicle) try: bng.load_scenario(scenario) # this is where the error happens window = open_window(SIZE, SIZE) lidar_vis = LidarVisualiser(Lidar.max_points) lidar_vis.open(SIZE, SIZE) bng.set_steps_per_second(60) bng.set_deterministic() bng.hide_hud() bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') def update(): sensors = bng.poll_sensors(vehicle) points = sensors['lidar']['points'] bng.step(3, wait=False) lidar_vis.update_points(points, vehicle.state) glutPostRedisplay() glutReshapeFunc(lidar_resize) glutIdleFunc(update) glutMainLoop() except Exception as e: print(e) finally: bng.close()
def beamng(): setup_logging() # Get environment variables BNG_HOME = os.getenv('BNG_HOME') BNG_RESEARCH = os.getenv('BNG_RESEARCH') host = '127.0.0.1' port = 64256 # Instantiates a BeamNGpy instance beamng = BeamNGpy(host, port, BNG_HOME, BNG_RESEARCH) return beamng
def setup_beamng(traffic=2): global sp, beamng_home, beamng_user setup_logging() beamng = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user) scenario = Scenario( 'west_coast_usa', 'lidar_tour', description= 'Tour through highway with variable traffic vehicles gathering ' 'Lidar data') # setup vehicle vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR', color='Red') vehicle = setup_sensors(vehicle) # setup vehicle poses lane = random.choice([1, 2, 3, 4]) ego_sp = spawn_point(sp, lane) ego_pos = ego_sp['pos'] ego_rot_quat = ego_sp['rot_quat'] lane = ego_sp['lane'] # add vehicles to scenario # print("adding vehicle to scenario...") scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat) tvs = traffic_vehicles(traffic) ps = generate_vehicle_positions(ego_pos, ego_rot_quat, lane, tvs) for i in range(len(tvs)): print("adding vehicle {}...".format(i)) scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat) print("making scenario...") scenario.make(beamng) print("opening beamNG...") bng = beamng.open(launch=True) st = time.time() print("loading scenario...") bng.load_scenario(scenario) bng.set_steps_per_second(60) bng.set_deterministic() #print("Bounding box: {}".format(vehicle.get_bbox())) bng.hide_hud() print("starting scenario...") bng.start_scenario() vehicle.ai_set_mode('traffic') #"DecalRoad31765_8" vehicle.ai_drive_in_lane(False) vehicle.ai_set_aggression(2.0) bng.start_traffic(tvs) bng.switch_vehicle(vehicle) bng.pause() return vehicle, bng
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256) scenario = Scenario('west_coast_usa', 'lidar_tour', description='Tour through the west coast gathering ' 'Lidar data') vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR') lidar = Lidar() vehicle.attach_sensor('lidar', lidar) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795)) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) window = open_window(SIZE, SIZE) lidar_vis = LidarVisualiser(Lidar.max_points) lidar_vis.open(SIZE, SIZE) bng.set_steps_per_second(60) bng.set_deterministic() bng.hide_hud() bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') def update(): sensors = bng.poll_sensors(vehicle) points = sensors['lidar']['points'] bng.step(3, wait=False) lidar_vis.update_points(points, vehicle.state) glutPostRedisplay() glutReshapeFunc(lidar_resize) glutIdleFunc(update) glutMainLoop() finally: bng.close()
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256) scenario = Scenario('west_coast_usa', 'ai_sine') vehicle = Vehicle('ego_vehicle', model='etk800', licence='AI') orig = (-769.1, 400.8, 142.8) scenario.add_vehicle(vehicle, pos=orig, rot=None, rot_quat=(0, 0, 1, 0)) scenario.make(beamng) script = list() for i in range(3600): node = { # Calculate the position as a sinus curve that makes the vehicle # drive from left to right. The z-coordinate is not calculated in # any way because `ai_set_script` by default makes the polyline to # follow cling to the ground, meaning the z-coordinate will be # filled in automatically. 'x': 4 * np.sin(np.radians(i)) + orig[0], 'y': i * 0.2 + orig[1], 'z': orig[2], # Calculate timestamps for each node such that the speed between # points has a sinusoidal variance to it. 't': (2 * i + (np.abs(np.sin(np.radians(i)))) * 64) / 64, } script.append(node) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_script(script) while True: bng.step(60) finally: bng.close()
def run_scenario_spawns_backwards(vehicle_model='etk800'): global base_filename, default_color, spawn, steps_per_sec, home random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home=home) scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('vehicle', model=vehicle_model, licence='CORRECT', color='Red') vehicle = setup_sensors(vehicle) # 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']) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() return_str = "SPAWNED VEHICLE STATE:\n{}".format(vehicle.state) print(return_str) bng.close() return return_str
# Example scenario for usage with example.py (run this first) from beamngpy import BeamNGpy, Scenario, Vehicle, Road from beamngpy import setup_logging import time import sys import fileinput setup_logging() bng = BeamNGpy('localhost', 64256) scenario = Scenario('smallgrid', 'small_test') vehicle = Vehicle('egovehicle', model='etk800', licence='313') scenario.add_vehicle(vehicle, pos=(-1.5, -1, 0), rot=(0, 0, 0)) nodes = [(0, 0, 0, 6), (0, -20, 0, 6), (0, -100, 0, 6), (-25, -200, 0, 6), (25, -300, 0, 6)] road = Road(material='a_asphalt_01_a', rid='main_road', looped=False, texture_length=10) road.nodes.extend(nodes) scenario.add_road(road) scenario.make(bng) for i, line in enumerate(fileinput.input(scenario.get_prefab_path(),
def main(MAX_SPEED): setup_logging() # 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') # 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) # Setup the SHM with DeepDrive # Create shared memory object Memory = sd.CSharedMemory(TargetResolution=[280, 210]) # Enable Pause-Mode Memory.setSyncMode(True) Memory.Data.Game.UniqueRaceID = int(time.time()) print("Setting Race ID at ", Memory.Data.Game.UniqueRaceID) # Setting Max_Speed for the Vehicle. # TODO What's this? Maybe some hacky way to pass a parameter which is not supposed to be there... Memory.Data.Game.UniqueTrackID = int(MAX_SPEED) # Speed is KM/H print("Setting speed at ", Memory.Data.Game.UniqueTrackID) # Default for AsFault Memory.Data.Game.Lanes = 1 # By default the AI is in charge Memory.Data.Control.IsControlling = 1 deep_drive_engaged = True STATE = "NORMAL" Memory.waitOnRead() if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0: print("\n\n\nState not reset ! ", Memory.Data.Control.Breaking) Memory.Data.Control.Breaking = 0.0 # Pass the computation to DeepDrive # Not sure this will have any effect Memory.indicateWrite() Memory.waitOnRead() if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0: print("\n\n\nState not reset Again! ", Memory.Data.Control.Breaking) Memory.Data.Control.Breaking = 0.0 # Pass the computation to DeepDrive Memory.indicateWrite() # Connect to running beamng beamng = BeamNGpy( 'localhost', 64256, home='C://Users//Alessio//BeamNG.research_unlimited//trunk') bng = beamng.open(launch=False) try: bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(MAX_FPS) # With 60hz temporal resolution # Connect to the existing vehicle (identified by the ID set in the vehicle instance) bng.connect_vehicle(vehicle) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # Road interface is not available in BeamNG.research yet # Get the road map from the level # roads = bng.get_roads() # # find the actual road. Dividers lane markings are all represented as roads # theRoad = None # for road in enumerate(roads): # # ((left, centre, right), (left, centre, right), ...) # # Compute the width of the road # left = Point(road[0][0]) # right = Point(road[0][1]) # distance = left.distance( right ) # if distance < 2.0: # continue # else: # theRoad = road; # break # # if theRoad is None: # print("WARNING Cannot find the main road of the map") while True: # 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) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) # print("vehicle.state", vehicle.state) # # 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) imageData = preprocess(sensors['front_cam']['colour'], brightness) Height, Width = imageData.shape[:2] # print("Image size ", Width, Height) # TODO Size of image should be right since the beginning Memory.write(Width, Height, imageData, speed) # Pass the computation to DeepDrive Memory.indicateWrite() # Wait for the control commands to send to the vehicle # This includes a sleep and will be unlocked by writing data to it Memory.waitOnRead() # TODO Assumption. As long as the car is out of the road for too long this value stays up if Memory.Data.Control.Breaking == 3.0: if STATE != "DISABLED": print( "Abnormal situation detected. Disengage DeepDrive and enable BeamNG AI" ) vehicle.ai_set_mode("manual") vehicle.ai_drive_in_lane(True) vehicle.ai_set_speed(MAX_SPEED) vehicle.ai_set_waypoint("waypoint_goal") deep_drive_engaged = False STATE = "DISABLED" elif Memory.Data.Control.Breaking == 2.0: if STATE != "GRACE": print("Grace period. Deep Driving still disengaged") vehicle.ai_set_mode("manual") vehicle.ai_set_waypoint("waypoint_goal") # vehicle.ai_drive_in_lane(True) STATE = "GRACE" else: if STATE != "NORMAL": print("DeepDrive re-enabled") # Disable BeamNG AI driver vehicle.ai_set_mode("disabled") deep_drive_engaged = True STATE = "NORMAL" # print("State ", STATE, "Memory ",Memory.Data.Control.Breaking ) if STATE == "NORMAL": vehicle.ai_set_mode("disabled") # Get commands from SHM # Apply Control - not sure cutting at 3 digit makes a difference steering = round( translate_steering(Memory.Data.Control.Steering), 3) throttle = round(Memory.Data.Control.Accelerating * acc_gain, 3) brake = round(Memory.Data.Control.Breaking * brake_gain, 3) # Apply commands vehicle.control(throttle=throttle, steering=steering, brake=brake) # # print("Suggested Driving Actions: ") # print(" Steer: ", steering) # print(" Accel: ", throttle) # print(" Brake: ", brake) finally: bng.close()
def test_quats(beamng): with beamng as bng: setup_logging() scenario = Scenario('smallgrid', 'test_quat') blue_etk = Vehicle('ego_vehicle', model='etk800', color='Blue', licence="angle") scenario.add_vehicle(blue_etk, pos=(0, 0, 0), rot=(0, 0, 0)) blue_etk = Vehicle('ego_vehicle2', model='etk800', color='Green', license="quat") rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229) scenario.add_vehicle(blue_etk, pos=(5, 0, 0), rot_quat=rot_quat) rb = ScenarioObject(oid='roadblock', name='sawhorse', otype='BeamNGVehicle', pos=(-10, -5, 0), rot=(0, 0, 0), scale=(1, 1, 1), JBeam='sawhorse', datablock="default_vehicle") scenario.add_object(rb) cn = ScenarioObject(oid='cones', name='cones', otype='BeamNGVehicle', pos=(0, -5, 0), rot=None, rot_quat=(0, 0, 0, 1), scale=(1, 1, 1), JBeam='cones', datablock="default_vehicle") scenario.add_object(cn) scenario.make(beamng) bng.load_scenario(scenario) bng.start_scenario() white_etk = Vehicle('ego_vehicle3', model='etk800', color='White') bng.spawn_vehicle(white_etk, (-10, 0, 0), (0, 0, 0)) pickup = Vehicle('ego_vehicle4', model='pickup') pos = (-15, 0, 0) bng.spawn_vehicle(pickup, pos, None, rot_quat=(0, 0, 0, 1)) resp = bng.get_current_vehicles() assert len(resp) == 6 pickup.connect(bng) pickup.poll_sensors() pos_before = pickup.state['pos'] bng.teleport_vehicle(pickup.vid, pos, rot=(0, 45, 0)) pickup.poll_sensors() pos_after = pickup.state['pos'] assert (pos_before != pos_after) pickup.poll_sensors() pos_before = pickup.state['pos'] rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229) bng.teleport_vehicle(pickup.vid, pos, rot_quat=rot_quat) pickup.poll_sensors() pos_after = pickup.state['pos'] assert (pos_before != pos_after) try: bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0)) assert True except socket.timeout: assert False try: rot_quat = (-0.003337, -0.0021882, -0.6891695, 0.7245892) bng.teleport_scenario_object(rb, (-10, 5, 0), rot_quat=rot_quat) assert True except socket.timeout: assert False
def run_scenario(traffic=2, run_number=0): global sp setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') scenario = Scenario('west_coast_usa', 'lidar_tour', description='Tour through the west coast gathering ' 'Lidar data') vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR', color='Red') vehicle = setup_sensors(vehicle) lidar = setup_lidar('hood') vehicle.attach_sensor('lidar', lidar) ego_sp = spawn_point(sp) ego_pos = ego_sp['pos'] ego_rot_quat = ego_sp['rot_quat'] scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat) tvs = traffic_vehicles(traffic) ps = generate_vehicle_positions(ego_pos, ego_rot_quat, tvs) for i in range(len(tvs)): scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.set_steps_per_second(60) #bng.set_deterministic() #print("Bounding box: {}".format(vehicle.get_bbox())) bng.hide_hud() bng.start_scenario() vehicle.ai_set_mode('traffic') vehicle.ai_drive_in_lane(True) #vehicle.ai_set_speed(16, mode='limit') #vehicle.ai_set_target('traffic') vehicle.ai_set_aggression(0.9) bng.start_traffic(tvs) bng.switch_vehicle(vehicle) start = time.time() end = time.time() damage_prev = None with open( 'H:/traffic_traces/traffic_lidar_data_{}traffic_run{}.csv'. format(traffic, run_number), 'w') as f: f.write( "TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH,EXTERNAL_VEHICLES \n" ) for _ in range(1024): sensors = bng.poll_sensors(vehicle) points = sensors['lidar']['points'] damage = sensors['damage'] v_state = vehicle.state #print("vehicle_state = {}".format(v_state)) ai_state = vehicle.ai_get_state() #print("ai_state = {}".format(ai_state)) new_damage = diff_damage(damage, damage_prev) damage_prev = damage if new_damage > 0: print("new damage = {}".format(new_damage)) f.write("{},{},{},{},{},{},{}\n".format( _ * 0.5, v_state['pos'], v_state['dir'], v_state['vel'], points.tolist(), str(new_damage), traffic)) #print("Time passed since last step: {}".format(time.time() - end)) end = time.time() #print("Time passed since scenario begun: {}\n".format(end - start)) print() if end - start >= 45: bng.close() bng.step(30, wait=False) except Exception as e: print(e) finally: bng.close()
def testrun(speed=11, risk=0.6): global base_filename, training_dir, default_model, setpoint global spawnpoint f = setup_dir(training_dir) spawn_pt = spawn_point(default_scenario, spawnpoint) random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean', 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='RED', 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(36) # # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() bng.switch_vehicle(vehicle) vehicle.ai_set_mode('traffic') vehicle.ai_drive_in_lane(False) vehicle.ai_set_speed(speed, mode='set') vehicle.ai_set_aggression(risk) # 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 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' ) reached_start = False vels = [] vel_dict = {} while imagecount < 10000: # collect images 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) # 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'])) if sensors['timer']['time'] > 10: kph = sensors['electrics']['wheelspeed'] * 3.6 vels.append(kph) vel_dict[sensors['timer']['time']] = kph if distance(spawn_pt['pos'], vehicle.state['pos'] ) < 5 and sensors['timer']['time'] > 10: reached_start = True break if sensors['damage']['damage'] > 0: print("CRASHED; QUITTING") break # save the image image.save(full_filename) imagecount += 1 # plt.title("90 degrees FOV") # plt.imshow(image) # plt.pause(0.01) # plt.title("120 degrees FOV") # plt.imshow(sensors['back_cam']['colour'].convert('RGB')) # plt.pause(0.01) # step sim forward bng.step(1, wait=True) wheelspeed_avg = round((sum(vels) / float(len(vels))), 3) wheelspeed_var = round(np.var(vels), 3) return_str = '' if sensors['damage']['damage'] > 0: # print("QUIT DUE TO CRASH VALUE {}".format(sensors['damage']['damage'])) return_str = "QUIT DUE TO CRASH VALUE {}".format( sensors['damage']['damage']) print(return_str) maxtime = from_val_get_key(vel_dict, max(vels)) mintime = from_val_get_key(vel_dict, min(vels)) info = "IMAGE COUNT:{}\nSIM TIME:{} WALLCLOCK TIME:{}\nWHEELSPEED AVG:{} VAR:{} \nMAX:{} @ timestep {} MIN:{} @ timestep {} ".format( imagecount, str(round(sensors['timer']['time'], 2)), time.time() - start_time, wheelspeed_avg, wheelspeed_var, round(max(vels), 3), round(maxtime, 3), round(min(vels), 3), round(mintime, 3)) print("SPEED:{} RISK:{}".format(speed, risk)) print(info) return_str = "{}\n{}".format(return_str, info) bng.close() return return_str
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
import mmap
def run_scenario(self): global base_filename, default_model, default_color, default_scenario, setpoint global prev_error #vehicle_loadfile = 'vehicles/pickup/pristine.save.json' # setup DNN model + weights m = Model() model = m.define_model_BeamNG("BeamNGmodel-4.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=default_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() # perturb vehicle #vehicle.ai_set_mode('span') #vehicle.ai_drive_in_lane(True) #vehicle_loadfile = 'vehicles/etk800/fronttires_0psi.pc' # vehicle_loadfile = 'vehicles/etk800/backtires_0psi.pc' # vehicle_loadfile = 'vehicles/etk800/chassis_forcefeedback201.pc' # vehicle.load_pc(vehicle_loadfile, False) vehicle.deflate_tires([1,1,1,1]) #vehicle.break_all_breakgroups() #vehicle.break_hinges() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() wheelspeed = 0.0; throttle = 0.0; prev_error = setpoint; damage_prev = None; runtime = 0.0 kphs = [] damage = None final_img = None # Send random inputs to vehice and advance the simulation 20 steps for _ in range(1024): # collect images image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') img = m.process_image(image) prediction = model.predict(img) # control params kph = ms_to_kph(wheelspeed) throttle = throttle_PID(kph, dt) brake = 0 #if throttle < 0: if setpoint < kph: brake = throttle / 1000.0 throttle = 0.0 # throttle = 0.2 # random.uniform(0.0, 1.0) # brake = random.choice([0, 0, 0.1 , 0.2]) steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0) vehicle.control(throttle=throttle, steering=steering, brake=brake) steering_state = bng.poll_sensors(vehicle)['electrics']['steering'] steering_input = bng.poll_sensors(vehicle)['electrics']['steering_input'] avg_wheel_av = bng.poll_sensors(vehicle)['electrics']['avg_wheel_av'] wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed'] damage = bng.poll_sensors(vehicle)['damage'] new_damage = diff_damage(damage, damage_prev) damage_prev = damage print("\n") # #print("steering state: {}".format(steering_state)) # print("AI steering_input: {}".format(steering_input)) #print("avg_wheel_av: {}".format(avg_wheel_av)) # print("DAVE2 steering prediction: {}".format(float(prediction[0][0]))) print("throttle:{}".format(throttle)) print("brake:{}".format(brake)) print("kph: {}".format(ms_to_kph(wheelspeed))) print("new_damage:{}".format(new_damage)) kphs.append(ms_to_kph(wheelspeed)) if new_damage > 0.0: final_img = image break bng.step(5) runtime += (0.05) # 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.pause(0.01) return runtime, avg_kph, damage['damage'], kphs
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256) scenario = Scenario('smallgrid', 'test_quat') vehicle = Vehicle('ego_vehicle', model='etk800', color='Blue', licence="angle") scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0)) vehicle = Vehicle('ego_vehicle2', model='etk800', color='Green', license="quat") scenario.add_vehicle(vehicle, pos=(5, 0, 0), rot_quat=(-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)) rb = ScenarioObject(oid='roadblock', name='sawhorse', otype='BeamNGVehicle', pos=(-10, -5, 0), rot=(0, 0, 0), scale=(1, 1, 1), JBeam='sawhorse', datablock="default_vehicle") scenario.add_object(rb) cn = ScenarioObject(oid='cones', name='cones', otype='BeamNGVehicle', pos=(0, -5, 0), rot=None, rot_quat=(0, 0, 0, 1), scale=(1, 1, 1), JBeam='cones', datablock="default_vehicle") scenario.add_object(cn) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() input('Press Enter to spawn vehicle during sim with rot and rotquat') vehicle = Vehicle('ego_vehicle3', model='etk800', color='White') bng.spawn_vehicle(vehicle, (-10, 0, 0), (0, 0, 0)) vehicle = Vehicle('ego_vehicle4', model='pickup') pos = (-15, 0, 0) bng.spawn_vehicle(vehicle, pos, None, rot_quat=(0, 0, 0, 1)) input('press Enter to teleport last vehicle with angle') bng.teleport_vehicle(vehicle, pos, rot=(0, 45, 0)) input('press Enter to teleport last vehicle with quaternion') bng.teleport_vehicle(vehicle, pos, rot_quat=(-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)) input('press Enter to teleport roadblock with angle') bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0)) input('press Enter to teleport roadblock with quaternion') bng.teleport_scenario_object(rb, (-10, 5, 0), rot_quat=(-0.003337, -0.0021882, -0.6891695, 0.7245892)) input('Press ENTER to exit') finally: bng.close()
def main(): random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') config = Config() loaded_config = config.load("{}/config.json".format(beamng.home)) # 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') # Set up sensors pos = (-0.3, 1, 1.0) direction = (0, 1, 0) fov = 120 resolution = (512, 512) front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) pos = (0.0, 3, 1.0) direction = (0, -1, 0) fov = 90 resolution = (512, 512) back_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) gforces = GForces() electrics = Electrics() damage = Damage() lidar = Lidar(visualized=False) timer = Timer() # Attach them vehicle.attach_sensor('front_cam', front_camera) vehicle.attach_sensor('back_cam', back_camera) vehicle.attach_sensor('gforces', gforces) vehicle.attach_sensor('electrics', electrics) vehicle.attach_sensor('damage', damage) vehicle.attach_sensor('timer', timer) #scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795)) #config_rot_quat = beamngpy.angle_to_quat(config.dir) # N.B. using rot is deprecated in favor of rot_quat #scenario.add_vehicle(vehicle, pos=tuple(config.pos), rot=tuple(config.dir), rot_quat=None) scenario.add_vehicle(vehicle, pos=tuple(config.pos), rot=None, rot_quat=beamngpy.angle_to_quat(config.dir)) # 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() assert vehicle.skt # Send random inputs to vehicle and advance the simulation 20 steps #for _ in range(1024): for _ in range(100): 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(_)) for s in sensors.keys(): print("{} : {}".format(s, sensors[s])) damage_dict = vehicle.sensors['damage'].encode_vehicle_request() damage_dict = sensors['damage'] config.update(sensors['damage']) config.update(vehicle.state) config.save('{}/config.json'.format(beamng.home)) gamestate = beamng.get_gamestate() if _ > 990: print("late in sim") finally: bng.close()
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') scenario = Scenario( 'west_coast_usa', 'lidar_tour', description='Tour through the west coast gathering Lidar data') vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR') vehicle = setup_sensors(vehicle) lidar = setup_lidar('hood') vehicle.attach_sensor('lidar', lidar) spawn_pt = spawn_point() ego_pos = spawn_pt['pos'] ego_rot_quat = spawn_pt['rot_quat'] scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat) tvs = traffic_vehicles() scenario.make(beamng) bng = beamng.open(launch=True) #try: bng.load_scenario(scenario) bng.set_steps_per_second(60) bng.set_deterministic() bng.hide_hud() bng.start_scenario() vehicle.ai_set_mode('traffic') vehicle.ai_set_aggression(0.5) vehicle.ai_drive_in_lane(True) #vehicle.ai_set_speed(16, mode='limit') #vehicle.ai_set_target('traffic') bng.switch_vehicle(vehicle) damage_prev = None start = time.time() end = time.time() bng.pause() print("Bounding box: {}".format(vehicle.get_bbox())) with open('lidar_data.csv', 'w') as f: f.write( "TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH\n" ) for _ in range(1024): sensors = bng.poll_sensors(vehicle) points = sensors['lidar']['points'] damage = sensors['damage'] v_state = vehicle.state print("vehicle_state = {}".format(v_state)) print("vehicle_state[pos] = {}".format(v_state['pos'])) print("vehicle_state[dir] = {}".format(v_state['dir'])) print("Vehicle bounding box:{}".format(vehicle.get_bbox())) #ai_state = vehicle.ai_get_state() #print("ai_state = {}".format(ai_state)) new_damage = diff_damage(damage, damage_prev) damage_prev = damage #print("new damage = {}".format(new_damage)) print() f.write("{},{},{},{},{},{}\n".format(_ * 0.5, v_state['pos'], v_state['dir'], v_state['vel'], points.tolist(), str(new_damage))) #bng.step(30, wait=False) bng.step(30) #print("Time passed since last step: {}".format(time.time() - end)) end = time.time() #print("Time passed since scenario begun: {}\n".format(end - start)) #screenrecord() if end - start >= 30: #bng.close() continue
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')
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 main(): global base_filename, training_dir, default_model f = setup_dir(training_dir) spawn_pt = spawn_point(default_scenario) random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=default_model, licence='RED', color='Red') 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) images = [] 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() vehicle.ai_set_mode('span') vehicle.ai_drive_in_lane(True) vehicle.ai_set_aggression(0.1) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # Send random inputs to vehice and advance the simulation 20 steps imagecount = 0 wheelvel = [0.1, 0.1, 0.1] 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\n') #for _ in range(1024): for _ in range(32768): #throttle = 1.0 #random.uniform(0.0, 1.0) #steering = random.uniform(-1.0, 1.0) #brake = random.choice([0, 0, 0, 1]) #vehicle.control(throttle=throttle) # collect images sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') imagecount += 1 filename = "{}{}.bmp".format(base_filename, imagecount) # collect ancillary data datafile.write('{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(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'],)) # save the image image.save(filename) # step sim forward bng.step(20) print('{} seconds passed.'.format(str(round(sensors['timer']['time'], 2)))) # check for stopping condition for i in range(len(wheelvel)-1): wheelvel[i] = wheelvel[i+1] wheelvel[2] = float(sensors['electrics']['wheelspeed']) print('wheelvel = {}'.format(sum(wheelvel) / 3.0 )) if sum(wheelvel) / 3.0 == 0.0: print("avg wheelspeed is zero; exiting...") bng.close() break
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()
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(): global sp setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean') scenario = Scenario('west_coast_usa', 'lidar_tour', description='Tour through the west coast gathering ' 'Lidar data') vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR', color='Red') lidar = Lidar() lidar.__init__(offset=(0, 0, 1.7), direction=(-0.707, -0.707, 0), vres=32, vangle=0.01, rps=2200000, hz=20, angle=360, max_dist=200, visualized=True) vehicle.attach_sensor('lidar', lidar) ego_sp = spawn_point(sp) ego_pos = ego_sp['pos'] ego_rot_quat = ego_sp['rot_quat'] scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat) tvs = traffic_vehicles() #ps = [(-722.121, 103, 118.675), (-714.121, 101, 118.675), (-715.121, 105, 118.675), (-721.121, 100, 118.675)] ps = [(ego_pos[0]-10, ego_pos[1]+2, ego_pos[2]), (ego_pos[0]-10, ego_pos[1]-2, ego_pos[2]), (ego_pos[0]-14, ego_pos[1]+5, ego_pos[2]), (ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]), (ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]), (ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]), (ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])] ps1 = [(ego_pos[0] - 10, ego_pos[1] + 3, ego_pos[2]), (ego_pos[0] - 10, ego_pos[1] - 1, ego_pos[2]), (ego_pos[0] - 14, ego_pos[1] + 5, ego_pos[2]), (ego_pos[0] + 5, ego_pos[1] - 1, ego_pos[2]), (ego_pos[0] + 8, ego_pos[1] - 4, ego_pos[2]), (ego_pos[0] + 10, ego_pos[1] + 9, ego_pos[2]), (ego_pos[0] + 1, ego_pos[1] + 11, ego_pos[2])] ps_oldorig = [(ego_pos[0]-7, ego_pos[1]+3, ego_pos[2]), (ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]), (ego_pos[0]+4, ego_pos[1]+5, ego_pos[2]), (ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]), (ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]), (ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]), (ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])] for i in range(len(tvs)): scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.set_steps_per_second(60) bng.set_deterministic() bng.hide_hud() bng.start_scenario() vehicle.ai_set_mode('traffic') vehicle.ai_drive_in_lane(True) #vehicle.ai_set_speed(16, mode='limit') #vehicle.ai_set_target('traffic') vehicle.ai_set_aggression(0.5) #0.7) bng.start_traffic(tvs) bng.switch_vehicle(vehicle) start = time.time() end = time.time() damage_prev = bng.poll_sensors(vehicle) with open('lidar_data.csv', 'w') as f: for _ in range(1024): sensors = bng.poll_sensors(vehicle) points = sensors['lidar']['points'] #print(points.tolist()) #print() #f.write("{}\n".format(points.tolist())) print("Time passed since last step: {}".format(time.time() - end)) end = time.time() print("Time passed since scenario begun: {}\n".format(end - start)) if end - start >= 30: bng.close() bng.step(30, wait=False) except Exception as e: print(e) finally: bng.close()
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
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(): random.seed(1703) setup_logging() # Plotting code setting up a 3x2 figure fig = plt.figure(1, figsize=(10, 5)) axarr = fig.subplots(2, 3) a_colour = axarr[0, 0] b_colour = axarr[1, 0] a_depth = axarr[0, 1] b_depth = axarr[1, 1] a_annot = axarr[0, 2] b_annot = axarr[1, 2] plt.ion() beamng = BeamNGpy('localhost', 64256) bng = beamng.open(launch=True) # Create a scenario in west_coast_usa scenario = Scenario('west_coast_usa', 'tech_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') # Set up sensors pos = (-0.3, 1, 1.0) direction = (0, 1, 0) fov = 120 resolution = (512, 512) front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) pos = (0.0, 3, 1.0) direction = (0, -1, 0) fov = 90 resolution = (512, 512) back_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) gforces = GForces() electrics = Electrics() damage = Damage() lidar = Lidar(visualized=False) timer = Timer() # Attach them vehicle.attach_sensor('front_cam', front_camera) vehicle.attach_sensor('back_cam', back_camera) vehicle.attach_sensor('gforces', gforces) vehicle.attach_sensor('electrics', electrics) vehicle.attach_sensor('damage', damage) vehicle.attach_sensor('timer', timer) 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(bng) # Start BeamNG and enter the main loop 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 # Send random inputs to vehice and advance the simulation 20 steps for _ in range(1024): 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) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) print('{} seconds passed.'.format(sensors['timer']['time'])) a_colour.imshow(sensors['front_cam']['colour'].convert('RGB')) a_depth.imshow(sensors['front_cam']['depth'].convert('L')) a_annot.imshow(sensors['front_cam']['annotation'].convert('RGB')) b_colour.imshow(sensors['back_cam']['colour'].convert('RGB')) b_depth.imshow(sensors['back_cam']['depth'].convert('L')) b_annot.imshow(sensors['back_cam']['annotation'].convert('RGB')) plt.pause(0.00001) finally: bng.close()
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