def run_scenario_ai_version(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config='vehicles/hopper/custom.pc'): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec, prev_error random.seed(1703) setup_logging() home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.research.v1.7.0.1untouched/BeamNG.research.v1.7.0.1' #'H:/BeamNG.tech.v0.21.3.0' # beamng = BeamNGpy('localhost', 64256, home=home) #, user='******') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='AI', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # create vehicle to be chased # chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red') # bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None, # rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666)) # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() ai_line, bng = create_ai_line_from_road(spawn, bng) # ai_line, bng = create_ai_line_from_centerline(bng) # ai_line, bng = create_ai_line(bng) vehicle.ai_set_script(ai_line, cling=True) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # bng.resume() # vehicle.ai_set_mode('chase') # vehicle.ai_set_target('chase_vehicle') # vehicle.ai_set_mode("traffic") # vehicle.ai_set_speed(12, mode='set') # vehicle.ai_drive_in_lane(True) damage_prev = None runtime = 0.0 traj = [] kphs = [] # with open("ai_lap_data.txt", 'w') as f: for _ in range(1024): sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') damage = sensors['damage'] wheelspeed = sensors['electrics']['wheelspeed'] new_damage = diff_damage(damage, damage_prev) damage_prev = damage runtime = sensors['timer']['time'] vehicle.update_vehicle() traj.append(vehicle.state['pos']) # f.write("{}\n".format(vehicle.state['pos'])) kphs.append(ms_to_kph(wheelspeed)) if new_damage > 0.0: break # if distance(spawn['pos'], vehicle.state['pos']) < 3 and sensors['timer']['time'] > 90: # reached_start = True # plt.imshow(image) # plt.show() # break bng.step(1) bng.close() plot_trajectory(traj, "AI Lap") results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': image } return results
def run_scenario(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config=None): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec global integral, prev_error integral = 0.0 prev_error = 0.0 # setup DNN model + weights # m = Model() # model = m.define_model_BeamNG("BeamNGmodel-racetrack.h5") random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean') scenario = Scenario(default_scenario, 'research_test') # unperturbed_vehicle = Vehicle('unperturbed_vehicle', model=vehicle_model, # licence='SAFE', color='Red') # unperturbed_vehicle = setup_sensors(unperturbed_vehicle) vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='EGO', color=default_color) vehicle = setup_sensors(vehicle) spawn = get_spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) temp = copy.deepcopy(spawn['pos']) temp = [temp[0] + lanewidth, temp[1] + lanewidth, temp[2]] # scenario.add_vehicle(unperturbed_vehicle, pos=temp, rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) bng = beamng.open(launch=True) #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() #bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt bng.resume() # perturb vehicle vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() damage = 0 runtime = 0 while damage <= 0: sensors = bng.poll_sensors(vehicle) headlight_img = sensors['headlight_cam']['colour'].convert('RGB') # sensors[''] # prediction = model.predict() # steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0) # vehicle.control(throttle=throttle, steering=steering, brake=brake) return_str = '\nPERTURBED headlight_cam INFO:' print('\nPERTURBED headlight_cam INFO:') temp = bng.poll_sensors(vehicle)['headlight_cam'] for key in temp: if key == 'rotation': degs = euler_from_quaternion(temp[key][0], temp[key][1], temp[key][2], temp[key][3]) return_str = "{}\nquaternions {}".format(return_str, temp[key]) return_str = "{}\n{} {}".format(return_str, key, [round(i, 3) for i in degs]) print(key, degs) elif key != "colour" and key != "annotation" and key != "depth": return_str = "{}\n{} {}".format(return_str, key, temp[key]) print(key, temp[key]) print('\nPERTURBED rearview_cam INFO:') return_str = "{}\nPERTURBED rearview_cam INFO:".format(return_str) # temp = bng.poll_sensors(vehicle)['rearview_cam'] for key in temp: if key == 'rotation': degs = euler_from_quaternion(temp[key][0], temp[key][1], temp[key][2], temp[key][3]) return_str = "{}\nquaternions {}".format(return_str, temp[key]) return_str = "{}\n{} {}".format(return_str, key, [round(i, 3) for i in degs]) print(key, degs) elif key != "colour" and key != "annotation" and key != "depth": return_str = "{}\n{} {}".format(return_str, key, temp[key]) print(key, temp[key]) # rearview_img = bng.poll_sensors(vehicle)['rearview_cam']['colour'].convert('RGB') headlight_img = sensors['headlight_cam']['colour'].convert('RGB') bng.step(1) damage = sensors['damage']['damage'] runtime = sensors['timer']['time'] # print("runtime:{}".format(round(runtime, 2))) print("time to crash:{} damage:{}".format(round(runtime, 2), damage)) bng.close() # avg_kph = float(sum(kphs)) / len(kphs) # plt.imshow(rearview_img) # plt.pause(0.01) plt.imshow(headlight_img) plt.pause(0.01) # results = {'pitch': round(pitch,3), 'roll':round(roll,3), "z":round(z,3), 'rearview_img':rearview_img, 'headlight_img':headlight_img} return return_str
] csv_file = "pos_crash_analysis.csv" try: with open(csv_file, 'a', encoding='utf-8') as csvfile: writer = csv.DictWriter(csvfile, fieldnames=csv_columns, delimiter=',', lineterminator='\n') writer.writerow(pos_crash_dict) except IOError: print("I/O error") # ------------------------------------------------------------------------- beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN') scenario = Scenario('GridMap', 'crash_simulation_3') road_a = Road('custom_track_center', looped=False) collision_point = [] three_way = [] # 3 Way intersection for tup in graph_degree: three_way_coordinate = [] if tup[1] == 3: three_way_coordinate.append(node_dict[tup[0]]) three_way_points = graph.neighbors(tup[0]) way_geo = (tup[0], node_dict[tup[0]], lane_dict[tup[0]], width_dict[tup[0]])
bng.teleport_vehicle(pickup.vid, pos, rot=(0, 45, 0)) pickup.poll_sensors() pos_after = pickup.state['pos'] assert (pos_before != pos_after) pickup.poll_sensors() pos_before = pickup.state['pos'] rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229) bng.teleport_vehicle(pickup.vid, pos, rot_quat=rot_quat) pickup.poll_sensors() pos_after = pickup.state['pos'] assert (pos_before != pos_after) try: bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0)) assert True except socket.timeout: assert False try: rot_quat = (-0.003337, -0.0021882, -0.6891695, 0.7245892) bng.teleport_scenario_object(rb, (-10, 5, 0), rot_quat=rot_quat) assert True except socket.timeout: assert False if __name__ == '__main__': bng = BeamNGpy('localhost', 64256) test_quats(bng)
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()
def main(): random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') state_cfg = Config() parts_cfg = Config() # Create a scenario in west_coast_usa scenario = Scenario('west_coast_usa', 'research_test', description='Random driving for research') # Set up first vehicle, with two cameras, gforces sensor, lidar, electrical # sensors, and damage sensors vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') vehicle = setup_sensors(vehicle) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795)) # Compile the scenario and place it in BeamNG's map folder #scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) try: #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() pcfg = vehicle.get_part_config() assert vehicle.skt # Send random inputs to vehicle and advance the simulation 20 steps #for _ in range(1024): for _ in range(30): throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1.0, 1.0) brake = random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) # bng.step(20) bng.step(20) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) print('\n{} seconds passed.'.format(sensors['timer']['time'])) print("step in loop {}".format(_)) finally: sensors = bng.poll_sensors(vehicle) for s in sensors.keys(): print("{} : {}".format(s, sensors[s])) damage_dict = sensors['damage'] state_cfg.update(sensors['damage']) state_cfg.update(vehicle.state) state_cfg.save('{}/state_cfg.json'.format(beamng.home)) vehicle.annotate_parts() vehicle.update_vehicle() #part_options = vehicle.get_part_options() parts_cfg_dict = vehicle.get_part_config() parts_cfg.load_values(vehicle.get_part_config()) #parts_cfg.update(vehicle.get_part_config()) parts_cfg.save('{}/parts_cfg.json'.format(beamng.home)) vehicle.save() bng.close() # reload scenario with saved config random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') loaded_config = state_cfg.load("{}/state_cfg.json".format(beamng.home)) scenario = Scenario('west_coast_usa', 'research_test', description='Random driving for research') vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') vehicle = setup_sensors(vehicle) vehicle.set_part_config(parts_cfg) scenario.add_vehicle(vehicle, pos=tuple(state_cfg.pos), rot=None, rot_quat=beamngpy.angle_to_quat(state_cfg.dir)) scenario.make(beamng) bng = beamng.open(launch=True) bng.spawn_vehicle(vehicle, pos=tuple(state_cfg.pos), rot=None, rot_quat=beamngpy.angle_to_quat(state_cfg.dir)) # TODO: debug scenario restart # scenario.restart() try: bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt vehicle.load() # Send random inputs to vehicle and advance the simulation 20 steps #for _ in range(1024): for _ in range(30): throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1.0, 1.0) brake = random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) # bng.step(20) bng.step(20) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) print('\n{} seconds passed.'.format(sensors['timer']['time'])) print("step in loop {}".format(_)) finally: sensors = bng.poll_sensors(vehicle) for s in sensors.keys(): print("{} : {}".format(s, sensors[s])) state_cfg.update(sensors['damage']) state_cfg.update(vehicle.state) state_cfg.save('{}/state_cfg.json'.format(beamng.home)) parts_cfg.update(vehicle.get_part_config()) parts_cfg.save('{}/parts_cfg.json'.format(beamng.home)) bng.close()
def run_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()
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)
#%matplotlib inline import matplotlib import matplotlib.pyplot as plt # for later visualization import numpy as np # for easier vector computations from beamngpy import BeamNGpy, Scenario, Vehicle from beamngpy.sensors import Camera import os beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1') # Create a vehicle with a camera sensor attached to it vehicle = Vehicle('ego_vehicle', model='etki', licence='PYTHON', color='Green') cam_pos = np.array([0, 0, 0]) # placeholder values that will be recomputed later cam_dir = np.array([0, 0, 0]) # placeholder values that will be recomputed later cam_fov = 70 cam_res = (1024, 1024) #(512, 512) camera = Camera(cam_pos, cam_dir, cam_fov, cam_res, colour=True) vehicle.attach_sensor('camera', camera) # Simple scenario with the vehicle we just created standing at the origin car_pos = np.array([0, 0, 0]) scenario = Scenario( 'smallgrid', 'multishot', description='Demo of the camera sensor used like a multishot camera')
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()
activatedSensors[x] = True valid = True elif (choice == 'n'): activatedSensors[x] = False valid = True elif (choice != 'n' and choice != 'y'): print("Please only enter 'y' for yes or 'n' for no") #endregion sns.set() # Make seaborn set matplotlib styling beamNGPAth = "C:/Users/julie/Desktop/beamng" # Instantiate a BeamNGpy instance the other classes use for reference & communication beamng = BeamNGpy( 'localhost', 64256, beamNGPAth) # This is the host & port used to communicate over # Create a vehicle instance that will be called 'ego' in the simulation # using the etk800 model the simulator ships with vehicle = Vehicle('ego', model='etkc', licence='LIFLAB', colour='Blue') # Create an Electrics sensor and attach it to the vehicle electrics = Electrics() vehicle.attach_sensor('electrics', electrics) #Create a Damage sensor and attach it to the vehicle damage = Damage() vehicle.attach_sensor('damage', damage)
def main(MAX_SPEED): setup_logging() # Gains to port TORCS actuators to BeamNG # steering_gain = translate_steering() acc_gain = 0.5 # 0.4 brake_gain = 1.0 # BeamNG images are too bright for DeepDrive brightness = 0.4 # Set up first vehicle # ! A vehicle with the name you specify here has to exist in the scenario vehicle = Vehicle('egovehicle') # Set up sensors # Set up sensors resolution = (280, 210) # Original Settings #pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below # 0.4 is inside pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below direction = (0, 1, 0) # direction = (180, 0, 180) # FOV 60, MAX_SPEED 100, 20 (3) Hz fails # FOV 60, MAX_SPEED 80, 20 (3) Hz Ok # FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations # FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations # FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower # FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower fov = 60 # MAX_SPEED = 70 MAX_FPS = 60 SIMULATION_STEP = 6 # Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes ! # # WORKING SETTINGS: 20 Freq, 90 FOV. front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) electrics = Electrics() vehicle.attach_sensor('front_cam', front_camera) vehicle.attach_sensor('electrics', electrics) # Setup the SHM with DeepDrive # Create shared memory object Memory = sd.CSharedMemory(TargetResolution=[280, 210]) # Enable Pause-Mode Memory.setSyncMode(True) Memory.Data.Game.UniqueRaceID = int(time.time()) print("Setting Race ID at ", Memory.Data.Game.UniqueRaceID) # Setting Max_Speed for the Vehicle. # TODO What's this? Maybe some hacky way to pass a parameter which is not supposed to be there... Memory.Data.Game.UniqueTrackID = int(MAX_SPEED) # Speed is KM/H print("Setting speed at ", Memory.Data.Game.UniqueTrackID) # Default for AsFault Memory.Data.Game.Lanes = 1 # By default the AI is in charge Memory.Data.Control.IsControlling = 1 deep_drive_engaged = True STATE = "NORMAL" Memory.waitOnRead() if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0: print("\n\n\nState not reset ! ", Memory.Data.Control.Breaking) Memory.Data.Control.Breaking = 0.0 # Pass the computation to DeepDrive # Not sure this will have any effect Memory.indicateWrite() Memory.waitOnRead() if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0: print("\n\n\nState not reset Again! ", Memory.Data.Control.Breaking) Memory.Data.Control.Breaking = 0.0 # Pass the computation to DeepDrive Memory.indicateWrite() # Connect to running beamng beamng = BeamNGpy( 'localhost', 64256, home='C://Users//Alessio//BeamNG.research_unlimited//trunk') bng = beamng.open(launch=False) try: bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(MAX_FPS) # With 60hz temporal resolution # Connect to the existing vehicle (identified by the ID set in the vehicle instance) bng.connect_vehicle(vehicle) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # Road interface is not available in BeamNG.research yet # Get the road map from the level # roads = bng.get_roads() # # find the actual road. Dividers lane markings are all represented as roads # theRoad = None # for road in enumerate(roads): # # ((left, centre, right), (left, centre, right), ...) # # Compute the width of the road # left = Point(road[0][0]) # right = Point(road[0][1]) # distance = left.distance( right ) # if distance < 2.0: # continue # else: # theRoad = road; # break # # if theRoad is None: # print("WARNING Cannot find the main road of the map") while True: # Resume the execution # 6 steps correspond to 10 FPS with a resolution of 60FPS # 5 steps 12 FPS # 3 steps correspond to 20 FPS bng.step(SIMULATION_STEP) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) # print("vehicle.state", vehicle.state) # # TODO: Is there a way to query for the speed directly ? speed = math.sqrt( vehicle.state['vel'][0] * vehicle.state['vel'][0] + vehicle.state['vel'][1] * vehicle.state['vel'][1]) # Speed is M/S ? # print("Speed from BeamNG is: ", speed, speed*3.6) imageData = preprocess(sensors['front_cam']['colour'], brightness) Height, Width = imageData.shape[:2] # print("Image size ", Width, Height) # TODO Size of image should be right since the beginning Memory.write(Width, Height, imageData, speed) # Pass the computation to DeepDrive Memory.indicateWrite() # Wait for the control commands to send to the vehicle # This includes a sleep and will be unlocked by writing data to it Memory.waitOnRead() # TODO Assumption. As long as the car is out of the road for too long this value stays up if Memory.Data.Control.Breaking == 3.0: if STATE != "DISABLED": print( "Abnormal situation detected. Disengage DeepDrive and enable BeamNG AI" ) vehicle.ai_set_mode("manual") vehicle.ai_drive_in_lane(True) vehicle.ai_set_speed(MAX_SPEED) vehicle.ai_set_waypoint("waypoint_goal") deep_drive_engaged = False STATE = "DISABLED" elif Memory.Data.Control.Breaking == 2.0: if STATE != "GRACE": print("Grace period. Deep Driving still disengaged") vehicle.ai_set_mode("manual") vehicle.ai_set_waypoint("waypoint_goal") # vehicle.ai_drive_in_lane(True) STATE = "GRACE" else: if STATE != "NORMAL": print("DeepDrive re-enabled") # Disable BeamNG AI driver vehicle.ai_set_mode("disabled") deep_drive_engaged = True STATE = "NORMAL" # print("State ", STATE, "Memory ",Memory.Data.Control.Breaking ) if STATE == "NORMAL": vehicle.ai_set_mode("disabled") # Get commands from SHM # Apply Control - not sure cutting at 3 digit makes a difference steering = round( translate_steering(Memory.Data.Control.Steering), 3) throttle = round(Memory.Data.Control.Accelerating * acc_gain, 3) brake = round(Memory.Data.Control.Breaking * brake_gain, 3) # Apply commands vehicle.control(throttle=throttle, steering=steering, brake=brake) # # print("Suggested Driving Actions: ") # print(" Steer: ", steering) # print(" Accel: ", throttle) # print(" Brake: ", brake) finally: bng.close()
def 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 Simulation(object): def __init__(self): super().__init__() # Break an episode upon user input. 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()) # At this point, BeamNG creates a prefab file which contains the specified roads. self.scenario.make(self.bng) prefab_path = self.scenario.get_prefab_path() # Update the road definitions in the prefab. 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): """ Break an episode in case of user input. :return: (None) """ while True: a = input() self.done = not self.done def take_action(self, action): """ Execute an action. :param action: ([float]) :return: (None) """ 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): """ Compute the reward based on the observed state. :param done: (bool) :param dist: (float) :return: (float) """ 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): """ Observe the current state. :return: (np.ndarray, float, bool, dict) """ 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] pos = self.vehicle.state['pos'] 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): """ Get the current velocity of the vehicle. :return: (float) """ state = self.vehicle.state velocity = np.linalg.norm(state["vel"]) return velocity * 3.6 def position(self): """ Get the current position coordinates of the vehicle. :return: ([float]) """ return self.vehicle.state["pos"] def refresh_dist(self, pos): """ Update the driven distance since the last observation. :param pos: ([float]) :return: (None) """ 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): """ Close connection to the BeamNG instance. :return: (None) """ self.bng.close() def reset(self): """ Resets the state of the simulation and starts a new episode. :return: (None) """ 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'] # Respawn the vehicle close to its crash point. closest_point = self.road.closest_roadpoint(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
def run_scenario(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config='vehicles/hopper/custom.pc', run_number=0): global base_filename, default_color, default_scenario, default_spawnpoint, steps_per_sec global integral, prev_error, setpoint integral = 0.0 prev_error = 0.0 # setup DNN model + weights sm = DAVE2Model() # steering_model = Model().define_model_BeamNG("BeamNGmodel-racetracksteering8.h5") # throttle_model = Model().define_model_BeamNG("BeamNGmodel-racetrackthrottle8.h5") dual_model = sm.define_dual_model_BeamNG() # dual_model = sm.load_weights("BeamNGmodel-racetrackdualcomparison10K.h5") # dual_model = sm.define_multi_input_model_BeamNG() # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison10K-PIDcontrolset-4.h5") # dual_model = sm.load_weights("BeamNGmodel-racetrackdual-comparison40K-PIDcontrolset-1.h5") # BeamNGmodel-racetrack-multiinput-dualoutput-comparison10K-PIDcontrolset-1.h5 # BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2 dual_model = sm.load_weights( "BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2.h5") # dual_model = sm.load_weights("BeamNGmodel-racetrack-multiinput-dualoutput-comparison103K-PIDcontrolset-1.h5") random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean', user='******') scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='EGO', color=default_color) vehicle = setup_sensors(vehicle) spawn = spawn_point(default_scenario, default_spawnpoint) scenario.add_vehicle( vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) #, partConfig=parts_config) add_barriers(scenario) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) #bng.hide_hud() bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 60hz temporal resolution bng.load_scenario(scenario) bng.start_scenario() # bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt # bng.resume() # perturb vehicle print("vehicle position before deflation via beamstate:{}".format( vehicle.get_object_position())) print("vehicle position before deflation via vehicle state:{}".format( vehicle.state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state)) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) # bng.resume() # vehicle.break_all_breakgroups() # vehicle.break_hinges() wheelspeed = 0.0 throttle = 0.0 prev_error = setpoint damage_prev = None runtime = 0.0 kphs = [] traj = [] pitches = [] rolls = [] steering_inputs = [] throttle_inputs = [] timestamps = [] damage = None final_img = None # Send random inputs to vehice and advance the simulation 20 steps overall_damage = 0.0 total_loops = 0 total_imgs = 0 total_predictions = 0 while overall_damage <= 0: # collect images sensors = bng.poll_sensors(vehicle) image = sensors['front_cam']['colour'].convert('RGB') # plt.imshow(image) # plt.pause(0.01) total_imgs += 1 img = sm.process_image(np.asarray(image)) wheelspeed = sensors['electrics']['wheelspeed'] kph = ms_to_kph(wheelspeed) dual_prediction = dual_model.predict(x=[img, np.array([kph])]) # steering_prediction = steering_model.predict(img) # throttle_prediction = throttle_model.predict(img) dt = sensors['timer']['time'] - runtime runtime = sensors['timer']['time'] # control params brake = 0 # steering = float(steering_prediction[0][0]) #random.uniform(-1.0, 1.0) # throttle = float(throttle_prediction[0][0]) steering = float(dual_prediction[0][0]) #random.uniform(-1.0, 1.0) throttle = float(dual_prediction[0][1]) total_predictions += 1 if abs(steering) > 0.2: setpoint = 20 else: setpoint = 40 # if runtime < 10: throttle = throttle_PID(kph, dt) # if throttle > 1: # throttle = 1 # if setpoint < kph: # brake = 0.0 #throttle / 10000.0 # throttle = 0.0 vehicle.control(throttle=throttle, steering=steering, brake=brake) steering_inputs.append(steering) throttle_inputs.append(throttle) timestamps.append(runtime) steering_state = sensors['electrics']['steering'] steering_input = sensors['electrics']['steering_input'] avg_wheel_av = sensors['electrics']['avg_wheel_av'] damage = sensors['damage'] overall_damage = damage["damage"] new_damage = diff_damage(damage, damage_prev) damage_prev = damage vehicle.update_vehicle() traj.append(vehicle.state['pos']) pitches.append(vehicle.state['pitch'][0]) rolls.append(vehicle.state['roll'][0]) kphs.append(ms_to_kph(wheelspeed)) total_loops += 1 if new_damage > 0.0: final_img = image break bng.step(1, wait=True) if runtime > 300: print("Exited after 5 minutes successful runtime") break if distance( spawn['pos'], vehicle.state['pos']) < 5 and sensors['timer']['time'] > 10: reached_start = True break # print("runtime:{}".format(round(runtime, 2))) # print("time to crash:{}".format(round(runtime, 2))) bng.close() # avg_kph = float(sum(kphs)) / len(kphs) plt.imshow(final_img) plt.savefig("Run-{}-finalimg.png".format(run_number)) plt.pause(0.01) plot_input(timestamps, steering_inputs, "Steering", run_number=run_number) plot_input(timestamps, throttle_inputs, "Throttle", run_number=run_number) plot_input(timestamps, kphs, "KPHs", run_number=run_number) print("Number of steering_inputs:", len(steering_inputs)) print("Number of throttle inputs:", len(throttle_inputs)) results = "Total loops: {} \ntotal images: {} \ntotal predictions: {} \nexpected predictions ={}*{}={}".format( total_loops, total_imgs, total_predictions, round(runtime, 3), steps_per_sec, round(runtime * steps_per_sec, 3)) print(results) results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': final_img, 'total_predictions': total_predictions, 'expected_predictions': round(runtime * steps_per_sec, 3) } return results
map_beamng_serialize = path + filename + '.nodes.serialize.beamng' map_degree_serialize = path + filename + '.ways.degree.serialize' node_dict = pickle.load(open(map_nodes_serialize, "rb")) print("Nodes Loaded") graph = pickle.load(open(map_ways_serialize, "rb")) print("Graph Loaded") beamng_dict = pickle.load(open(map_beamng_serialize, "rb")) print("BeamNG Nodes Loaded") graph_degree = pickle.load(open(map_degree_serialize, "rb")) print("Graph Degree") beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN') scenario = Scenario('GridMap', 'road_test') def getRoadPolyLineLaneMarking(point1,point2, width): print("Road Lanes") #https://stackoverflow.com/questions/47040213/find-perpendicular-line-using-points-on-that-line print(point1,point2, width) dx = float(point2[0] - point1[0]) dy = float(point2[1] - point1[1]) L = float(math.sqrt(float(float(dx * dx) + float(dy * dy)))) U = (float(-dy / L), float(dx / L)) F = float(float(width) - 0.05)
import matplotlib import numpy as np import matplotlib.pyplot as plt import pandas as pd import seaborn as sns from beamngpy import BeamNGpy, Vehicle, Scenario from beamngpy.sensors import Camera, GForces, Lidar, Electrics, Damage sns.set() # Instantiate a BeamNGpy instance the other classes use for reference & communication # This is the host & port used to communicate over beamng = BeamNGpy('localhost', 64294) #loads Custom Map scenario = Scenario('Mytest', 'demo') #adding ego Vehicle to the scenario vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Green') electrics = Electrics() vehicle.attach_sensor('electrics', electrics) damage1 = Damage() vehicle.attach_sensor('damage1', damage1) scenario.add_vehicle(vehicle, pos=(104.647, -1.92827, -3.54338), rot=(0, 0, 90)) #add vehicle as Obstacle
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 main(): random.seed(1703) setup_logging() # Plotting code setting up a 3x2 figure fig = plt.figure(1, figsize=(10, 5)) axarr = fig.subplots(2, 3) a_colour = axarr[0, 0] b_colour = axarr[1, 0] a_depth = axarr[0, 1] b_depth = axarr[1, 1] a_annot = axarr[0, 2] b_annot = axarr[1, 2] plt.ion() beamng = BeamNGpy('localhost', 64256) # Create a scenario in west_coast_usa scenario = Scenario('west_coast_usa', 'research_test', description='Random driving for research') # Set up first vehicle, with two cameras, gforces sensor, lidar, electrical # sensors, and damage sensors vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Red') # Set up sensors pos = (-0.3, 1, 1.0) direction = (0, 1, 0) fov = 120 resolution = (512, 512) front_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) pos = (0.0, 3, 1.0) direction = (0, -1, 0) fov = 90 resolution = (512, 512) back_camera = Camera(pos, direction, fov, resolution, colour=True, depth=True, annotation=True) gforces = GForces() electrics = Electrics() damage = Damage() lidar = Lidar() # Attach them vehicle.attach_sensor('front_cam', front_camera) vehicle.attach_sensor('back_cam', back_camera) vehicle.attach_sensor('gforces', gforces) vehicle.attach_sensor('lidar', lidar) vehicle.attach_sensor('electrics', electrics) vehicle.attach_sensor('damage', damage) scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=(0, 0, 45)) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) try: bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(60) # With 60hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # Put simulator in pause awaiting further inputs bng.pause() bng.hide_hud() assert vehicle.skt # Send random inputs to vehice and advance the simulation 20 steps for _ in range(1024): throttle = random.uniform(0.0, 1.0) steering = random.uniform(-1.0, 1.0) brake = random.choice([0, 0, 0, 1]) vehicle.control(throttle=throttle, steering=steering, brake=brake) bng.step(20) # Retrieve sensor data and show the camera data. sensors = bng.poll_sensors(vehicle) a_colour.imshow(sensors['front_cam']['colour'].convert('RGB')) a_depth.imshow(sensors['front_cam']['depth'].convert('L')) a_annot.imshow(sensors['front_cam']['annotation'].convert('RGB')) b_colour.imshow(sensors['back_cam']['colour'].convert('RGB')) b_depth.imshow(sensors['back_cam']['depth'].convert('L')) b_annot.imshow(sensors['back_cam']['annotation'].convert('RGB')) plt.pause(0.00001) finally: bng.close()
from beamngpy import BeamNGpy, Scenario, Vehicle, StaticObject from beamngpy.sensors import Electrics, Damage import numpy as np from time import sleep, time beamng = BeamNGpy('localhost', 64256, home=r'C:\BeamNG_unlimited\trunk') scenario = Scenario('west_coast_usa', 't_intersection_wo_obj') vut = Vehicle('vut', model='coupe', licence='VUT', colour='Red') electrics = Electrics() damage = Damage() vut.attach_sensor('electrics', electrics) vut.attach_sensor('damage', damage) scenario.add_vehicle(vut, pos=(-198.5, -164.189, 119.7), rot=(0, 0, -126.25)) car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue') scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55)) car_2 = Vehicle('car_2', model='etk800', licence='CAR 2', colour='Blue') scenario.add_vehicle(car_2, pos=(-140, -121.233, 119.586), rot=(0, 0, 55)) scenario.make(beamng) bng = beamng.open(launch=True) bng.load_scenario(scenario) bng.start_scenario() vut.ai_set_mode('span') vut.ai_drive_in_lane(True) car_1.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}]) car_2.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}])
def main(): beamng = BeamNGpy('localhost', 64256) bng = beamng.open(launch=True) scenario = Scenario('smallgrid', 'mesh_test') cylinder = ProceduralCylinder(name='cylinder', radius=3.5, height=5, pos=(10, -10, 0), rot=None, rot_quat=(0, 0, 0, 1)) scenario.add_procedural_mesh(cylinder) bump = ProceduralBump(name='bump', pos=(-10, -10, 0), rot=None, rot_quat=(0, 0, 0, 1), width=5, length=7, height=2, upper_length=2, upper_width=2 ) scenario.add_procedural_mesh(bump) cone = ProceduralCone(name='cone', pos=(-10, -20, 0), rot=None, rot_quat=(0, 0, 0, 1), radius=3.5, height=5) scenario.add_procedural_mesh(cone) cube = ProceduralCube(name='cube', pos=(0, -20, 0), rot=None, rot_quat=(0, 0, 0, 1), size=(5, 2, 3)) scenario.add_procedural_mesh(cube) ring = ProceduralRing(name='ring', pos=(10, -20, 0), rot=None, rot_quat=(0, 0.7071068, 0, 0.7071068), radius=2, thickness=1 ) scenario.add_procedural_mesh(ring) vehicle = Vehicle('ego_vehicle', model='etk800') scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=None, rot_quat=(0, 0, 0, 1)) scenario.make(bng) try: bng.load_scenario(scenario) bng.start_scenario() input('Press enter when done...') finally: bng.close()
def run_scenario_ai_version(vehicle_model='etk800', deflation_pattern=[0, 0, 0, 0], parts_config=None): global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec global prev_error random.seed(1703) setup_logging() beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean', user="******") scenario = Scenario(default_scenario, 'research_test') vehicle = Vehicle('ego_vehicle', model=vehicle_model, licence='AI', color=default_color) vehicle = setup_sensors(vehicle) spawn = get_spawn_point(default_scenario, default_spawnpoint) # scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat']) # Compile the scenario and place it in BeamNG's map folder scenario.make(beamng) # Start BeamNG and enter the main loop bng = beamng.open(launch=True) bng.set_deterministic() # Set simulator to be deterministic bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution # Load and start the scenario bng.load_scenario(scenario) bng.start_scenario() # create vehicle to be chased chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red') bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None, rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666)) bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config) # Put simulator in pause awaiting further inputs bng.pause() assert vehicle.skt #bng.resume() # perturb vehicle print("vehicle position before deflation via beamstate:{}".format( vehicle.get_object_position())) print("vehicle position before deflation via vehicle state:{}".format( vehicle.state)) image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') plt.imshow(image) plt.pause(0.01) vehicle.deflate_tires(deflation_pattern) bng.step(steps_per_sec * 6) vehicle.update_vehicle() # print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position())) # print("vehicle position after deflation via vehicle state:{}".format(vehicle.state)) pitch = vehicle.state['pitch'][0] roll = vehicle.state['roll'][0] z = vehicle.state['pos'][2] image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') plt.imshow(image) plt.pause(0.01) bng.resume() vehicle.ai_set_mode('chase') vehicle.ai_set_target('chase_vehicle') vehicle.ai_drive_in_lane(True) damage_prev = None runtime = 0.0 traj = [] kphs = [] for _ in range(650): image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB') damage = bng.poll_sensors(vehicle)['damage'] wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed'] new_damage = diff_damage(damage, damage_prev) damage_prev = damage runtime = bng.poll_sensors(vehicle)['timer']['time'] vehicle.update_vehicle() traj.append(vehicle.state['pos']) kphs.append(ms_to_kph(wheelspeed)) if new_damage > 0.0: break bng.step(5) bng.close() results = { 'runtime': round(runtime, 3), 'damage': damage, 'kphs': kphs, 'traj': traj, 'pitch': round(pitch, 3), 'roll': round(roll, 3), "z": round(z, 3), 'final_img': image } return results
def beamng(): beamng = BeamNGpy('localhost', 64256) return beamng
def set_time_of_day_to(self, instance: BeamNGpy): if self.time_of_day: instance.set_tod(self.time_of_day)