def main(): bng_home = os.environ['BEAMNG_HOME'] road = TrainingRoad(ASFAULT_PREFAB) road.calculate_road_line(back=True) bng = BeamNGpy('localhost', 64256, home=bng_home) scenario = Scenario('smallgrid', 'train') scenario.add_road(road.asphalt) scenario.add_road(road.mid_line) scenario.add_road(road.left_line) scenario.add_road(road.right_line) vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON') front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV, resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), colour=True, depth=False, annotation=False) vehicle.attach_sensor("front_camera", front_camera) # Add it to our scenario at this position and rotation spawn_point = road.spawn_point() scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot()) # Place files defining our scenario for the simulator to read scenario.make(bng) prefab_path = scenario.get_prefab_path() update_prefab(prefab_path) bng.open() bng.set_nondeterministic() bng.set_steps_per_second(60) # Load and start our scenario bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_mode('span') vehicle.ai_set_speed(5) vehicle.ai_set_line([{ 'pos': node.pos(), 'speed': 10 } for node in road.road_line]) number_of_images = 0 while number_of_images < 9000: bng.poll_sensors(vehicle) number_of_images += 1 #print(number_of_images) bng.step(1) sensors = bng.poll_sensors(vehicle) image = sensors['front_camera']['colour'].convert('RGB') image = np.array(image) image = image[:, :, ::-1] dist = road.dist_to_center(vehicle.state['pos']) cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image) bng.close()
class BeamNGBrewer: def __init__(self, beamng_home=None, road_nodes: List4DTuple = None): self.beamng = BeamNGpy('localhost', 64256, home=beamng_home) self.vehicle: Vehicle = None self.camera: BeamNGCamera = None if road_nodes: self.setup_road_nodes(road_nodes) steps = 5 self.params = SimulationParams(beamng_steps=steps, delay_msec=int(steps * 0.05 * 1000)) self.vehicle_start_pose = BeamNGPose() def setup_road_nodes(self, road_nodes): self.road_nodes = road_nodes self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points( road_nodes) self.road_points = RoadPoints().add_middle_nodes(road_nodes) def setup_vehicle(self) -> Vehicle: assert self.vehicle is None self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') return self.vehicle def setup_scenario_camera(self, resolution=(1280, 1280), fov=120) -> BeamNGCamera: assert self.camera is None self.camera = BeamNGCamera(self.beamng, 'brewer_camera') return self.camera def bring_up(self): self.scenario = Scenario('tig', 'tigscenario') if self.vehicle: self.scenario.add_vehicle(self.vehicle, pos=self.vehicle_start_pose.pos, rot=self.vehicle_start_pose.rot) if self.camera: self.scenario.add_camera(self.camera.camera, self.camera.name) self.scenario.make(self.beamng) if not self.beamng.server: self.beamng.open() self.beamng.pause() self.beamng.set_deterministic() self.beamng.load_scenario(self.scenario) self.beamng.start_scenario() def __del__(self): if self.beamng: try: self.beamng.close() except: pass
def run_sim(street_1: DecalRoad, ai_aggression): 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()) 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(False) vehicle.ai_set_speed(25) vehicle.ai_set_waypoint(waypoint_goal.name) #vehicle.ai_set_mode("manual") steps = 5 def start(): for idx in range(1000): if (idx * 0.05 * steps) > 3.: sim_data_collector.collect_and_write_current_data() if sim_data_collector.oob_monitor.is_oob(wrt="center"): raise ValueError("OOB detected during training") 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()
def main() -> None: from beamngpy import BeamNGpy, Scenario, Vehicle, beamngcommon from beamngpy.sensors import Damage, Camera, Electrics from time import sleep bng = BeamNGpy("localhost", 64523) ego_vehicle = Vehicle('ego_vehicle', model='etk800', licence='EGO', color='Red') scenario = Scenario( "west_coast_usa", "DamageSensorTest", authors="Stefan Huber", description="Test usage and check output of the damage sensor") direction = (0, 1, 0) fov = 120 resolution = (320, 160) y, z = (1.7, 1.0) cam_center = Camera((-0.3, y, z), direction, fov, resolution, colour=True, depth=True, annotation=True) cam_left = Camera((-1.3, y, z), direction, fov, resolution, colour=True, depth=True, annotation=True) cam_right = Camera((0.4, y, z), direction, fov, resolution, colour=True, depth=True, annotation=True) #cameras_array = [camera_center, camera_left, camera_right] ego_vehicle.attach_sensor('cam_center', cam_center) ego_vehicle.attach_sensor('cam_left', cam_left) ego_vehicle.attach_sensor('cam_right', cam_right) ego_vehicle.attach_sensor("electrics", Electrics()) #electrics_data = Electrics.encode_vehicle_request(Electrics) #print(electrics_data) #scenario.add_vehicle(ego_vehicle,pos=(-717.121, 101, 118.675), rot=(0, 0, 45)) scenario.add_vehicle(ego_vehicle, pos=(-725.7100219726563, 554.3270263671875, 121.0999984741211), rot=(0, 0, 45)) scenario.make(bng) bng.open(launch=True) def save_image(data, ind, cam_name): img = data[cam_name]['colour'].convert('RGB') file_name = str(ind) + "_" + cam_name + ".jpg" filepath = os.path.join('C:/Users/HP/Documents/Data_Log/10_lap_log', file_name) img.save(filepath) return file_name def save(data, ind): cam_left_file_name = save_image(data, ind, 'cam_left') cam_right_file_name = save_image(data, ind, 'cam_right') cam_center_file_name = save_image(data, ind, 'cam_center') steering_in_value = data['electrics']['values']['steering_input'] steering_value = data['electrics']['values']['steering'] throttle_in_value = data['electrics']['values']['throttle_input'] throttle_value = data['electrics']['values']['throttle'] clutch_value = data['electrics']['values']['clutch'] clutch_in_value = data['electrics']['values']['clutch_input'] wheel_speed_value = data['electrics']['values']['wheelspeed'] rpmspin_value = data['electrics']['values']['rpmspin'] #add here print(cam_left_file_name, cam_right_file_name, cam_center_file_name, steering_in_value, steering_value, throttle_in_value, throttle_value, clutch_in_value, clutch_value, rpmspin_value, wheel_speed_value) print() temp_df = temp_dataframe() temp_df.loc[0] = [ cam_left_file_name, cam_right_file_name, cam_center_file_name, steering_in_value, steering_value, throttle_in_value, throttle_value, clutch_in_value, clutch_value, wheel_speed_value, rpmspin_value ] # modify #append with existing and save df_orig = pd.read_csv('my_data_lap3.csv') df_orig = pd.concat([df_orig, temp_df]) df_orig.to_csv('my_data_lap3.esc', index=False) del [[df_orig, temp_df]] def temp_dataframe(): df1 = pd.DataFrame(columns=[ 'cam_left', 'cam_right', 'cam_centre', 'steering_in_value', 'steering_value', 'throttle_in_value', 'throttle_value', 'clutch_in_value', 'clutch_value', 'wheelspeed_value', 'rpmspin_value' ]) # modify return df1 try: bng.load_scenario(scenario) bng.set_steps_per_second(60) bng.set_deterministic() bng.start_scenario() bng.hide_hud() ego_vehicle.ai_drive_in_lane(lane=True) ego_vehicle.ai_set_mode('span') ego_vehicle.ai_set_speed(10) ind = 0 while True: sensor_data = bng.poll_sensors(ego_vehicle) save(sensor_data, ind) ind += 1 finally: bng.close()
class BeamNG_ACC_Test(): def __init__(self, **kwargs): keys = kwargs.keys() if "test_controller" in keys: self.controller = kwargs.get("test_controller") else: self.controller = ACC_Test() config = BeamNG_Cruise_Control.Config(KP=CC_P, KI=CC_I, KD=CC_D, target=ACC_CAR_SPEED) config2 = BeamNG_Cruise_Control.Config(KP=CC_P, KI=CC_I, KD=CC_D, target=FRONT_CAR_SPEED) self.PID = BeamNG_Cruise_Control.PID_Controller(config=config) self.PID2 = BeamNG_Cruise_Control.PID_Controller(config=config2) def setup_bngServer(self): # Instantiate BeamNGpy instance running the simulator from the given path, if ('BNG_HOME' in os.environ): self.bng = BeamNGpy('localhost', 64256, home=os.environ['BNG_HOME']) else: print( "WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)" ) self.bng = BeamNGpy( 'localhost', 64256, home='C:\\Users\\Ingars\\Beamng-unlimited\\trunk') def setup_BeamNG(self): # Create a scenario in test map self.scenario = Scenario('cruise-control', 'example') # Create an ETK800 with the licence plate 'PID' self.vehicle = Vehicle('ACC', model='etk800', licence='ACC') electrics = Electrics() self.vehicle.attach_sensor('electrics', electrics) # Cars electric system # Add it to our scenario at this position and rotation self.scenario.add_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 0)) self.vehicle2 = Vehicle('FRONT', model='etk800', licence='FRONT') self.vehicle2.attach_sensor('electrics', electrics) # Cars electric system self.scenario.add_vehicle(self.vehicle2, pos=FRONT_CAR_POS, rot=(0, 0, 180)) # Place files defining our scenario for the simulator to read self.scenario.make(self.bng) # Launch BeamNG.research self.bng.open() # Load and start our scenario self.bng.load_scenario(self.scenario) self.bng.start_scenario() def runTest(self): self.bng.restart_scenario() self.bng.teleport_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 180)) self.run() def run(self): self.setupTest() wanted_t = ACC_CAR_SPEED wanted_d = WANTED_DISTANCE soft_brake_d = wanted_d - (wanted_d / 2) wanted_d_buffer = wanted_d - (wanted_d / 5) # Wanted distance has a buffer of 25%, to compensate holding same constant speed at bigger distance deacc_d = wanted_d_buffer + 10 # +10 meters is buffer for starting to decelerate towards front car speed and wanted distance. # the buffer should be calculated dynamically instead to compensate for different speeds while self.controller.ended == False: start_time = datetime.datetime.now() sensors = self.bng.poll_sensors(self.vehicle) position = self.vehicle.state['pos'] position2 = self.vehicle2.state['pos'] sensors2 = self.bng.poll_sensors( self.vehicle2 ) # replace with self.vehicle.update_vehicle when not using pre-generated coordinates of vehicle to follow wheel_speed = sensors['electrics']['values']['wheelspeed'] distance = self.controller.setDistance(position, position2) front_car_speed = self.controller.getFrontCarSpeed(wheel_speed) curr_target = self.PID.get_target() print("distance: " + str(distance)) #FORMULA USED for deacceleration = front_car_speed^2 = wheel_speed^2 + (distance-20)*2*a #d = (front_car_speed - wheel_speed) / (-0.3 * 2) # 0.3 deacceleration? distance for reducing speed to front cars speed with -0.3 acceleration #print("d: " + str(d)) if distance < 5: # 5 is manually set as last allowed distance between both cars, will not work good if ACC car is driving too fast. Would be better to calculate it as braking distance. print("brake1") self.vehicle.control(brake=1) self.vehicle.control(throttle=0) elif distance < soft_brake_d: print("brake1") self.vehicle.control( brake=0.1 ) #Softness of brake could also be calculated dynamically to compensate different speeds self.vehicle.control(throttle=0) else: if distance <= wanted_d_buffer: print("wanted") if front_car_speed > curr_target + 3.5 or front_car_speed < curr_target - 2: #or front_car_speed < curr_target - 1 // calibrate 3.5 and 2 self.PID.change_target(max(front_car_speed, 0)) elif distance <= deacc_d: a = (front_car_speed - wheel_speed) / ( (distance - wanted_d_buffer) * 2) print("a:" + str(a)) self.PID.change_target(a + wheel_speed) elif curr_target != wanted_t: self.PID.change_target(wanted_t) print("throttle1") value = self.PID.calculate_throttle(wheel_speed) value = min(1, value) self.vehicle.control(brake=0) self.vehicle.control(throttle=value) #PID for front car wheel_speed2 = sensors2['electrics']['values']['wheelspeed'] # print("real front:" + str(wheel_speed2)) value2 = self.PID2.calculate_throttle(wheel_speed2) value2 = min(1, value2) self.vehicle2.control(brake=0) self.vehicle2.control(throttle=value2) elapsed_time = datetime.datetime.now() - start_time while (elapsed_time.total_seconds() * 1000) < 100: elapsed_time = datetime.datetime.now() - start_time elapsed_total = self.controller.calculateElapsed() # Change front car speed after 10 seconds and after 20 seconds if elapsed_total.total_seconds() > float(10): self.PID2.change_target(20) if elapsed_total.total_seconds() > float(20): self.PID2.change_target(10) print("Ending Test") def close(self): self.bng.close() def setupTest(self): self.vehicle.update_vehicle() self.controller.last_position = self.vehicle.state['pos'] self.controller.start()
vehicle.ai_set_speed(20, "limit") vehicle.ai_set_mode("span") # Data collecting loop. Collects every three steps data. counter = 0 last_counter = 0 steps = 2 while counter < 300: if counter > last_counter + steps: cov_collector.collect() counter += 1 if counter % 10 == 0: print(counter) # bng.stop_scenario() # not working? # bng.restart_scenario() bng.close() # adds binned behavior to dict of road coverage = {'cov_collector': cov_collector, 'steering': cov_collector.get_steering_bins(), 'throttle': cov_collector.get_throttle_bins(), 'speed_steering': cov_collector.get_speed_steering_2d(), 'obe': cov_collector.get_obe_speed_angle_bins()} cov_collector.get_speed_steering_2d() # remove #print(coverage['obe']) # print("entropy steering: ", utils.entropy_compute_1d(coverage['steering'])) # print("2d diff: ", utils.bin_difference_2d(coverage['speed_steering'], coverage['speed_steering'], 'binary', False)) # adds the dictionary of the current road to the global one tests_dict[str(i)] = coverage suitebh = SuiteBehaviourComputer(tests_dict, FIRST_TEST, LAST_TEST) suitebh.calculate_suite_speed_steering_coverage() suitebh.road_compare_1d(str(FIRST_TEST), 'steering') suitebh.calculate_suite_coverage_1d('steering')
def scenario(beamNG_path): if (beamNG_path == ""): beamNG_path = 'D:/BeamNGReasearch/Unlimited_Version/trunk' bng = BeamNGpy('localhost', 64256, beamNG_path) #scenario = Scenario('west_coast_usa', 'example') scenario = Scenario('GridMap', 'example') # Create an ETK800 with the licence plate 'PYTHON' vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red') vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue') electrics = Electrics() vehicle1.attach_sensor('electrics', electrics) pos2 = (-13.04469108581543, -107.0409164428711, 0.202297180891037) pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775) rot2 = (0.0009761620895005763, -0.9999936819076538, -0.0034209610894322395) rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936) scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1) scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2) scenario.make(bng) # Launch BeamNG.research bng.open(launch=True) SIZE = 500 try: bng.load_scenario(scenario) bng.start_scenario() vehicle1.ai_set_speed(10.452066507339481,mode = 'set') vehicle1.ai_set_mode('span') vehicle2.ai_set_mode('chase') #collect data positions = list() directions = list() wheel_speeds = list() for _ in range(100): time.sleep(0.1) vehicle1.update_vehicle() # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicle1) # Polls the data of all sensors attached to the vehicle positions.append(vehicle1.state['pos']) directions.append(vehicle1.state['dir']) wheel_speeds.append(sensors['electrics']['values']['wheelspeed']) print('The Final result - position :') print(positions) print('The Final result - direction :') print(directions) print('The Final result - speed :') print(wheel_speeds) bng.stop_scenario() bng.close() finally: bng.close()
class BeamNG_Cruise_Controller_Test(): def __init__(self, **kwargs): keys = kwargs.keys() if "test_controller" in keys: self.controller = kwargs.get("test_controller") else: self.controller = PID_Controller_Test() if "testing_times" in keys: self.testing_times = kwargs.get("testing_times") else: self.testing_times = 1 if "test_name" in keys: self.test_name = kwargs.get("test_name") else: self.test_name = "test" if "targets" in keys: self.targets = kwargs.get("targets") else: self.targets = [0] def setup_bngServer(self): # Instantiate BeamNGpy instance running the simulator from the given path, if ('BNG_HOME' in os.environ): self.bng = BeamNGpy('localhost', 64256, home=os.environ['BNG_HOME']) else: print( "WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)" ) self.bng = BeamNGpy('localhost', 64256, home='') def setup_BeamNG(self): # Create a scenario in test map scenario = Scenario('cruise-control', 'example') # Create an ETK800 with the licence plate 'PID' self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='PID') electrics = Electrics() self.vehicle.attach_sensor('electrics', electrics) # Cars electric system # Add it to our scenario at this position and rotation scenario.add_vehicle(self.vehicle, pos=(406.787, 1115.518, 0), rot=(0, 0, 0)) # Place files defining our scenario for the simulator to read scenario.make(self.bng) # Launch BeamNG.research self.bng.open() # Load and start our scenario self.bng.load_scenario(scenario) self.bng.start_scenario() def runTest(self, test_K_values, test_name): for test_K_value in test_K_values: KP_in, KI_in, KD_in = map(float, test_K_value) if KP_in == 0 and KI_in == 0 and KD_in == 0: config = Config() controller = On_Off_Controller(config=config) else: config = Config(KP=KP_in, KI=KI_in, KD=KD_in) controller = PID_Controller(config=config) self.controller.controller = controller self.test_name = str(test_name) + "_" + str(KP_in) + "_" + str( KI_in) + "_" + str(KD_in) self.runTestForPID() def runTestForPID(self): for speed in self.targets: self.controller.setTarget(speed) self.controller.setTime(float(30)) #self.runTestOfType("up_{0}_".format(speed), (406.787, 700.517, 0.214829), (0, 0, 0)) self.runTestOfType("straight_{0}_".format(speed), (406.787, 715.517, 0.214829), (0, 0, 180)) #self.runTestOfType("down_{0}_".format(speed), (406.787, 304.896, 100.211), (0, 0, 180)) def runTestOfType(self, type, pos, rot): i = 1 while i <= self.testing_times: self.controller.newLogFile("../logs/" + self.test_name + "_" + str(type) + "_" + str(i) + ".txt") self.bng.restart_scenario() self.bng.teleport_vehicle(self.vehicle, pos=pos, rot=rot) self.run() i += 1 def run(self): self.controller.start() while self.controller.ended == False: start_time = datetime.datetime.now() sensors = self.bng.poll_sensors(self.vehicle) wheel_speed = sensors['electrics']['values']['wheelspeed'] value = self.controller.calculate_speed_with_logs(wheel_speed) if value <= 0: value = max(-1, value - 0.1) * -1 self.vehicle.control(brake=value) self.vehicle.control(throttle=0) else: value = min(1, value) self.vehicle.control(brake=0) self.vehicle.control(throttle=value) elapsed_time = datetime.datetime.now() - start_time while (elapsed_time.total_seconds() * 1000) < 100: elapsed_time = datetime.datetime.now() - start_time print("Ending Test") def close(self): self.bng.close()
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
class DataCollector: def __init__(self): self.collected_data = {"image_name": [], "steering": []} self.scenario = Scenario(config.get("data_collecting.scenario_level"), config.get("data_collecting.scenario_name")) self.vehicle = Vehicle( 'ego_vehicle', model=config.get("data_collecting.vehicle_model"), licence='PYTHON') self.bng = BeamNGpy(config.get("beamNG.host"), int(config.get("beamNG.port")), home=config.get("beamNG.home")) def launch_beam_ng(self, mode="manual_mode"): # Create an ETK800 with the licence plate 'PYTHON' electrics = Electrics() # attach to get steering angles self.vehicle.attach_sensor('electrics', electrics) # attach to get images from camera self.vehicle.attach_sensor('front_cam', self.create_camera_sensor()) # Add it to our scenario at this position and rotation # self.scenario.add_vehicle(self.vehicle) self.scenario.add_vehicle( self.vehicle, pos=tuple(map(float, config.get("data_collecting.pos").split(","))), rot_quat=tuple( map(float, config.get("data_collecting.rot_quat").split(",")))) # Place files defining our scenario for the simulator to read self.scenario.make(self.bng) # Launch BeamNG.research self.bng.open() # Load and start our scenario self.bng.load_scenario(self.scenario) self.bng.start_scenario() if mode == "ai_mode": # Make the vehicle's AI span the map self.vehicle.ai_drive_in_lane(True) self.vehicle.ai_set_mode('span') def save_image_manually(self, cam_name='front_cam'): img = self.bng.poll_sensors(self.vehicle)[cam_name]['colour'] steering = self.bng.poll_sensors(self.vehicle)['electrics']['steering'] self.save_data(img, steering) @staticmethod def create_camera_sensor( pos=(-0.3, 2, 1.0), direction=(0, 1, 0), fov=100, res=None): # Set up camera sensor resolution = res if res is None: resolution = (int(config.get("data_collecting.image_width")), int(config.get("data_collecting.image_height"))) pos = pos direction = direction fov = fov front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) return front_camera def save_data(self, img, steering, i: str = "0"): file_name = str(int(time.time())) + i + ".jpg" try: image_path = definitions.ROOT_DIR + config.get( 'data_collecting.data_path') + file_name imageio.imwrite(image_path, np.asarray(img.convert('RGB')), "jpg") self.collected_data["image_name"].append(file_name) self.collected_data["steering"].append(steering) except Exception as ex: logger.info(f"Error while saving data -- {ex}") raise Exception def collect_data(self, number_of_images: int, mode="manual_mode"): self.launch_beam_ng(mode) logger.info("Start after 3 seconds...") time.sleep(5) logger.info( f"Start collecting {config.get('data_collecting.number_of_images')} training images" ) i = 0 exit_normally = True try: while i < number_of_images: # image is training image and steering is label img = self.bng.poll_sensors( self.vehicle)['front_cam']['colour'] steering = self.bng.poll_sensors( self.vehicle)['electrics']['steering'] logger.info(f"Saving data {i + 1} ...") self.save_data(img, steering, str(i)) logger.info("Saved data successfully") i = i + 1 time.sleep(int(config.get("data_collecting.sleep"))) except Exception as ex: exit_normally = False logger.info(f"Error while collecting data {ex}") finally: self.bng.close() return exit_normally def save_csv_data(self): logger.info("Start saving csv file......") csv_path = definitions.ROOT_DIR + config.get( 'data_collecting.csv_path') df = pd.DataFrame(self.collected_data, columns=['image_name', 'steering']) if not os.path.isfile(csv_path): df.to_csv(csv_path, index=False, header=True) else: # else it exists so append without writing the header df.to_csv(csv_path, index=False, mode='a', header=False)
def main(): """ Generate a bunch of images by driving along a predefined sequence of points, while capturing camera images to JPG files. :return: (None) """ bng_home = os.environ['BEAMNG_HOME'] road = TrainingRoad(ASFAULT_PREFAB) road.calculate_road_line(back=True) bng = BeamNGpy('localhost', 64256, home=bng_home) scenario = Scenario('smallgrid', 'train') # Asphalt and lines are actually considered as differently colored roads by beamngpy. scenario.add_road(road.asphalt) scenario.add_road(road.mid_line) scenario.add_road(road.left_line) scenario.add_road(road.right_line) vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON') # Create a dash cam that is somewhat down-sloped. front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV, resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), colour=True, depth=False, annotation=False) vehicle.attach_sensor("front_camera", front_camera) # Get a spawn point and initial rotation of the vehicle. spawn_point = road.spawn_point() scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot()) # Place files defining our scenario for the simulator to read. scenario.make(bng) prefab_path = scenario.get_prefab_path() update_prefab(prefab_path) bng.open() bng.set_nondeterministic() bng.set_steps_per_second(60) # Load and start our scenario bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_mode('span') vehicle.ai_set_speed(5) vehicle.ai_set_line([{ 'pos': node.pos(), 'speed': 10 } for node in road.road_line]) number_of_images = 0 while number_of_images < NUMBER_OF_IMAGES: bng.poll_sensors(vehicle) number_of_images += 1 bng.step(1) sensors = bng.poll_sensors(vehicle) image = sensors['front_camera']['colour'].convert('RGB') image = np.array(image) image = image[:, :, ::-1] dist = road.dist_to_center(vehicle.state['pos']) cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image) bng.close()
class WCARaceGeometry(gym.Env): sps = 50 rate = 5 front_dist = 800 front_step = 100 trail_dist = 104 trail_step = 13 starting_proj = 1710 max_damage = 100 def __init__(self, host='localhost', port=64256): self.steps = WCARaceGeometry.sps // WCARaceGeometry.rate self.host = host self.port = port self.action_space = self._action_space() self.observation_space = self._observation_space() self.episode_steps = 0 self.spine = None self.l_edge = None self.r_edge = None self.polygon = None front_factor = WCARaceGeometry.front_dist / WCARaceGeometry.front_step trail_factor = WCARaceGeometry.trail_dist / WCARaceGeometry.trail_step self.front = lambda step: +front_factor * step self.trail = lambda step: -trail_factor * step self.bng = BeamNGpy(self.host, self.port) self.vehicle = Vehicle('racecar', model='sunburst', licence='BEAMNG', colour='red', partConfig='vehicles/sunburst/hillclimb.pc') electrics = Electrics() damage = Damage() self.vehicle.attach_sensor('electrics', electrics) self.vehicle.attach_sensor('damage', damage) scenario = Scenario('west_coast_usa', 'wca_race_geometry_v0') scenario.add_vehicle(self.vehicle, pos=(394.5, -247.925, 145.25), rot=(0, 0, 90)) scenario.make(self.bng) self.bng.open(launch=True) self.bng.set_deterministic() self.bng.set_steps_per_second(WCARaceGeometry.sps) self.bng.load_scenario(scenario) self._build_racetrack() self.observation = None self.last_observation = None self.last_spine_proj = None self.bng.start_scenario() self.bng.pause() def __del__(self): self.bng.close() def _build_racetrack(self): roads = self.bng.get_roads() track = roads['race_ref'] l_vtx = [] s_vtx = [] r_vtx = [] for right, middle, left in track: r_vtx.append(right) s_vtx.append(middle) l_vtx.append(left) self.spine = LinearRing(s_vtx) self.r_edge = LinearRing(r_vtx) self.l_edge = LinearRing(l_vtx) r_vtx = [v[0:2] for v in r_vtx] l_vtx = [v[0:2] for v in l_vtx] self.polygon = Polygon(l_vtx, holes=[r_vtx]) def _action_space(self): action_lo = [-1., -1.] action_hi = [+1., +1.] return gym.spaces.Box(np.array(action_lo), np.array(action_hi), dtype=float) def _observation_space(self): # n vertices of left and right polylines ahead and behind, 3 floats per # vtx scope = WCARaceGeometry.trail_step + WCARaceGeometry.front_step obs_lo = [ -np.inf, ] * scope * 3 obs_hi = [ np.inf, ] * scope * 3 obs_lo.extend([ -np.inf, # Distance to left edge -np.inf, # Distance to right edge -2 * np.pi, # Inclination -2 * np.pi, # Angle -2 * np.pi, # Vertical angle -np.inf, # Spine speed 0, # RPM -1, # Gear 0, # Throttle 0, # Brake -1.0, # Steering 0, # Wheel speed -np.inf, # Altitude ]) obs_hi.extend([ np.inf, # Distance to left edge np.inf, # Distance to right edge 2 * np.pi, # Inclincation 2 * np.pi, # Angle 2 * np.pi, # Vertical angle np.inf, # Spine speed np.inf, # RPM 8, # Gear 1.0, # Throttle 1.0, # Brake 1.0, # Steering np.inf, # Wheel speed np.inf, # Altitude ]) return gym.spaces.Box(np.array(obs_lo), np.array(obs_hi), dtype=float) def _make_commands(self, action): brake = 0 throttle = action[1] steering = action[0] if throttle < 0: brake = -throttle throttle = 0 self.vehicle.control(steering=steering, throttle=throttle, brake=brake) def _project_vehicle(self, pos): r_proj = self.r_edge.project(pos) r_proj = self.r_edge.interpolate(r_proj) l_proj = self.l_edge.project(r_proj) l_proj = self.l_edge.interpolate(l_proj) s_proj = self.spine.project(r_proj) s_proj = self.spine.interpolate(s_proj) return l_proj, s_proj, r_proj def _get_vehicle_angles(self, vehicle_pos, spine_seg): spine_beg = spine_seg.coords[+0] spine_end = spine_seg.coords[-1] spine_angle = np.arctan2(spine_end[1] - spine_beg[1], spine_end[0] - spine_beg[0]) vehicle_angle = self.vehicle.state['dir'][0:2] vehicle_angle = np.arctan2(vehicle_angle[1], vehicle_angle[0]) vehicle_angle = normalise_angle(vehicle_angle - spine_angle) vehicle_angle -= np.pi elevation = np.arctan2(spine_beg[2] - spine_end[2], spine_seg.length) vehicle_elev = self.vehicle.state['dir'] vehicle_elev = np.arctan2(vehicle_elev[2], np.linalg.norm(vehicle_elev)) return vehicle_angle, vehicle_elev, elevation def _wrap_length(self, target): length = self.spine.length while target < 0: target += length while target > length: target -= length return target def _gen_track_scope_loop(self, it, fn, base, s_scope, s_width): for step in it: distance = base + fn(step) distance = self._wrap_length(distance) s_proj = self.spine.interpolate(distance) s_scope.append(s_proj) l_proj = self.l_edge.project(s_proj) l_proj = self.l_edge.interpolate(l_proj) r_proj = self.r_edge.project(s_proj) r_proj = self.r_edge.interpolate(r_proj) width = l_proj.distance(r_proj) s_width.append(width) def _gen_track_scope(self, pos, spine_seg): s_scope = [] s_width = [] base = self.spine.project(pos) it = range(WCARaceGeometry.trail_step, 0, -1) self._gen_track_scope_loop(it, self.trail, base, s_scope, s_width) it = range(1) self._gen_track_scope_loop(it, lambda x: x, base, s_scope, s_width) it = range(WCARaceGeometry.front_step + 1) self._gen_track_scope_loop(it, self.front, base, s_scope, s_width) s_proj = self.spine.interpolate(base) offset = (-s_proj.x, -s_proj.y, -s_proj.z) s_line = LineString(s_scope) s_line = affinity.translate(s_line, *offset) spine_beg = spine_seg.coords[+0] spine_end = spine_seg.coords[-1] direction = [spine_end[i] - spine_beg[i] for i in range(3)] angle = np.arctan2(direction[1], direction[0]) + np.pi / 2 s_line = affinity.rotate(s_line, -angle, origin=(0, 0), use_radians=True) ret = list() s_scope = s_line.coords for idx in range(1, len(s_scope) - 1): curvature = calculate_curvature(s_scope, idx) inclination = calculate_inclination(s_scope, idx) width = s_width[idx] ret.append(curvature) ret.append(inclination) ret.append(width) return ret def _spine_project_vehicle(self, vehicle_pos): proj = self.spine.project(vehicle_pos) - WCARaceGeometry.starting_proj if proj < 0: proj += self.spine.length proj = self.spine.length - proj return proj def _get_spine_speed(self, vehicle_pos, vehicle_dir, spine_seg): spine_beg = spine_seg.coords[0] future_pos = Point(vehicle_pos.x + vehicle_dir[0], vehicle_pos.y + vehicle_dir[1], vehicle_pos.z + vehicle_dir[2]) spine_end = self.spine.project(future_pos) spine_end = self.spine.interpolate(spine_end) return spine_end.distance(Point(*spine_beg)) def _make_observation(self, sensors): electrics = sensors['electrics']['values'] vehicle_dir = self.vehicle.state['dir'] vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) spine_beg = self.spine.project(vehicle_pos) spine_end = spine_beg spine_end += WCARaceGeometry.front_dist / WCARaceGeometry.front_step spine_beg = self.spine.interpolate(spine_beg) spine_end = self.spine.interpolate(spine_end) spine_seg = LineString([spine_beg, spine_end]) spine_speed = self._get_spine_speed(vehicle_pos, vehicle_dir, spine_seg) l_dist = self.l_edge.distance(vehicle_pos) r_dist = self.r_edge.distance(vehicle_pos) angle, vangle, elevation = self._get_vehicle_angles( vehicle_pos, spine_seg) l_proj, s_proj, r_proj = self._project_vehicle(vehicle_pos) s_scope = self._gen_track_scope(vehicle_pos, spine_seg) obs = list() obs.extend(s_scope) obs.append(l_dist) obs.append(r_dist) obs.append(elevation) obs.append(angle) obs.append(vangle) obs.append(spine_speed) obs.append(electrics['rpm']) obs.append(electrics['gearIndex']) obs.append(electrics['throttle']) obs.append(electrics['brake']) obs.append(electrics['steering']) obs.append(electrics['wheelspeed']) obs.append(electrics['altitude']) return np.array(obs) def _compute_reward(self, sensors): damage = sensors['damage'] vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) if damage['damage'] > WCARaceGeometry.max_damage: return -1, True if not self.polygon.contains(Point(vehicle_pos.x, vehicle_pos.y)): return -1, True score, done = -1, False spine_proj = self._spine_project_vehicle(vehicle_pos) if self.last_spine_proj is not None: diff = spine_proj - self.last_spine_proj if diff >= -0.10: if diff < 0.5: return -1, False else: score, done = diff / self.steps, False elif np.abs(diff) > self.spine.length * 0.95: score, done = 1, True else: score, done = -1, True self.last_spine_proj = spine_proj return score, done def reset(self): self.episode_steps = 0 self.vehicle.control(throttle=0, brake=0, steering=0) self.bng.restart_scenario() self.bng.step(30) self.bng.pause() self.vehicle.set_shift_mode(3) self.vehicle.control(gear=2) sensors = self.bng.poll_sensors(self.vehicle) self.observation = self._make_observation(sensors) vehicle_pos = self.vehicle.state['pos'] vehicle_pos = Point(*vehicle_pos) self.last_spine_proj = self._spine_project_vehicle(vehicle_pos) return self.observation def advance(self): self.bng.step(self.steps, wait=False) def observe(self): sensors = self.bng.poll_sensors(self.vehicle) new_observation = self._make_observation(sensors) return new_observation, sensors def step(self, action): action = [*np.clip(action, -1, 1), action[0], action[1]] action = [float(v) for v in action] self.episode_steps += 1 self._make_commands(action) self.advance() new_observation, sensors = self.observe() if self.observation is not None: self.last_observation = self.observation self.observation = new_observation score, done = self._compute_reward(sensors) print((' A: {:5.2f} B: {:5.2f} ' ' S: {:5.2f} T: {:5.2f} R: {:5.2f}').format( action[2], action[3], action[0], action[1], score)) # if done: # self.bng.step(WCARaceGeometry.sps * 1) return self.observation, score, done, {}
class ImpactGenerator: parts = [ 'etk800_bumper_F', 'etk800_bumperbar_F', 'etk800_bumper_R', 'etk800_fender_L', 'etk800_fender_R', 'etk800_hood', 'etk800_towhitch', 'etk800_steer', 'etk800_radiator', 'etk800_roof_wagon', 'wheel_F_5', ] emptyable = { 'etk800_bumperbar_F', 'etk800_towhitch', } wca = { 'level': 'west_coast_usa', 'a_spawn': (-270.75, 678, 74.9), 'b_spawn': (-260.25, 678, 74.9), 'pole_pos': (-677.15, 848, 75.1), 'linear_pos_a': (-630, 65, 103.4), 'linear_pos_b': (-619, 77, 102.65), 'linear_rot_b': (0, 0, 45.5), 't_pos_a': (-440, 688, 75.1), 't_pos_b': (-453, 700, 75.1), 't_rot_b': (0, 0, 315), 'ref_pos': (-18, 610, 75), } smallgrid = { 'level': 'smallgrid', 'a_spawn': (-270.75, 678, 0.1), 'b_spawn': (-260.25, 678, 0.1), 'pole_pos': (-677.15, 848, 0.1), 'pole': (-682, 842, 0), 'linear_pos_a': (-630, 65, 0.1), 'linear_pos_b': (-619, 77, 0.1), 'linear_rot_b': (0, 0, 45.5), 't_pos_a': (-440, 688, 0.1), 't_pos_b': (-453, 700, 0.1), 't_rot_b': (0, 0, 315), 'ref_pos': (321, 321, 0.1), } def __init__(self, bng_home, output, config, poolsize=2, smallgrid=False, sim_mtx=None, similarity=0.5, random_select=False, single=False): self.bng_home = bng_home self.output = output self.config = config self.smallgrid = smallgrid self.single = single self.impactgen_mats = None if smallgrid: mats = materialmngr.get_impactgen_materials(bng_home) self.impactgen_mats = sorted(list(mats)) self.poolsize = poolsize self.similarity = similarity self.sim_mtx = sim_mtx self.random_select = random_select self.pole_space = None self.t_bone_space = None self.linear_space = None self.nocrash_space = None self.post_space = None self.total_possibilities = 0 self.bng = BeamNGpy('localhost', 64256, home=bng_home) self.scenario = None scenario_props = ImpactGenerator.wca if smallgrid: scenario_props = ImpactGenerator.smallgrid self.vehicle_a = Vehicle('vehicle_a', model='etk800') self.vehicle_b = Vehicle('vehicle_b', model='etk800') self.scenario = Scenario(scenario_props['level'], 'impactgen') self.scenario.add_vehicle(self.vehicle_a, pos=scenario_props['a_spawn'], rot=(0, 0, 0)) self.scenario.add_vehicle(self.vehicle_b, pos=scenario_props['b_spawn'], rot=(0, 0, 0)) self.vehicle_a_parts = defaultdict(set) self.vehicle_a_config = None self.vehicle_b_config = None def generate_colors(self): return copy.deepcopy(self.config['colors']) def generate_nocrash_space(self, props): nocrash_options = [] for part in ImpactGenerator.parts: # Vary each configurable part nocrash_options.append(self.vehicle_a_parts[part]) self.nocrash_space = OptionSpace(nocrash_options) def generate_pole_space(self, props): pole_options = [(False, True)] # Vehicle facing forward/backward pole_options.append(np.linspace(-0.75, 0.75, 5)) # Position offset pole_options.append(np.linspace(0.15, 0.5, 4)) # Throttle intensity for part in ImpactGenerator.parts: # Vary each configurable part pole_options.append(self.vehicle_a_parts[part]) self.pole_space = OptionSpace(pole_options) def generate_t_bone_space(self, props): t_options = [(False, True)] # Vehicle hit left/right t_options.append(np.linspace(-30, 30, 11)) # A rotation offset t_options.append(np.linspace(-1.5, 1.5, 5)) # B pos. offset t_options.append(np.linspace(0.2, 0.5, 4)) # B throttle for part in ImpactGenerator.parts: t_options.append(self.vehicle_a_parts[part]) self.t_bone_space = OptionSpace(t_options) def generate_linear_space(self, props): linear_options = [(False, True)] # Vehicle hit front/back linear_options.append(np.linspace(-15, 15, 5)) # A rot. offset linear_options.append(np.linspace(-1.33, 1.33, 5)) # B pos. offset linear_options.append(np.linspace(0.25, 0.5, 4)) # B throttle for part in ImpactGenerator.parts: linear_options.append(self.vehicle_a_parts[part]) self.linear_space = OptionSpace(linear_options) def get_material_options(self): if not self.random_select: selected = materialmngr.pick_materials(self.impactgen_mats, self.sim_mtx, poolsize=self.poolsize, similarity=self.similarity) if selected is None: log.info('Could not find material pool through similarity. ' 'Falling back to random select.') else: selected = random.sample(self.impactgen_mats, self.poolsize) return selected def generate_post_space(self): colors = self.generate_colors() post_options = [] post_options.append(self.config['times']) if self.smallgrid: post_options.append([0]) post_options.append([0]) else: post_options.append(self.config['clouds']) post_options.append(self.config['fogs']) post_options.append(colors) if self.smallgrid: mats = self.get_material_options() if mats is not None: post_options.append(list(mats)) post_options.append(list(mats)) return OptionSpace(post_options) def generate_spaces(self): props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid self.generate_nocrash_space(props) self.generate_t_bone_space(props) self.generate_linear_space(props) self.generate_pole_space(props) def scan_parts(self, parts, known=set()): with open('out.json', 'w') as outfile: outfile.write(json.dumps(parts, indent=4, sort_keys=True)) for part_type in ImpactGenerator.parts: options = parts[part_type] self.vehicle_a_parts[part_type].update(options) def init_parts(self): self.vehicle_a_config = self.vehicle_a.get_part_config() self.vehicle_b_config = self.vehicle_b.get_part_config() b_parts = self.vehicle_b_config['parts'] b_parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU' b_parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU' b_parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1' options = self.vehicle_a.get_part_options() self.scan_parts(options) for k in self.vehicle_a_parts.keys(): self.vehicle_a_parts[k] = list(self.vehicle_a_parts[k]) if k in ImpactGenerator.emptyable: self.vehicle_a_parts[k].append('') def init_settings(self): self.bng.set_particles_enabled(False) self.generate_spaces() log.info('%s pole crash possibilities.', self.pole_space.count) log.info('%s T-Bone crash possibilities.', self.t_bone_space.count) log.info('%s parallel crash possibilities.', self.linear_space.count) log.info('%s no crash possibilities.', self.nocrash_space.count) self.total_possibilities = \ self.pole_space.count + \ self.t_bone_space.count + \ self.linear_space.count + \ self.nocrash_space.count log.info('%s total incidents possible.', self.total_possibilities) def get_vehicle_config(self, setting): parts = dict() for idx, part in enumerate(ImpactGenerator.parts): parts[part] = setting[idx] refwheel = parts['wheel_F_5'] parts['wheel_R_5'] = refwheel.replace('_F', '_R') # Force licence plate to always be German parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU' parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU' parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1' config = copy.deepcopy(self.vehicle_a_config) config['parts'] = parts return config def set_annotation_paths(self): part_path = os.path.join(self.bng_home, PART_ANNOTATIONS) part_path = os.path.abspath(part_path) obj_path = os.path.join(self.bng_home, OBJ_ANNOTATIONS) obj_path = os.path.abspath(obj_path) req = dict(type='ImpactGenSetAnnotationPaths') req['partPath'] = part_path req['objPath'] = obj_path self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenAnnotationPathsSet' def set_image_properties(self): req = dict(type='ImpactGenSetImageProperties') req['imageWidth'] = self.config['imageWidth'] req['imageHeight'] = self.config['imageHeight'] req['colorFmt'] = self.config['colorFormat'] req['annotFmt'] = self.config['annotFormat'] req['radius'] = self.config['cameraRadius'] req['height'] = self.config['cameraHeight'] req['fov'] = self.config['fov'] self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenImagePropertiesSet' def setup(self): self.scenario.make(self.bng) log.debug('Loading scenario...') self.bng.load_scenario(self.scenario) log.debug('Setting steps per second...') self.bng.set_steps_per_second(50) log.debug('Enabling deterministic mode...') self.bng.set_deterministic() log.debug('Starting scenario...') self.bng.start_scenario() log.debug('Scenario started. Sleeping 20s.') time.sleep(20) self.init_parts() self.init_settings() log.debug('Setting annotation properties.') self.set_annotation_paths() self.set_image_properties() def settings_exhausted(self): return self.t_bone_space.exhausted() and \ self.linear_space.exhausted() and \ self.pole_space.exhausted() and \ self.nocrash_space.exhausted() def set_post_settings(self, vid, settings): req = dict(type='ImpactGenPostSettings') req['ego'] = vid req['time'] = settings[0] req['clouds'] = settings[1] req['fog'] = settings[2] req['color'] = settings[3] if len(settings) > 4: req['skybox'] = settings[4] if len(settings) > 5: req['ground'] = settings[5] self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenPostSet' def finished_producing(self): req = dict(type='ImpactGenOutputGenerated') self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenZipGenerated' return resp['state'] def produce_output(self, color_name, annot_name): while not self.finished_producing(): time.sleep(0.2) req = dict(type='ImpactGenGenerateOutput') req['colorName'] = color_name req['annotName'] = annot_name self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenZipStarted' 'ImpactGenZipStarted' def capture_post(self, crash_setting): log.info('Enumerating post-crash settings and capturing output.') self.bng.switch_vehicle(self.vehicle_a) ref_pos = ImpactGenerator.wca['ref_pos'] if self.smallgrid: ref_pos = ImpactGenerator.smallgrid['ref_pos'] self.bng.teleport_vehicle(self.vehicle_a, ref_pos) self.bng.teleport_vehicle(self.vehicle_b, (10000, 10000, 10000)) self.bng.step(50, wait=True) self.bng.pause() self.post_space = self.generate_post_space() while not self.post_space.exhausted(): post_setting = self.post_space.sample_new() scenario = [[str(s) for s in crash_setting]] scenario.append([str(s) for s in post_setting]) key = str(scenario).encode('ascii') key = hashlib.sha512(key).hexdigest()[:30] t = int(time.time()) color_name = '{}_{}_0_image.zip'.format(t, key) annot_name = '{}_{}_0_annotation.zip'.format(t, key) color_name = os.path.join(self.output, color_name) annot_name = os.path.join(self.output, annot_name) log.info('Setting post settings.') self.set_post_settings(self.vehicle_a.vid, post_setting) log.info('Producing output.') self.produce_output(color_name, annot_name) if self.single: break self.bng.resume() def run_t_bone_crash(self): log.info('Running t-bone crash setting.') if self.t_bone_space.exhausted(): log.debug('T-Bone crash setting exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.t_bone_space.sample_new() side, angle, offset, throttle = setting[:4] config = setting[4:] config = self.get_vehicle_config(config) if side: angle += 225 else: angle += 45 pos_a = props['t_pos_a'] rot_b = props['t_rot_b'] pos_b = list(props['t_pos_b']) pos_b[0] += offset req = dict(type='ImpactGenRunTBone') req['ego'] = self.vehicle_a.vid req['other'] = self.vehicle_b.vid req['config'] = config req['aPosition'] = pos_a req['angle'] = angle req['bPosition'] = pos_b req['bRotation'] = rot_b req['throttle'] = throttle log.debug('Sending t-bone crash config.') self.bng.send(req) log.debug('T-Bone crash response received.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenTBoneRan' return setting def run_linear_crash(self): log.info('Running linear crash setting.') if self.linear_space.exhausted(): log.debug('Linear crash settings exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.linear_space.sample_new() back, angle, offset, throttle = setting[:4] config = setting[4:] config = self.get_vehicle_config(config) if back: angle += 225 else: offset += 1.3 angle += 45 pos_a = props['linear_pos_a'] rot_b = props['linear_rot_b'] pos_b = list(props['linear_pos_b']) pos_b[0] += offset req = dict(type='ImpactGenRunLinear') req['ego'] = self.vehicle_a.vid req['other'] = self.vehicle_b.vid req['config'] = config req['aPosition'] = pos_a req['angle'] = angle req['bPosition'] = pos_b req['bRotation'] = rot_b req['throttle'] = throttle log.debug('Sending linear crash config.') self.bng.send(req) log.debug('Linear crash response received.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenLinearRan' return setting def run_pole_crash(self): log.info('Running pole crash setting.') if self.pole_space.exhausted(): log.debug('Pole crash settings exhausted.') return None props = ImpactGenerator.wca if self.smallgrid: props = ImpactGenerator.smallgrid setting = self.pole_space.sample_new() back, offset, throttle = setting[:3] config = setting[3:] config = self.get_vehicle_config(config) angle = 45 if back: angle = 225 offset += 0.85 throttle = -throttle pos = list(props['pole_pos']) pos[0] += offset req = dict(type='ImpactGenRunPole') req['ego'] = self.vehicle_a.vid req['config'] = config req['position'] = pos req['angle'] = angle req['throttle'] = throttle if self.smallgrid: req['polePosition'] = props['pole'] log.debug('Sending pole crash config.') self.bng.send(req) log.debug('Got pole crash response.') resp = self.bng.recv() assert resp['type'] == 'ImpactGenPoleRan' return setting def run_no_crash(self): log.info('Running non-crash scenario.') if self.nocrash_space.exhausted(): return None setting = self.nocrash_space.sample_new() log.info('Got new setting: %s', setting) vehicle_config = self.get_vehicle_config(setting) log.info('Got new vehicle config: %s', vehicle_config) req = dict(type='ImpactGenRunNonCrash') req['ego'] = self.vehicle_a.vid req['config'] = vehicle_config log.info('Sending Non-Crash request: %s', req) self.bng.send(req) resp = self.bng.recv() assert resp['type'] == 'ImpactGenNonCrashRan' log.info('Non-crash finished.') return setting def run_incident(self, incident): log.info('Setting up next incident.') self.bng.display_gui_message('Setting up next incident...') setting = incident() self.capture_post(setting) return setting def run_incidents(self): log.info('Enumerating possible incidents.') count = 1 incidents = [ self.run_t_bone_crash, self.run_linear_crash, self.run_pole_crash, self.run_no_crash, ] while not self.settings_exhausted(): log.info('Running incident %s of %s...', count, self.total_possibilities) self.bng.restart_scenario() log.info('Scenario restarted.') time.sleep(5.0) self.vehicle_b.set_part_config(self.vehicle_b_config) log.info('Vehicle B config set.') incident = incidents[count % len(incidents)] if self.run_incident(incident) is None: log.info('Ran out of options for: %s', incident) incidents.remove(incident) # Possibility space exhausted count += 1 def run(self): log.info('Starting up BeamNG instance.') self.bng.open(['impactgen/crashOutput']) self.bng.skt.settimeout(1000) try: log.info('Setting up BeamNG instance.') self.setup() self.run_incidents() finally: log.info('Closing BeamNG instance.') self.bng.close()
def collect_data(beamNG_path): bng = BeamNGpy('localhost', 64256, beamNG_path) #bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk') scenario = Scenario('GridMap', 'example') # Create an ETK800 with the licence plate 'PYTHON' vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red') #vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue') electrics = Electrics() vehicle1.attach_sensor('electrics', electrics) pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775) rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936) scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1) # Place files defining our scenario for the simulator to read scenario.make(bng) # Launch BeamNG.research bng.open(launch=True) #SIZE = 50 try: bng.load_scenario(scenario) bng.start_scenario() #vehicle1.ai_set_speed(10.452066507339481,mode = 'set') vehicle1.ai_set_mode('span') #collect data print("\n Position and rotation of car \n ") # for _ in range(200): for _ in range(200): time.sleep(0.1) vehicle1.update_vehicle( ) # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors( vehicle1 ) # Polls the data of all sensors attached to the vehicle positions.append(vehicle1.state['pos']) directions.append(vehicle1.state['dir']) print([vehicle1.state['pos'], vehicle1.state['dir']]) #write data into file # print ("position :",positions) # print ("position :",directions) sys_output.print_title( "\n The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \"" ) write_data(FILE_POS, positions) write_data(FILE_ROT, directions) bng.stop_scenario() bng.close() time.sleep(2) finally: bng.close() return (positions, directions)
class Driver: vehicle = None bng = None labeler = None left_path = None right_path = None driving_path = None road_profiler = None car_model = None road_model = None def __init__(self, car_model: dict, road_model: dict, control_model: dict): self.car_model = car_model self.road_model = road_model self.control_model = control_model # Note: Speed limit must be m/s self.road_profiler = RoadProfiler( road_model['mu'], road_model['speed_limit'] / 3.6, control_model['discretization_factor']) def _compute_driving_path(self, car_state, road_name): road_geometry = self.bng.get_road_edges(road_name) left_edge_x = np.array([e['left'][0] for e in road_geometry]) left_edge_y = np.array([e['left'][1] for e in road_geometry]) right_edge_x = np.array([e['right'][0] for e in road_geometry]) right_edge_y = np.array([e['right'][1] for e in road_geometry]) road_edges = dict() road_edges['left_edge_x'] = left_edge_x road_edges['left_edge_y'] = left_edge_y road_edges['right_edge_x'] = right_edge_x road_edges['right_edge_y'] = right_edge_y self.right_edge = LineString( zip(road_edges['right_edge_x'][::-1], road_edges['right_edge_y'][::-1])) self.left_edge = LineString( zip(road_edges['left_edge_x'], road_edges['left_edge_y'])) current_position = Point(car_state['pos'][0], car_state['pos'][1]) from shapely.ops import nearest_points from shapely.affinity import rotate projection_point_on_right = nearest_points(self.right_edge, current_position)[0] projection_point_on_left = nearest_points(self.left_edge, current_position)[0] # If the car is closest to the left, then we need to switch the direction of the road... if current_position.distance( projection_point_on_right) > current_position.distance( projection_point_on_left): # Swap the axis and recompute the projection points l.debug("Reverse traffic direction") temp = self.right_edge self.right_edge = self.left_edge self.left_edge = temp del temp projection_point_on_right = nearest_points(self.right_edge, current_position)[0] projection_point_on_left = nearest_points(self.left_edge, current_position)[0] # Traffic direction is always 90-deg counter clockwise from right # Now rotate right point 90-deg counter clockwise from left and we obtain the traffic direction rotated_right = rotate(projection_point_on_right, 90.0, origin=projection_point_on_left) # Vector defining the direction of the road traffic_direction = np.array([ rotated_right.x - projection_point_on_left.x, rotated_right.y - projection_point_on_left.y ]) # Find the segment containing the projection of current location # Starting point on right edge start_point = None for pair in pairs(list(self.right_edge.coords[:])): segment = LineString([pair[0], pair[1]]) # xs, ys = segment.coords.xy # plt.plot(xs, ys, color='green') if segment.distance(projection_point_on_right) < 1.8e-5: road_direction = np.array( [pair[1][0] - pair[0][0], pair[1][1] - pair[0][1]]) if dot(traffic_direction, road_direction) < 0: l.debug("Reverse order !") self.right_edge = LineString([ Point(p[0], p[1]) for p in self.right_edge.coords[::-1] ]) start_point = Point(pair[0][0], pair[0][1]) break else: l.debug("Original order !") start_point = Point(pair[1][0], pair[1][1]) break assert start_point is not None # At this point compute the driving path of the car (x, y, t) self.driving_path = [current_position] # plt.plot(current_position.x, current_position.y, color='black', marker="x") # # This might not be robust we need to get somethign close by # plt.plot([pair[0][0], pair[1][0]], [pair[0][1], pair[1][1]], marker="o") # plt.plot(projection_point_on_right.x, projection_point_on_right.y, color='b', marker="*") # started = False for right_position in [ Point(p[0], p[1]) for p in list(self.right_edge.coords) ]: if right_position.distance(start_point) < 1.8e-5: # print("Start to log positions") # plt.plot(right_position.x, right_position.y, color='blue', marker="o") started = True if not started: # print("Skip point") # plt.plot(right_position.x, right_position.y, color='red', marker="*") continue else: # print("Consider point") # plt.plot(right_position.x, right_position.y, color='green', marker="o") pass # Project right_position to left_edge projected_point = self.left_edge.interpolate( self.left_edge.project(right_position)) # Translate the right_position 2m toward the center line = LineString([(right_position.x, right_position.y), (projected_point.x, projected_point.y)]) self.driving_path.append(line.interpolate(2.0)) def plot_all(self, car_state): current_position = Point(car_state['pos'][0], car_state['pos'][1]) plt.figure(1, figsize=(5, 5)) plt.clf() ax = plt.gca() x, y = self.left_edge.coords.xy ax.plot(x, y, 'r-') x, y = self.right_edge.coords.xy ax.plot(x, y, 'b-') driving_lane = LineString([p for p in self.driving_path]) x, y = driving_lane.coords.xy ax.plot(x, y, 'g-') # node = { # 'x': target_position.x, # 'y': target_position.y, # 'z': 0.3, # 't': target_time, # } xs = [node['x'] for node in self.script] ys = [node['y'] for node in self.script] # print("{:.2f}".format(3.1415926)); vs = ['{:.2f}'.format(node['avg_speed'] * 3.6) for node in self.script] # plt.plot(xs, ys, marker='.') ax = plt.gca() for i, txt in enumerate(vs): ax.annotate(txt, (xs[i], ys[i])) plt.plot(current_position.x, current_position.y, marker="o", color="green") # Center around current_positions ax.set_xlim([current_position.x - 50, current_position.x + 50]) ax.set_ylim([current_position.y - 50, current_position.y + 50]) plt.draw() plt.pause(0.01) def run(self, debug=False): try: self.vehicle = Vehicle(car_model['id']) electrics = Electrics() self.vehicle.attach_sensor('electrics', electrics) # Connect to running beamng self.bng = BeamNGpy( 'localhost', 64256 ) #, home='C://Users//Alessio//BeamNG.research_unlimited//trunk') self.bng = self.bng.open(launch=False) # Put simulator in pause awaiting while planning the driving self.bng.pause() # Connect to the existing vehicle (identified by the ID set in the vehicle instance) self.bng.set_deterministic() # Set simulator to be deterministic self.bng.connect_vehicle(self.vehicle) assert self.vehicle.skt # Get Initial state of the car. This assumes that the script is invoked after the scenario is started self.bng.poll_sensors(self.vehicle) # Compute the "optimal" driving path and program the ai_script self._compute_driving_path(self.vehicle.state, self.road_model['street']) self.script = self.road_profiler.compute_ai_script( LineString(self.driving_path), self.car_model) # Enforce initial car direction nad up start_dir = (self.vehicle.state['dir'][0], self.vehicle.state['dir'][1], self.vehicle.state['dir'][2]) up_dir = (0, 0, 1) # Configure the ego car self.vehicle.ai_set_mode('disabled') # Note that set script teleports the car by default self.vehicle.ai_set_script(self.script, start_dir=start_dir, up_dir=up_dir) # Resume the simulation self.bng.resume() # At this point the controller can stop ? or wait till it is killed while True: if debug: self.bng.pause() self.bng.poll_sensors(self.vehicle) self.plot_all(self.vehicle.state) self.bng.resume() # Progress the simulation for some time... # self.bng.step(50) sleep(2) except Exception: # When we brutally kill this process there's no need to log an exception l.error("Fatal Error", exc_info=True) finally: self.bng.close()
class Simulation(object): def __init__(self) -> None: super().__init__() thread = Thread(target=self._intervene) thread.start() self.step = 0 self.dist_driven = 0 self.done = False self.last_action = (0.0, 0.0) self.bng = BeamNGpy('localhost', 64257, home=BEAMNG_HOME) self.scenario = Scenario('train', 'train', authors='Vsevolod Tymofyeyev', description='Reinforcement learning') self.road = TrainingRoad(ASFAULT_PREFAB) self.road.calculate_road_line() spawn_point = self.road.spawn_point() self.last_pos = spawn_point.pos() self.scenario.add_road(self.road.asphalt) self.scenario.add_road(self.road.mid_line) self.scenario.add_road(self.road.left_line) self.scenario.add_road(self.road.right_line) self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='RL FTW', color='Red') front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV, resolution=(CAMERA_WIDTH, CAMERA_HEIGHT), colour=True, depth=False, annotation=False) self.vehicle.attach_sensor("front_camera", front_camera) self.scenario.add_vehicle(self.vehicle, pos=spawn_point.pos(), rot=spawn_point.rot()) self.scenario.make(self.bng) prefab_path = self.scenario.get_prefab_path() update_prefab(prefab_path) self.bng.open() self.bng.set_deterministic() self.bng.set_steps_per_second(SPS) self.bng.load_scenario(self.scenario) self.bng.start_scenario() # self.bng.hide_hud() self.bng.pause() def _intervene(self): while True: a = input() self.done = not self.done def take_action(self, action): steering, throttle = action steering = steering.item() throttle = throttle.item() self.last_action = action self.step += 1 self.vehicle.control(steering=steering, throttle=throttle, brake=0.0) self.bng.step(STEPS_INTERVAL) def _reward(self, done, dist): steering = self.last_action[0] throttle = self.last_action[1] velocity = self.velocity() # km/h if not done: reward = REWARD_STEP + THROTTLE_REWARD_WEIGHT * throttle #- MID_DIST_PENALTY_WEIGHT * dist else: reward = REWARD_CRASH - CRASH_SPEED_WEIGHT * throttle return reward def observe(self): sensors = self.bng.poll_sensors(self.vehicle) image = sensors['front_camera']['colour'].convert("RGB") image = np.array(image) r = ROI # Cut to the relevant region image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])] # Convert to BGR state = image[:, :, ::-1] #done = self.done pos = self.vehicle.state['pos'] dir = self.vehicle.state['dir'] self.refresh_dist(pos) self.last_pos = pos dist = self.road.dist_to_center(self.last_pos) #velocity = self.velocity() done = dist > MAX_DIST #or velocity > MAX_VELOCITY reward = self._reward(done, dist) return state, reward, done, {} def velocity(self): state = self.vehicle.state velocity = np.linalg.norm(state["vel"]) return velocity * 3.6 def position(self): return self.vehicle.state["pos"] def refresh_dist(self, pos): pos = np.array(pos) last_pos = np.array(self.last_pos) dist = np.linalg.norm(pos - last_pos) self.dist_driven += dist def close_connection(self): self.bng.close() def reset(self): self.vehicle.control(throttle=0, brake=0, steering=0) self.bng.poll_sensors(self.vehicle) self.dist_driven = 0 self.step = 0 self.done = False current_pos = self.vehicle.state['pos'] closest_point = self.road.closest_waypoint(current_pos) #closest_point = self.road.random_waypoint() self.bng.teleport_vehicle(vehicle=self.vehicle, pos=closest_point.pos(), rot=closest_point.rot()) self.bng.pause() # TODO delete def wait(self): from client.aiExchangeMessages_pb2 import SimStateResponse return SimStateResponse.SimState.RUNNING
class BeamNGBrewer: def __init__(self, beamng_home=None, beamng_user=None, reuse_beamng=True, road_nodes: List4DTuple = None): self.reuse_beamng = reuse_beamng if self.reuse_beamng: # This represents the running BeamNG simulator. Since we use launch=True this should automatically # shut down when the main python process exits or when we call self.beamng_process.stop() self.beamng_process = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user) self.beamng_process = self.beamng_process.open(launch=True) # This is used to bring up each simulation without restarting the simulator self.beamng = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user) # We need to wait until this point otherwise the BeamNG loggers's level will be (re)configured by BeamNGpy log.info("Disabling BEAMNG logs") for id in [ "beamngpy", "beamngpy.beamngpycommon", "beamngpy.BeamNGpy", "beamngpy.beamng", "beamngpy.Scenario", "beamngpy.Vehicle" ]: logger = log.getLogger(id) logger.setLevel(log.CRITICAL) logger.disabled = True self.vehicle: Vehicle = None self.camera: BeamNGCamera = None if road_nodes: self.setup_road_nodes(road_nodes) steps = 5 self.params = SimulationParams(beamng_steps=steps, delay_msec=int(steps * 0.05 * 1000)) self.vehicle_start_pose = BeamNGPose() def setup_road_nodes(self, road_nodes): self.road_nodes = road_nodes self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points( road_nodes) self.road_points = RoadPoints().add_middle_nodes(road_nodes) def setup_vehicle(self) -> Vehicle: assert self.vehicle is None self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red') return self.vehicle def setup_scenario_camera(self, resolution=(1280, 1280), fov=120) -> BeamNGCamera: assert self.camera is None self.camera = BeamNGCamera(self.beamng, 'brewer_camera') return self.camera # TODO COnsider to transform brewer into a ContextManager or get rid of it... def bring_up(self): if self.reuse_beamng: # This assumes BeamNG is already running self.beamng.open(launch=False) else: self.beamng.open(launch=True) # After 1.18 to make a scenario one needs a running instance of BeamNG self.scenario = Scenario('tig', 'tigscenario') if self.vehicle: self.scenario.add_vehicle(self.vehicle, pos=self.vehicle_start_pose.pos, rot=self.vehicle_start_pose.rot) if self.camera: self.scenario.add_camera(self.camera.camera, self.camera.name) self.scenario.make(self.beamng) self.beamng.set_deterministic() self.beamng.load_scenario(self.scenario) self.beamng.start_scenario() # Pause the simulator only after loading and starting the scenario self.beamng.pause() def __del__(self): if self.beamng: try: self.beamng.close() except: pass
def main() -> None: # Read CommonRoad scenario cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \ .open() if cr_planning_problem_set: for _, pp in cr_planning_problem_set.planning_problem_dict.items(): cr_planning_problem = pp break # Read only the first one else: raise Exception( "The given CommonRoad scenario does not define a planning problem." ) # Setup BeamNG bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path) bng_scenario = Scenario( bng_scenario_environment, bng_scenario_name, authors='Stefan Huber', description='Simple visualization of the CommonRoad scenario ' + cr_scenario_name) # Add lane network lanes = cr_scenario.lanelet_network.lanelets for lane in lanes: lane_nodes = [] for center in lane.center_vertices: lane_nodes.append((center[0], center[1], 0, 4)) # FIXME Determine appropriate width road = Road(cr_road_material) road.nodes.extend(lane_nodes) bng_scenario.add_road(road) # Add ego vehicle ego_vehicle = Vehicle('ego_vehicle', model='etk800', licence='EGO', color='Cornflowerblue') ego_init_state = cr_planning_problem.initial_state ego_init_state.position[0] = 82.8235 ego_init_state.position[1] = 31.5786 add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state, etk800_z_offset) obstacles_to_move = dict() # Add truck semi = Vehicle('truck', model='semi', color='Red') semi_init_state = cr_scenario.obstacle_by_id(206).initial_state add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state, semi_z_offset) obstacles_to_move[206] = semi # Add truck trailer tanker_init_state = copy(semi_init_state) tanker_init_state.position += [6, 3] add_vehicle_to_bng_scenario( bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'), tanker_init_state, tanker_z_offset) # Add other traffic participant opponent = Vehicle('opponent', model='etk800', licence='VS', color='Cornflowerblue') add_vehicle_to_bng_scenario(bng_scenario, opponent, cr_scenario.obstacle_by_id(207).initial_state, etk800_z_offset) obstacles_to_move[207] = opponent # Add static obstacle obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape obstacle_pos = obstacle_shape.center obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset) for w in range(3): for h in range(3): for d in range(2): obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d), model='haybale', color='Red') haybale_pos_diff = obstacle_pos \ + pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \ + pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad) bng_scenario.add_vehicle(obstacle, pos=(haybale_pos_diff[0], haybale_pos_diff[1], h * 1), rot=(0, 0, obstacle_rot_deg)) bng_scenario.make(bng) # Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported) prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment, 'scenarios', bng_scenario_name + '.prefab') lines = open(prefab_path, 'r').readlines() for i in range(len(lines)): if 'overObjects' in lines[i]: lines[i] = lines[i].replace('0', '1') open(prefab_path, 'w').writelines(lines) # Start simulation bng.open(launch=True) try: bng.load_scenario(bng_scenario) bng.start_scenario() bng.pause() bng.display_gui_message( "The scenario is fully prepared and paused. You may like to position the camera first." ) delay_to_resume = 15 input("Press enter to continue the simulation... You have " + str(delay_to_resume) + " seconds to switch back to the BeamNG window.") sleep(delay_to_resume) bng.resume() for id, obstacle in obstacles_to_move.items(): obstacle.ai_drive_in_lane(False) obstacle.ai_set_line( generate_node_list(cr_scenario.obstacle_by_id(id))) ego_vehicle.ai_drive_in_lane(False) # ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit') speed = 65 / 3.6 ego_vehicle.ai_set_line([{ 'pos': ego_vehicle.state['pos'] }, { 'pos': (129.739, 56.907, etk800_z_offset), 'speed': speed }, { 'pos': (139.4, 62.3211, etk800_z_offset), 'speed': speed }, { 'pos': (149.442, 64.3655, etk800_z_offset), 'speed': speed }, { 'pos': (150.168, 63.3065, etk800_z_offset), 'speed': speed }, { 'pos': (188.495, 78.8517, etk800_z_offset), 'speed': speed }]) # FIXME Setting the speed does not work as expected # ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set') input("Press enter to end simulation...") finally: bng.close()
vehicle.ai_set_mode('disabled') vehicle.update_vehicle() sensors = bng.poll_sensors(vehicle) loopStartTime=datetime.datetime.now() for x in range(testTime*dataRate): loopIterationStartTime=time.time() vehicle.update_vehicle() # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicle) # Polls the data of all sensors attached to the vehicle data=VehicleData(sensors['electrics'],sensors['damage'],sensors['GForces'],vehicle.state['pos'], vehicle.state['dir'],sensors['electrics']['steering']).getData() data={'time':str(((datetime.datetime.now()-loopStartTime))),'data':data} print(json.dumps(data)) loopExecTime=(time.time() - loopIterationStartTime) if(actualisationTime>loopExecTime): time.sleep(actualisationTime-loopExecTime) beamng.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()