def main(): beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN') scenario = Scenario('GridMap', 'road_test') road_a = Road('custom_track_center', looped=False) nodes = [ (0, 0, 0, 4), (0, 400, 0, 4), ] road_a.nodes.extend(nodes) scenario.add_road(road_a) vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black') scenario.add_vehicle(vehicle, pos = ( 0, 0, 0.163609) , rot=(0,0,180)) # 0.163609 , scenario.make(beamng) script = list() node0 = { 'x': 0, 'y': 2, 'z': 0.163609, 't': 0.01, } node1 = { 'x': 0, 'y': 3, 'z': 0.163609, 't': 0.01, } node2 = { 'x': 0, 'y': 350, 'z': 0.163609, 't': 15, } node3 = { 'x': 0, 'y': 400, 'z': 0.163609, 't': 5, } script.append(node0) script.append(node1) script.append(node2) script.append(node3) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_script(script) input('Press enter when done...') finally: bng.close()
def main(): setup_logging() beamng = BeamNGpy('localhost', 64256) scenario = Scenario('west_coast_usa', 'ai_sine') vehicle = Vehicle('ego_vehicle', model='etk800', licence='AI') orig = (-769.1, 400.8, 142.8) scenario.add_vehicle(vehicle, pos=orig, rot=None, rot_quat=(0, 0, 1, 0)) scenario.make(beamng) script = list() for i in range(3600): node = { # Calculate the position as a sinus curve that makes the vehicle # drive from left to right. The z-coordinate is not calculated in # any way because `ai_set_script` by default makes the polyline to # follow cling to the ground, meaning the z-coordinate will be # filled in automatically. 'x': 4 * np.sin(np.radians(i)) + orig[0], 'y': i * 0.2 + orig[1], 'z': orig[2], # Calculate timestamps for each node such that the speed between # points has a sinusoidal variance to it. 't': (2 * i + (np.abs(np.sin(np.radians(i)))) * 64) / 64, } script.append(node) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_script(script) while True: bng.step(60) finally: bng.close()
def main(): path='C:/Users/marc/Desktop/BeamNG' beamng = BeamNGpy('localhost', 64256,path) scenario = Scenario('GridMap', 'vehicle_bbox_example') road = Road('track_editor_C_center', rid='main_road', texture_length=5) orig = (-107, 70, 0) goal = (-300, 70, 0) road.nodes = [ (*orig, 7), (*goal, 7), ] scenario.add_road(road) script = [{'x': orig[0], 'y': orig[1], 'z': .3, 't': 0}] h=0 i = 0.2 while script[h]['x'] > goal[0]: node = { 'x': -10 * i + orig[0], 'y': 8 * np.sin(i) + orig[1], 'z': 0.3, 't': 1.5 * i, } script.append(node) i += 0.2 h+=1 print(script) vehicle = Vehicle('ego_vehicle', model='etkc', licence='PYTHON') scenario.add_vehicle(vehicle, pos=orig) scenario.make(beamng) bng = beamng.open() bng.load_scenario(scenario) bng.start_scenario() vehicle.ai_set_script(script) bng.pause() bng.step(1)
from beamngpy import BeamNGpy, Vehicle, Scenario, Road from beamngpy.sensors import Camera from getAIScript import getAIScript beamng = BeamNGpy('localhost', 64256, getBeamngDirectory()) scenario = Scenario('smallgrid', 'vehicle_bbox_example') road = Road('track_editor_A_center', rid='main_road') orig = (0, -2, 0) goal = (1150, -22, 0) nodes = [(*orig, 7), (*goal, 7)] road.nodes.extend(nodes) scenario.add_road(road) vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON') overhead = Camera((0, -10, 5), (0, 1, -0.75), 60, (1024, 1024)) vehicle.attach_sensor('overhead', overhead) scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, -90)) scenario.make(beamng) bng = beamng.open(launch=True) try: bng.load_scenario(scenario) bng.start_scenario() script = getAIScript() vehicle.ai_set_script(script) while (True): vehicle.update_vehicle() print(vehicle.state['pos']) input() finally: bng.close()
import mmap
def test_vehicle_ai(beamng): with beamng as bng: bng.set_deterministic() scenario = Scenario('west_coast_usa', 'ai_test') vehicle = Vehicle('test_car', model='etk800') other = Vehicle('other', model='etk800') pos = [-717.121, 101, 118.675] scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45)) scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45)) scenario.make(bng) bng.load_scenario(scenario) bng.start_scenario() bng.pause() vehicle.ai_set_mode('span') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_waypoint('Bridge4_B') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='chase') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() vehicle.ai_set_target('other', mode='flee') assert_continued_movement(bng, vehicle, pos) bng.restart_scenario() bng.pause() script = [ { 'x': -735, 'y': 86.7, 'z': 119, 't': 0 }, { 'x': -752, 'y': 70, 'z': 119, 't': 5 }, { 'x': -762, 'y': 60, 'z': 119, 't': 8 }, ] vehicle.ai_set_script(script) bng.step(600, wait=True) vehicle.update_vehicle() ref = [script[1]['x'], script[1]['y'], script[1]['z']] pos = vehicle.state['pos'] ref, pos = np.array(ref), np.array(pos) assert np.linalg.norm(ref - pos) < 2.5 scenario.delete(beamng)
scenario.make(beamng) bng = beamng.open(launch=True) bng.set_deterministic() try: bng.load_scenario(scenario) bng.start_scenario() bng.add_debug_line(striker_path['points'], striker_path['point_colors'], spheres=striker_path['spheres'], sphere_colors=striker_path['sphere_colors'], cling=True, offset=0.1) bng.add_debug_line(victim_path['points'], victim_path['point_colors'], spheres=victim_path['spheres'], sphere_colors=victim_path['sphere_colors'], cling=True, offset=0.1) vehicleStriker.ai_set_script(striker_path['script']) vehicleVictim.ai_set_script(victim_path['script']) for number in range(60): time.sleep(0.20) vehicleStriker.update_vehicle() # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicleStriker) # Polls the data of all sensors attached to the vehicle # print(vehicleStriker.state['pos']) if vehicleStriker.state['pos'][0] > 236 and vehicleStriker.state['pos'][1] > 141: # print('free state') vehicleStriker.control(throttle=0, steering=0, brake=0, parkingbrake=0) vehicleStriker.update_vehicle() vehicleVictim.update_vehicle() # Synchs the vehicle's "state" variable with the simulator sensors = bng.poll_sensors(vehicleVictim) # Polls the data of all sensors attached to the vehicle
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
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()