def test_vehicle_ai(beamng): with beamng as bng: bng.set_deterministic() scenario = Scenario('west_coast_usa', 'ai_test') vehicle = Vehicle('test_car', model='etk800') other = Vehicle('other', model='etk800') pos = [-717.121, 101, 118.675] scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45)) scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45)) scenario.make(bng) bng.load_scenario(scenario) bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_waypoint('Bridge4_B') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='chase') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='flee') assert_continued_movement(bng, vehicle, pos) scenario.delete(beamng)
def test_vehicle_ai(beamng): with beamng as bng: bng.set_deterministic() scenario = Scenario('west_coast_usa', 'ai_test') vehicle = Vehicle('test_car', model='etk800') other = Vehicle('other', model='etk800') pos = [-717.121, 101, 118.675] scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45)) scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45)) scenario.make(bng) bng.load_scenario(scenario) bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_waypoint('Bridge4_B') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='chase') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='flee') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() script = [ { 'x': -735, 'y': 86.7, 'z': 119, 't': 0 }, { 'x': -752, 'y': 70, 'z': 119, 't': 5 }, { 'x': -762, 'y': 60, 'z': 119, 't': 8 }, ] vehicle.ai_set_script(script) bng.step(600, wait=True) vehicle.update_vehicle() ref = [script[1]['x'], script[1]['y'], script[1]['z']] pos = vehicle.state['pos'] ref, pos = np.array(ref), np.array(pos) assert np.linalg.norm(ref - pos) < 2.5 scenario.delete(beamng)
def 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 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 run_sim(nodes, ai_aggression, street_1: DecalRoad, street_2: DecalRoad): waypoint_goal = BeamNGWaypoint('waypoint_goal', get_node_coords(street_1.nodes[-1])) maps.beamng_map.generated().write_items(street_1.to_json() + '\n' + waypoint_goal.to_json() + '\n' + street_2.to_json()) 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_set_aggression(ai_aggression) vehicle.ai_drive_in_lane(True) # vehicle.ai_set_speed(25.0 / 4) vehicle.ai_set_waypoint(waypoint_goal.name) # vehicle.ai_set_mode("manual") # sleep(5) steps = 5 print(nodes) print(beamng.get_road_edges("street_1")) def start(): 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_goal.position) if dist < 15.0: beamng.resume() break # one step is 0.05 seconds (5/100) beamng.step(steps) try: start() finally: beamng.close()
pos=(104.647, -1.92827, -3.54338), rot=(0, 0, 90)) #add vehicle as Obstacle vehicle1 = Vehicle('test_vehicle', model='etk800', licence='RED', color='Blue') scenario.add_vehicle(vehicle1, pos=(-9.66595, -90.7465, -3.45737), rot=(0, 0, 180)) scenario.make(beamng) bng = beamng.open() bng.load_scenario(scenario) #set the ego vehicle to AI mode vehicle.ai_set_mode('Manual') vehicle.ai_set_waypoint('DecalRoad5248_4') vehicle1.control(throttle=0.44) bng.start_scenario() # After loading, the simulator waits for further input to actually start positions = list() directions = list() wheel_speeds = list() speed = list() brakes = list() damage1 = list() vehicle.update_vehicle() sensors = bng.poll_sensors(vehicle) print('The vehicle position is:') print(vehicle.state['pos'])