import matplotlib import numpy as np import matplotlib.pyplot as plt from matplotlib.pyplot import imshow from PIL import Image from shapely.geometry import Polygon from beamngpy import BeamNGpy, Vehicle, Scenario, Road from beamngpy.sensors import Camera beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.0') scenario = Scenario('GridMap', 'vehicle_bbox_example') road = Road('track_editor_C_center', rid='main_road', texture_length=5) orig = (-107, 70, 0) goal = (-300, 70, 0) road.nodes = [ (*orig, 7), (*goal, 7), ] 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) scenario.make(beamng)
from beamngpy import BeamNGpy, Scenario, Vehicle, StaticObject from beamngpy.sensors import Electrics, Damage import numpy as np from time import sleep, time beamng = BeamNGpy('localhost', 64256, home=r'C:\BeamNG_unlimited\trunk') scenario = Scenario('west_coast_usa', 't_intersection_w_obj_ai') vut = Vehicle('vut', model='coupe', licence='VUT', colour='Red') electrics = Electrics() damage = Damage() vut.attach_sensor('electrics', electrics) vut.attach_sensor('damage', damage) scenario.add_vehicle(vut, pos=(-198.5, -164.189, 119.7), rot=(0, 0, -126.25)) obj_1 = StaticObject( 'obj_1', pos=(-140, -121.233, 119.586), rot=(0, 0, 55), scale=(1, 1, 1), shape='/levels/west_coast_usa/art/shapes/objects/barrierfence_folk.dae') scenario.add_object(obj_1) 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)) car_2 = Vehicle('car_2', model='etk800', licence='CAR 2', colour='Blue') scenario.add_vehicle(car_2, pos=(-140, -121.233, 119.586), rot=(0, 0, 55)) scenario.make(beamng) bng = beamng.open(launch=True)
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
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 # 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 run_sim(street_1: DecalRoad): waypoints = [] for node in street_1.nodes: waypoint = BeamNGWaypoint("waypoint_" + str(node), get_node_coords(node)) waypoints.append(waypoint) print(len(waypoints)) maps.beamng_map.generated().write_items( street_1.to_json() + '\n' + "\n".join([waypoint.to_json() for waypoint in waypoints])) beamng = BeamNGpy('localhost', 64256) scenario = Scenario('tig', 'tigscenario') vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') sim_data_collector = TrainingDataCollectorAndWriter( vehicle, beamng, street_1) scenario.add_vehicle(vehicle, pos=get_node_coords(street_1.nodes[0]), rot=get_rotation(street_1)) scenario.make(beamng) beamng.open() beamng.set_deterministic() beamng.load_scenario(scenario) beamng.pause() beamng.start_scenario() vehicle.ai_drive_in_lane(True) vehicle.ai_set_mode("disabled") vehicle.ai_set_speed(10 / 4) steps = 5 def start(): for waypoint in waypoints[10:-1:20]: vehicle.ai_set_waypoint(waypoint.name) for idx in range(1000): if (idx * 0.05 * steps) > 3.: sim_data_collector.collect_and_write_current_data() dist = distance(sim_data_collector.last_state.pos, waypoint.position) if dist < 15.0: beamng.resume() break # one step is 0.05 seconds (5/100) beamng.step(steps) try: start() finally: beamng.close()