def main():
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('west_coast_usa', 'lidar_demo',
description='Spanning the map with a lidar sensor')
vehicle = Vehicle('ego_vehicle', model='etk800',
licence='RED', color='Red')
lidar = Lidar(offset=(0, 0, 1.6))
vehicle.attach_sensor('lidar', lidar)
scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=(0, 0, 45))
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
bng.load_scenario(scenario)
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('span')
print('Driving around for 60 seconds...')
sleep(60)
finally:
bng.close()
def setup_beamng(vehicle_model='etk800',
camera_pos=(-0.5, 0.38, 1.3),
camera_direction=(0, 1.0, 0),
pitch_euler=0.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
# version==keras
# sm = DAVE2Model()
# model = sm.define_dual_model_BeamNG()
# model = sm.load_weights("BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-2.h5")
# model = sm.load_weights("BeamNGmodel-racetrackdual-comparison100K-PIDcontrolset-3-sanitycheckretraining-model.h5")
# version==pytorch
# model = torch.load("C:/Users/merie/Downloads/dave/test_model_20.pt", map_location=torch.device('cpu')).eval()
# model = torch.load("F:/MODELS-FOR-MERIEL/models/model-100000D-1000B-100E-1p000000e-04LR-100.pt.pt", map_location=torch.device('cpu')).eval()
# model = torch.load("F:/MODELS-FOR-MERIEL/models/model-292757D-1000B-100E-1p000000e-04LR-100.pt", map_location=torch.device('cpu')).eval()
model = torch.load(
"H:/GitHub/DAVE2-Keras/test7-trad-50epochs-64batch-1e4lr-ORIGDATASET-singleoutput-model.pt",
map_location=torch.device('cpu')).eval()
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,
pos=camera_pos,
direction=camera_direction)
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)
add_qr_cubes(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()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# bng.resume()
# find_width_of_road(bng)
return vehicle, bng, model
def main():
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
#beamng.change_setting('research', True)
scenario = Scenario('west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR')
lidar = Lidar()
vehicle.attach_sensor('lidar', lidar)
#beamng.open_lidar('lidar', vehicle, 'shmem', 8000)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=None,
rot_quat=(0, 0, 0.3826834, 0.9238795))
scenario.make(beamng)
bng = beamng.open(launch=True)
#bng.open_lidar('lidar', vehicle, 'shmem', 8000)
#lidar.connect(bng, vehicle)
try:
bng.load_scenario(scenario) # this is where the error happens
window = open_window(SIZE, SIZE)
lidar_vis = LidarVisualiser(Lidar.max_points)
lidar_vis.open(SIZE, SIZE)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
bng.pause()
vehicle.ai_set_mode('span')
def update():
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
bng.step(3, wait=False)
lidar_vis.update_points(points, vehicle.state)
glutPostRedisplay()
glutReshapeFunc(lidar_resize)
glutIdleFunc(update)
glutMainLoop()
except Exception as e:
print(e)
finally:
bng.close()
def beamng():
setup_logging()
# Get environment variables
BNG_HOME = os.getenv('BNG_HOME')
BNG_RESEARCH = os.getenv('BNG_RESEARCH')
host = '127.0.0.1'
port = 64256
# Instantiates a BeamNGpy instance
beamng = BeamNGpy(host, port, BNG_HOME, BNG_RESEARCH)
return beamng
def setup_beamng(traffic=2):
global sp, beamng_home, beamng_user
setup_logging()
beamng = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user)
scenario = Scenario(
'west_coast_usa',
'lidar_tour',
description=
'Tour through highway with variable traffic vehicles gathering '
'Lidar data')
# setup vehicle
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='LIDAR',
color='Red')
vehicle = setup_sensors(vehicle)
# setup vehicle poses
lane = random.choice([1, 2, 3, 4])
ego_sp = spawn_point(sp, lane)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
lane = ego_sp['lane']
# add vehicles to scenario
# print("adding vehicle to scenario...")
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles(traffic)
ps = generate_vehicle_positions(ego_pos, ego_rot_quat, lane, tvs)
for i in range(len(tvs)):
print("adding vehicle {}...".format(i))
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
print("making scenario...")
scenario.make(beamng)
print("opening beamNG...")
bng = beamng.open(launch=True)
st = time.time()
print("loading scenario...")
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
#print("Bounding box: {}".format(vehicle.get_bbox()))
bng.hide_hud()
print("starting scenario...")
bng.start_scenario()
vehicle.ai_set_mode('traffic')
#"DecalRoad31765_8"
vehicle.ai_drive_in_lane(False)
vehicle.ai_set_aggression(2.0)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
bng.pause()
return vehicle, bng
def main():
setup_logging()
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR')
lidar = Lidar()
vehicle.attach_sensor('lidar', lidar)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=None,
rot_quat=(0, 0, 0.3826834, 0.9238795))
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
window = open_window(SIZE, SIZE)
lidar_vis = LidarVisualiser(Lidar.max_points)
lidar_vis.open(SIZE, SIZE)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
bng.pause()
vehicle.ai_set_mode('span')
def update():
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
bng.step(3, wait=False)
lidar_vis.update_points(points, vehicle.state)
glutPostRedisplay()
glutReshapeFunc(lidar_resize)
glutIdleFunc(update)
glutMainLoop()
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 run_scenario_spawns_backwards(vehicle_model='etk800'):
global base_filename, default_color, spawn, steps_per_sec, home
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost', 64256, home=home)
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('vehicle',
model=vehicle_model,
licence='CORRECT',
color='Red')
vehicle = setup_sensors(vehicle)
# 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'])
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
return_str = "SPAWNED VEHICLE STATE:\n{}".format(vehicle.state)
print(return_str)
bng.close()
return return_str
# Example scenario for usage with example.py (run this first)
from beamngpy import BeamNGpy, Scenario, Vehicle, Road
from beamngpy import setup_logging
import time
import sys
import fileinput
setup_logging()
bng = BeamNGpy('localhost', 64256)
scenario = Scenario('smallgrid', 'small_test')
vehicle = Vehicle('egovehicle', model='etk800', licence='313')
scenario.add_vehicle(vehicle, pos=(-1.5, -1, 0), rot=(0, 0, 0))
nodes = [(0, 0, 0, 6), (0, -20, 0, 6), (0, -100, 0, 6), (-25, -200, 0, 6),
(25, -300, 0, 6)]
road = Road(material='a_asphalt_01_a',
rid='main_road',
looped=False,
texture_length=10)
road.nodes.extend(nodes)
scenario.add_road(road)
scenario.make(bng)
for i, line in enumerate(fileinput.input(scenario.get_prefab_path(),
def main(MAX_SPEED):
setup_logging()
# Gains to port TORCS actuators to BeamNG
# steering_gain = translate_steering()
acc_gain = 0.5 # 0.4
brake_gain = 1.0
# BeamNG images are too bright for DeepDrive
brightness = 0.4
# Set up first vehicle
# ! A vehicle with the name you specify here has to exist in the scenario
vehicle = Vehicle('egovehicle')
# Set up sensors
resolution = (280, 210)
# Original Settings
#pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below
# 0.4 is inside
pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below
direction = (0, 1, 0)
# direction = (180, 0, 180)
# FOV 60, MAX_SPEED 100, 20 (3) Hz fails
# FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
# FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
# FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
# FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
# FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
fov = 60
# MAX_SPEED = 70
MAX_FPS = 60
SIMULATION_STEP = 6
# Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
#
# WORKING SETTINGS: 20 Freq, 90 FOV.
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
# Setup the SHM with DeepDrive
# Create shared memory object
Memory = sd.CSharedMemory(TargetResolution=[280, 210])
# Enable Pause-Mode
Memory.setSyncMode(True)
Memory.Data.Game.UniqueRaceID = int(time.time())
print("Setting Race ID at ", Memory.Data.Game.UniqueRaceID)
# Setting Max_Speed for the Vehicle.
# TODO What's this? Maybe some hacky way to pass a parameter which is not supposed to be there...
Memory.Data.Game.UniqueTrackID = int(MAX_SPEED)
# Speed is KM/H
print("Setting speed at ", Memory.Data.Game.UniqueTrackID)
# Default for AsFault
Memory.Data.Game.Lanes = 1
# By default the AI is in charge
Memory.Data.Control.IsControlling = 1
deep_drive_engaged = True
STATE = "NORMAL"
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset ! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
# Not sure this will have any effect
Memory.indicateWrite()
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset Again! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Connect to running beamng
beamng = BeamNGpy(
'localhost',
64256,
home='C://Users//Alessio//BeamNG.research_unlimited//trunk')
bng = beamng.open(launch=False)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(MAX_FPS) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# Road interface is not available in BeamNG.research yet
# Get the road map from the level
# roads = bng.get_roads()
# # find the actual road. Dividers lane markings are all represented as roads
# theRoad = None
# for road in enumerate(roads):
# # ((left, centre, right), (left, centre, right), ...)
# # Compute the width of the road
# left = Point(road[0][0])
# right = Point(road[0][1])
# distance = left.distance( right )
# if distance < 2.0:
# continue
# else:
# theRoad = road;
# break
#
# if theRoad is None:
# print("WARNING Cannot find the main road of the map")
while True:
# Resume the execution
# 6 steps correspond to 10 FPS with a resolution of 60FPS
# 5 steps 12 FPS
# 3 steps correspond to 20 FPS
bng.step(SIMULATION_STEP)
# Retrieve sensor data and show the camera data.
sensors = bng.poll_sensors(vehicle)
# print("vehicle.state", vehicle.state)
# # TODO: Is there a way to query for the speed directly ?
speed = math.sqrt(
vehicle.state['vel'][0] * vehicle.state['vel'][0] +
vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Speed is M/S ?
# print("Speed from BeamNG is: ", speed, speed*3.6)
imageData = preprocess(sensors['front_cam']['colour'], brightness)
Height, Width = imageData.shape[:2]
# print("Image size ", Width, Height)
# TODO Size of image should be right since the beginning
Memory.write(Width, Height, imageData, speed)
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Wait for the control commands to send to the vehicle
# This includes a sleep and will be unlocked by writing data to it
Memory.waitOnRead()
# TODO Assumption. As long as the car is out of the road for too long this value stays up
if Memory.Data.Control.Breaking == 3.0:
if STATE != "DISABLED":
print(
"Abnormal situation detected. Disengage DeepDrive and enable BeamNG AI"
)
vehicle.ai_set_mode("manual")
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_speed(MAX_SPEED)
vehicle.ai_set_waypoint("waypoint_goal")
deep_drive_engaged = False
STATE = "DISABLED"
elif Memory.Data.Control.Breaking == 2.0:
if STATE != "GRACE":
print("Grace period. Deep Driving still disengaged")
vehicle.ai_set_mode("manual")
vehicle.ai_set_waypoint("waypoint_goal")
# vehicle.ai_drive_in_lane(True)
STATE = "GRACE"
else:
if STATE != "NORMAL":
print("DeepDrive re-enabled")
# Disable BeamNG AI driver
vehicle.ai_set_mode("disabled")
deep_drive_engaged = True
STATE = "NORMAL"
# print("State ", STATE, "Memory ",Memory.Data.Control.Breaking )
if STATE == "NORMAL":
vehicle.ai_set_mode("disabled")
# Get commands from SHM
# Apply Control - not sure cutting at 3 digit makes a difference
steering = round(
translate_steering(Memory.Data.Control.Steering), 3)
throttle = round(Memory.Data.Control.Accelerating * acc_gain,
3)
brake = round(Memory.Data.Control.Breaking * brake_gain, 3)
# Apply commands
vehicle.control(throttle=throttle,
steering=steering,
brake=brake)
#
# print("Suggested Driving Actions: ")
# print(" Steer: ", steering)
# print(" Accel: ", throttle)
# print(" Brake: ", brake)
finally:
bng.close()
def test_quats(beamng):
with beamng as bng:
setup_logging()
scenario = Scenario('smallgrid', 'test_quat')
blue_etk = Vehicle('ego_vehicle',
model='etk800',
color='Blue',
licence="angle")
scenario.add_vehicle(blue_etk, pos=(0, 0, 0), rot=(0, 0, 0))
blue_etk = Vehicle('ego_vehicle2',
model='etk800',
color='Green',
license="quat")
rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)
scenario.add_vehicle(blue_etk, pos=(5, 0, 0), rot_quat=rot_quat)
rb = ScenarioObject(oid='roadblock',
name='sawhorse',
otype='BeamNGVehicle',
pos=(-10, -5, 0),
rot=(0, 0, 0),
scale=(1, 1, 1),
JBeam='sawhorse',
datablock="default_vehicle")
scenario.add_object(rb)
cn = ScenarioObject(oid='cones',
name='cones',
otype='BeamNGVehicle',
pos=(0, -5, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
scale=(1, 1, 1),
JBeam='cones',
datablock="default_vehicle")
scenario.add_object(cn)
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
white_etk = Vehicle('ego_vehicle3', model='etk800', color='White')
bng.spawn_vehicle(white_etk, (-10, 0, 0), (0, 0, 0))
pickup = Vehicle('ego_vehicle4', model='pickup')
pos = (-15, 0, 0)
bng.spawn_vehicle(pickup, pos, None, rot_quat=(0, 0, 0, 1))
resp = bng.get_current_vehicles()
assert len(resp) == 6
pickup.connect(bng)
pickup.poll_sensors()
pos_before = pickup.state['pos']
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
def run_scenario(traffic=2, run_number=0):
global sp
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario('west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='LIDAR',
color='Red')
vehicle = setup_sensors(vehicle)
lidar = setup_lidar('hood')
vehicle.attach_sensor('lidar', lidar)
ego_sp = spawn_point(sp)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles(traffic)
ps = generate_vehicle_positions(ego_pos, ego_rot_quat, tvs)
for i in range(len(tvs)):
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
#bng.set_deterministic()
#print("Bounding box: {}".format(vehicle.get_bbox()))
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
vehicle.ai_set_aggression(0.9)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
start = time.time()
end = time.time()
damage_prev = None
with open(
'H:/traffic_traces/traffic_lidar_data_{}traffic_run{}.csv'.
format(traffic, run_number), 'w') as f:
f.write(
"TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH,EXTERNAL_VEHICLES \n"
)
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
damage = sensors['damage']
v_state = vehicle.state
#print("vehicle_state = {}".format(v_state))
ai_state = vehicle.ai_get_state()
#print("ai_state = {}".format(ai_state))
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
if new_damage > 0:
print("new damage = {}".format(new_damage))
f.write("{},{},{},{},{},{},{}\n".format(
_ * 0.5, v_state['pos'], v_state['dir'], v_state['vel'],
points.tolist(), str(new_damage), traffic))
#print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
#print("Time passed since scenario begun: {}\n".format(end - start))
print()
if end - start >= 45:
bng.close()
bng.step(30, wait=False)
except Exception as e:
print(e)
finally:
bng.close()
def testrun(speed=11, risk=0.6):
global base_filename, training_dir, default_model, setpoint
global spawnpoint
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario, spawnpoint)
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean',
user='******')
scenario = Scenario(default_scenario, 'research_test')
# add barriers and cars to get the ego vehicle to avoid the barriers
add_barriers(scenario)
# add_barrier_cars(scenario)
vehicle = Vehicle('ego_vehicle',
model=default_model,
licence='RED',
color='White')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle,
pos=spawn_pt['pos'],
rot=None,
rot_quat=spawn_pt['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.hide_hud()
bng.set_nondeterministic() # Set simulator to be deterministic
bng.set_steps_per_second(36) #
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
bng.switch_vehicle(vehicle)
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(False)
vehicle.ai_set_speed(speed, mode='set')
vehicle.ai_set_aggression(risk)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# bng.resume()
start_time = time.time()
# Send random inputs to vehicle and advance the simulation 20 steps
imagecount = 0
with open(f, 'w') as datafile:
datafile.write(
'filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature,wheelspeed\n'
)
reached_start = False
vels = []
vel_dict = {}
while imagecount < 10000:
# collect images
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
full_filename = "{}{}{}.jpg".format(training_dir, base_filename,
imagecount)
qualified_filename = "{}{}.jpg".format(base_filename, imagecount)
# collect ancillary data
datafile.write(
'{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(
qualified_filename, str(
round(sensors['timer']['time'],
2)), sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],
sensors['electrics']['wheelspeed']))
if sensors['timer']['time'] > 10:
kph = sensors['electrics']['wheelspeed'] * 3.6
vels.append(kph)
vel_dict[sensors['timer']['time']] = kph
if distance(spawn_pt['pos'], vehicle.state['pos']
) < 5 and sensors['timer']['time'] > 10:
reached_start = True
break
if sensors['damage']['damage'] > 0:
print("CRASHED; QUITTING")
break
# save the image
image.save(full_filename)
imagecount += 1
# plt.title("90 degrees FOV")
# plt.imshow(image)
# plt.pause(0.01)
# plt.title("120 degrees FOV")
# plt.imshow(sensors['back_cam']['colour'].convert('RGB'))
# plt.pause(0.01)
# step sim forward
bng.step(1, wait=True)
wheelspeed_avg = round((sum(vels) / float(len(vels))), 3)
wheelspeed_var = round(np.var(vels), 3)
return_str = ''
if sensors['damage']['damage'] > 0:
# print("QUIT DUE TO CRASH VALUE {}".format(sensors['damage']['damage']))
return_str = "QUIT DUE TO CRASH VALUE {}".format(
sensors['damage']['damage'])
print(return_str)
maxtime = from_val_get_key(vel_dict, max(vels))
mintime = from_val_get_key(vel_dict, min(vels))
info = "IMAGE COUNT:{}\nSIM TIME:{} WALLCLOCK TIME:{}\nWHEELSPEED AVG:{} VAR:{} \nMAX:{} @ timestep {} MIN:{} @ timestep {} ".format(
imagecount, str(round(sensors['timer']['time'], 2)),
time.time() - start_time, wheelspeed_avg, wheelspeed_var,
round(max(vels), 3), round(maxtime, 3), round(min(vels), 3),
round(mintime, 3))
print("SPEED:{} RISK:{}".format(speed, risk))
print(info)
return_str = "{}\n{}".format(return_str, info)
bng.close()
return return_str
def run_spawn_for_deflation(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0]):
global base_filename, default_color, default_scenario, setpoint
global prev_error
global fcam
vehicle_loadfile = 'vehicles/hopper/crawler.pc'
# setup DNN model + weights
m = Model()
model = m.define_model_BeamNG("BeamNGmodel-5.h5")
random.seed(1703)
setup_logging()
# beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='LOWPRESS',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario, 'highway')
# 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.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# load part config
#pc = vehicle.get_part_config()
# loadfile = 'C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json'
# vehicle.load(loadfile)
bng.spawn_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'],
partConfig='vehicles/hopper/custom.pc')
#bng.set_relative_camera()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# vehicle.control(throttle=0.0, steering=0.0, brake=1.0)
# perturb vehicle
# before_state = copy.deepcopy(vehicle.state)
before_state = bng.poll_sensors(vehicle)['front_cam']
before_state['vel'] = vehicle.state['vel']
before_state['roll'] = vehicle.state['roll']
before_state['pitch'] = vehicle.state['pitch']
before_state['yaw'] = vehicle.state['yaw']
# print("vehicle position before deflation via beamstate:{}".format(vehicle.get_object_position()))
# print("vehicle position before deflation via vehicle state:{}".format(vehicle.state))
print(
"vehicle position before deflation via camera:{}".format(before_state))
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
# print("camera sensor before deflation: {}".format(bng.poll_sensors(vehicle)['front_cam']))
#get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up'])
img = m.process_image(image)
before_prediction = model.predict(img)
before_state["prediction"] = before_prediction
plt.imshow(image)
plt.pause(0.01)
if deflation_pattern != [0, 0, 0, 0]:
print("deflating according to pattern {}".format(deflation_pattern))
vehicle.deflate_tires(deflation_pattern)
time.sleep(1)
bng.resume()
bng.set_steps_per_second(100)
bng.set_deterministic()
totalsecs = 0.0
deflation_traj = []
while totalsecs <= 30:
vehicle.update_vehicle()
# vehicle.control(throttle=0.0, steering=0.0, brake=1.0)
after_state = bng.poll_sensors(vehicle)['front_cam']
after_state['vel'] = vehicle.state['vel']
after_state['roll'] = vehicle.state['roll']
after_state['pitch'] = vehicle.state['pitch']
after_state['yaw'] = vehicle.state['yaw']
print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'],
vehicle.state['pitch'],
vehicle.state['yaw']))
deflation_traj.append(round(math.degrees(vehicle.state['pitch'][0]),
4))
bng.step(100)
totalsecs += 1
# after_state = copy.deepcopy(vehicle.state)
# print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
# print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
print("vehicle position after deflation via camera:{}".format(after_state))
#print("camera sensor output: {}".format(bng.poll_sensors(vehicle)['front_cam']['rot']))
#print("camera pos output: {}".format(bng.poll_sensors(vehicle)['front_cam']['pos']))
# print("camera rot output: {}".format(bng.poll_sensors(vehicle)['front_cam']['direction']))
# print("fcam.encode_engine_request() = {}".format(fcam.encode_engine_request()))
damages = bng.poll_sensors(vehicle)['damage']['deform_group_damage']
d = ["{}={}".format(k, damages[k]['damage']) for k in damages.keys()]
print("vehicle pressure after deflation: lowpressure={} damages:".format(
bng.poll_sensors(vehicle)['damage']['lowpressure'], d))
img = m.process_image(image)
after_state["prediction"] = model.predict(img)
plt.imshow(image)
plt.pause(0.01)
bng.close()
return before_state, after_state, deflation_traj
import mmap
def run_scenario(self):
global base_filename, default_model, default_color, default_scenario, setpoint
global prev_error
#vehicle_loadfile = 'vehicles/pickup/pristine.save.json'
# setup DNN model + weights
m = Model()
model = m.define_model_BeamNG("BeamNGmodel-4.h5")
random.seed(1703)
setup_logging()
#beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle', model=default_model,
licence='LOWPRESS', color=default_color)
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario, 'highway')
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.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(100) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# perturb vehicle
#vehicle.ai_set_mode('span')
#vehicle.ai_drive_in_lane(True)
#vehicle_loadfile = 'vehicles/etk800/fronttires_0psi.pc'
# vehicle_loadfile = 'vehicles/etk800/backtires_0psi.pc'
# vehicle_loadfile = 'vehicles/etk800/chassis_forcefeedback201.pc'
# vehicle.load_pc(vehicle_loadfile, False)
vehicle.deflate_tires([1,1,1,1])
#vehicle.break_all_breakgroups()
#vehicle.break_hinges()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
wheelspeed = 0.0; throttle = 0.0; prev_error = setpoint; damage_prev = None; runtime = 0.0
kphs = []
damage = None
final_img = None
# Send random inputs to vehice and advance the simulation 20 steps
for _ in range(1024):
# collect images
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
img = m.process_image(image)
prediction = model.predict(img)
# control params
kph = ms_to_kph(wheelspeed)
throttle = throttle_PID(kph, dt)
brake = 0
#if throttle < 0:
if setpoint < kph:
brake = throttle / 1000.0
throttle = 0.0
# throttle = 0.2 # random.uniform(0.0, 1.0)
# brake = random.choice([0, 0, 0.1 , 0.2])
steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0)
vehicle.control(throttle=throttle, steering=steering, brake=brake)
steering_state = bng.poll_sensors(vehicle)['electrics']['steering']
steering_input = bng.poll_sensors(vehicle)['electrics']['steering_input']
avg_wheel_av = bng.poll_sensors(vehicle)['electrics']['avg_wheel_av']
wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed']
damage = bng.poll_sensors(vehicle)['damage']
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
print("\n")
# #print("steering state: {}".format(steering_state))
# print("AI steering_input: {}".format(steering_input))
#print("avg_wheel_av: {}".format(avg_wheel_av))
# print("DAVE2 steering prediction: {}".format(float(prediction[0][0])))
print("throttle:{}".format(throttle))
print("brake:{}".format(brake))
print("kph: {}".format(ms_to_kph(wheelspeed)))
print("new_damage:{}".format(new_damage))
kphs.append(ms_to_kph(wheelspeed))
if new_damage > 0.0:
final_img = image
break
bng.step(5)
runtime += (0.05)
# 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.pause(0.01)
return runtime, avg_kph, damage['damage'], kphs
def main():
setup_logging()
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('smallgrid', 'test_quat')
vehicle = Vehicle('ego_vehicle',
model='etk800',
color='Blue',
licence="angle")
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
vehicle = Vehicle('ego_vehicle2',
model='etk800',
color='Green',
license="quat")
scenario.add_vehicle(vehicle,
pos=(5, 0, 0),
rot_quat=(-0.00333699025, -0.00218820246,
-0.689169466, 0.724589229))
rb = ScenarioObject(oid='roadblock',
name='sawhorse',
otype='BeamNGVehicle',
pos=(-10, -5, 0),
rot=(0, 0, 0),
scale=(1, 1, 1),
JBeam='sawhorse',
datablock="default_vehicle")
scenario.add_object(rb)
cn = ScenarioObject(oid='cones',
name='cones',
otype='BeamNGVehicle',
pos=(0, -5, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
scale=(1, 1, 1),
JBeam='cones',
datablock="default_vehicle")
scenario.add_object(cn)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press Enter to spawn vehicle during sim with rot and rotquat')
vehicle = Vehicle('ego_vehicle3', model='etk800', color='White')
bng.spawn_vehicle(vehicle, (-10, 0, 0), (0, 0, 0))
vehicle = Vehicle('ego_vehicle4', model='pickup')
pos = (-15, 0, 0)
bng.spawn_vehicle(vehicle, pos, None, rot_quat=(0, 0, 0, 1))
input('press Enter to teleport last vehicle with angle')
bng.teleport_vehicle(vehicle, pos, rot=(0, 45, 0))
input('press Enter to teleport last vehicle with quaternion')
bng.teleport_vehicle(vehicle,
pos,
rot_quat=(-0.00333699025, -0.00218820246,
-0.689169466, 0.724589229))
input('press Enter to teleport roadblock with angle')
bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0))
input('press Enter to teleport roadblock with quaternion')
bng.teleport_scenario_object(rb, (-10, 5, 0),
rot_quat=(-0.003337, -0.0021882,
-0.6891695, 0.7245892))
input('Press ENTER to exit')
finally:
bng.close()
def main():
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
config = Config()
loaded_config = config.load("{}/config.json".format(beamng.home))
# 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(visualized=False)
timer = Timer()
# Attach them
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('back_cam', back_camera)
vehicle.attach_sensor('gforces', gforces)
vehicle.attach_sensor('electrics', electrics)
vehicle.attach_sensor('damage', damage)
vehicle.attach_sensor('timer', timer)
#scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=None, rot_quat=(0, 0, 0.3826834, 0.9238795))
#config_rot_quat = beamngpy.angle_to_quat(config.dir)
# N.B. using rot is deprecated in favor of rot_quat
#scenario.add_vehicle(vehicle, pos=tuple(config.pos), rot=tuple(config.dir), rot_quat=None)
scenario.add_vehicle(vehicle,
pos=tuple(config.pos),
rot=None,
rot_quat=beamngpy.angle_to_quat(config.dir))
# 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()
assert vehicle.skt
# Send random inputs to vehicle and advance the simulation 20 steps
#for _ in range(1024):
for _ in range(100):
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(_))
for s in sensors.keys():
print("{} : {}".format(s, sensors[s]))
damage_dict = vehicle.sensors['damage'].encode_vehicle_request()
damage_dict = sensors['damage']
config.update(sensors['damage'])
config.update(vehicle.state)
config.save('{}/config.json'.format(beamng.home))
gamestate = beamng.get_gamestate()
if _ > 990:
print("late in sim")
finally:
bng.close()
def main():
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario(
'west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR')
vehicle = setup_sensors(vehicle)
lidar = setup_lidar('hood')
vehicle.attach_sensor('lidar', lidar)
spawn_pt = spawn_point()
ego_pos = spawn_pt['pos']
ego_rot_quat = spawn_pt['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles()
scenario.make(beamng)
bng = beamng.open(launch=True)
#try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_set_aggression(0.5)
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
bng.switch_vehicle(vehicle)
damage_prev = None
start = time.time()
end = time.time()
bng.pause()
print("Bounding box: {}".format(vehicle.get_bbox()))
with open('lidar_data.csv', 'w') as f:
f.write(
"TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH\n"
)
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
damage = sensors['damage']
v_state = vehicle.state
print("vehicle_state = {}".format(v_state))
print("vehicle_state[pos] = {}".format(v_state['pos']))
print("vehicle_state[dir] = {}".format(v_state['dir']))
print("Vehicle bounding box:{}".format(vehicle.get_bbox()))
#ai_state = vehicle.ai_get_state()
#print("ai_state = {}".format(ai_state))
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
#print("new damage = {}".format(new_damage))
print()
f.write("{},{},{},{},{},{}\n".format(_ * 0.5, v_state['pos'],
v_state['dir'],
v_state['vel'],
points.tolist(),
str(new_damage)))
#bng.step(30, wait=False)
bng.step(30)
#print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
#print("Time passed since scenario begun: {}\n".format(end - start))
#screenrecord()
if end - start >= 30:
#bng.close()
continue
def getOob(oobList, test, port):
# Gains to port TORCS actuators to BeamNG
# steering_gain = translate_steering()
acc_gain = 0.5 # 0.4
brake_gain = 1.0
# BeamNG images are too bright for DeepDrive
brightness = 0.4
# Set up first vehicle
# ! A vehicle with the name you specify here has to exist in the scenario
vehicle = Vehicle('egovehicle')
scenario = Scenario('asfault', 'asfault', True)
# Set up sensors
resolution = (280, 210)
# Original Settings
#pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below
# 0.4 is inside
pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below
direction = (0, 1, 0)
# direction = (180, 0, 180)
# FOV 60, MAX_SPEED 100, 20 (3) Hz fails
# FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
# FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
# FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
# FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
# FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
fov = 60
# MAX_SPEED = 70
MAX_FPS = 60
SIMULATION_STEP = 6
# Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
#
# WORKING SETTINGS: 20 Freq, 90 FOV.
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
with open(pathToScenarioBeamng + '/asfault.json', 'r') as f:
jsonScenario = json.load(f)
with open(pathToScenarioBeamng + '/asfault.lua', 'r') as f:
luaScenario = f.readlines()
with open(pathToScenarioBeamng + '/asfault.prefab', 'r') as f:
prefabScenario = f.readlines()
with open(pathToScenarioDocuments + '/asfault.json', 'w') as outfile:
json.dump(jsonScenario, outfile)
with open(pathToScenarioDocuments + '/asfault.lua', 'w') as outfile:
outfile.writelines(luaScenario)
with open(pathToScenarioDocuments + '/asfault.prefab', 'w') as outfile:
outfile.writelines(prefabScenario)
deep_drive_engaged = True
STATE = "NORMAL"
notFinished = True
portCount = 1
multiplier = 2
turtleMode = False
turtleSpeed = 10
offRoad = False
oobSegKey = None
oobSearch = False
print(oobList)
speedList = []
currentSpeed = oobList[0][0][0]
for speed in oobList[0][0]:
speedList.append(speed)
if speed > currentSpeed:
currentSpeed = speed
currentSpeed = currentSpeed * 3.6
waypointList = []
resultList = []
oobWaypoint = []
newOobWaypointList = []
for key in oobList[0][1]:
waypointList.append(key)
print(waypointList)
for waypoint in waypointList:
for key in waypointList:
print(key)
for k in key:
oobWaypoint.append(k)
resultList.append(k)
while notFinished:
# Connect to running beamng
setup_logging()
beamng = BeamNGpy('localhost', port)
bng = beamng.open(launch=True)
scenario.make(bng)
bng.load_scenario(scenario)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle, port + portCount)
bng.start_scenario()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
testDic = RoadTest.to_dict(test)
parentage = testDic['network']['parentage']
for parent in parentage:
roadKeys = []
for keys in parentage:
for key in keys:
roadKeys.append(key)
nodesDict = testDic['network']['nodes']
nodes = []
for node in nodesDict:
nodes.append(node)
vehicle.update_vehicle()
oobCount = 0
inOob = False
vehicle.ai_set_speed(70)
if oobSearch:
if turtleMode:
currentSpeed = turtleSpeed
else:
currentSpeed = currentSpeed - 10
print(currentSpeed)
running = True
while running:
# Resume the execution
# 6 steps correspond to 10 FPS with a resolution of 60FPS
# 5 steps 12 FPS
# 3 steps correspond to 20 FPS
#bng.step(SIMULATION_STEP)
# # TODO: Is there a way to query for the speed directly ?
#speed = math.sqrt(vehicle.state['vel'][0] * vehicle.state['vel'][0] + vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Speed is M/S ?
# print("Speed from BeamNG is: ", speed, speed*3.6)
#vehicle.ai_drive_in_lane(True)
vehicle.update_vehicle()
tup = get_segment(test, vehicle.state)
currentSegmentDict = None
currentSegKey = None
if tup is not None:
currentSegmentDict = NetworkNode.to_dict(tup)
currentSegKey = currentSegmentDict['key']
if offRoad:
if oobSegKey in resultList:
index = resultList.index(oobSegKey)
lostSeg = resultList.pop(index)
print('resList without seg', resultList)
print(lostSeg)
if oobSegKey not in newOobWaypointList:
newOobWaypointList.append(oobSegKey)
print('new OOB list', newOobWaypointList)
offRoad = False
if inOob and off_track(test, vehicle.state):
print('off track')
offRoad = True
oobCount = oobCount + 1
if currentSegKey in oobWaypoint:
vehicle.ai_drive_in_lane(True)
inOob = True
oobSegKey = currentSegKey
vehicle.ai_set_speed(currentSpeed)
else:
inOob = False
vehicle.ai_set_speed(70)
vehicle.ai_set_waypoint('waypoint_goal')
vehicle.ai_drive_in_lane(True)
if goal_reached(test, vehicle.state):
print(turtleMode)
for res in resultList:
segOobDict = testDic['execution']['seg_oob_count']
segOobDict[res] = currentSpeed / 3.6
if oobCount == 0:
testDic['execution']['oobs'] = 0
print('waypointlist', newOobWaypointList)
print('resultlist', resultList)
test = RoadTest.from_dict(testDic)
return test
else:
running = False
portCount = portCount + 1
multiplier = multiplier + 1
oobSearch = True
oobWaypoint = newOobWaypointList
newOobWaypointList = []
if currentSpeed < 20:
print(currentSpeed)
print('NewoobWay: ', newOobWaypointList)
print('resultlist: ', resultList)
if turtleMode:
for res in oobWaypoint:
print(res)
segOobDict = testDic['execution'][
'seg_oob_count']
segOobDict[res] = 12
testDic['execution']['oobs'] = 0
test = RoadTest.from_dict(testDic)
print(segOobDict)
print(resultList)
running = False
oobCount = 0
return test
else:
turtleMode = True
print('mode on')
bng.close()
finally:
if oobCount == 0:
bng.close()
else:
print('restart')
def 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_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 main():
global base_filename, training_dir, default_model
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario)
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle', model=default_model,
licence='RED', color='Red')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle, pos=spawn_pt['pos'], rot=None, rot_quat=spawn_pt['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)
images = []
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()
vehicle.ai_set_mode('span')
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_aggression(0.1)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# Send random inputs to vehice and advance the simulation 20 steps
imagecount = 0
wheelvel = [0.1, 0.1, 0.1]
with open(f, 'w') as datafile:
datafile.write('filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature\n')
#for _ in range(1024):
for _ in range(32768):
#throttle = 1.0 #random.uniform(0.0, 1.0)
#steering = random.uniform(-1.0, 1.0)
#brake = random.choice([0, 0, 0, 1])
#vehicle.control(throttle=throttle)
# collect images
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
imagecount += 1
filename = "{}{}.bmp".format(base_filename, imagecount)
# collect ancillary data
datafile.write('{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(filename,
str(round(sensors['timer']['time'], 2)),
sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],))
# save the image
image.save(filename)
# step sim forward
bng.step(20)
print('{} seconds passed.'.format(str(round(sensors['timer']['time'], 2))))
# check for stopping condition
for i in range(len(wheelvel)-1):
wheelvel[i] = wheelvel[i+1]
wheelvel[2] = float(sensors['electrics']['wheelspeed'])
print('wheelvel = {}'.format(sum(wheelvel) / 3.0 ))
if sum(wheelvel) / 3.0 == 0.0:
print("avg wheelspeed is zero; exiting...")
bng.close()
break
def main():
setup_logging()
beamng = BeamNGpy('localhost', 64256, home='D:/Programs/BeamNG/trunk')
#scenario = Scenario('west_cost_usa', 'ai_sine')
scenario = Scenario('smallgrid', 'ai_sine')
vehicle = Vehicle('ego_vehicle', model='etk800', license='AI')
#orig = (-769.1, 400.8, 142.8)
orig = (-10, -1309, 0.215133) #smallgrid
#scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, 180))
scenario.add_vehicle(vehicle, pos=orig, rot=(1, 0, 0)) #smallgrid
scenario.make(beamng)
#Run simulation without emg_processing_time to obtain target trajectory.
bng = beamng.open(launch=True)
count = 0
index = 0
vehicle_dynamics = []
try:
print('Running simulation with target trajectory.')
bng.load_scenario(scenario)
bng.start_scenario()
#Set throttle to maintain speed near 7 km/h during turn.
vehicle.control(throttle=0.04)
start_time = time.time()
while True:
bng.step(1)
vehicle.update_vehicle()
if vehicle.state['pos'][1] <= -1319 and count == 0:
vehicle.control(steering=-1.0)
count += 1
if vehicle.state['pos'][1] >= -1319 and count == 1:
break
#Store vehicle dynamics.
vehicle_dynamics.append(','.join(
map(str, [
index,
time.time() - start_time,
math.sqrt(vehicle.state['vel'][0]**2 +
vehicle.state['vel'][1]**2),
vehicle.state['pos'][0], vehicle.state['pos'][1]
])))
print('Time:' + str(time.time() - start_time) + 'Position: ' +
str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str(
math.sqrt(vehicle.state['vel'][0]**2 +
vehicle.state['vel'][1]**2)))
index += 1
finally:
bng.close()
#Save target trajectory results.
heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate']
with open('sim_target_trajectory_left_uturn.txt', "w") as results:
heading = map(str, heading)
results.write(",".join(heading) + '\n' +
"\n".join(str(element) for element in vehicle_dynamics))
results.close()
#Read total EMG signal processing time (seconds/feature) per trial from txt files.
f = [
line.rstrip('\n') for line in open(
'D:/Research Projects/sEMG_control_for_automobiles/putemg-downloader/putemg_examples/total_processing_time_wrist_flexion.txt'
)
][1:]
total_EMG_processing_time = []
for line in f:
total_EMG_processing_time.append(float(line))
#Run simulations that delay steering initiation based based on EMG signal processing time from trials.
for emg_processing_time in range(len(total_EMG_processing_time)):
#for emg_processing_time in range(0,1):
print(
'\n' +
'Running simulations with EMG signal processing time from trial ' +
str(emg_processing_time) + '.')
bng = beamng.open(launch=True)
count = 0
index = 0
vehicle_dynamics = []
try:
bng.load_scenario(scenario)
bng.start_scenario()
#Set throttle to maintain speed near 7 km/h during turn.
vehicle.control(throttle=0.04)
start_time = time.time()
while True:
bng.step(1)
vehicle.update_vehicle()
if vehicle.state['pos'][1] <= -1319 and count == 0:
sleep(total_EMG_processing_time[emg_processing_time])
vehicle.control(steering=-1.0)
count += 1
if vehicle.state['pos'][1] >= -1319 and count == 1:
break
#Store vehicle dynamics.
vehicle_dynamics.append(','.join(
map(str, [
index,
time.time() - start_time,
math.sqrt(vehicle.state['vel'][0]**2 +
vehicle.state['vel'][1]**2),
vehicle.state['pos'][0], vehicle.state['pos'][1]
])))
print('Time:' + str(time.time() - start_time) + 'Position: ' +
str(vehicle.state['pos'][1]) + ' ' + 'Velocity: ' + str(
math.sqrt(vehicle.state['vel'][0]**2 +
vehicle.state['vel'][1]**2)))
index += 1
finally:
bng.close()
heading = ['', 'time', 'speed', 'x_coordinate', 'y_coordinate']
with open('sim_results_trial_' + str(emg_processing_time) + '.txt',
"w") as results:
heading = map(str, heading)
results.write(",".join(heading) + '\n' + "\n".join(
str(element) for element in vehicle_dynamics))
results.close()
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
def main():
global sp
setup_logging()
beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario('west_coast_usa', 'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR', color='Red')
lidar = Lidar()
lidar.__init__(offset=(0, 0, 1.7), direction=(-0.707, -0.707, 0), vres=32,
vangle=0.01, rps=2200000, hz=20, angle=360, max_dist=200,
visualized=True)
vehicle.attach_sensor('lidar', lidar)
ego_sp = spawn_point(sp)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles()
#ps = [(-722.121, 103, 118.675), (-714.121, 101, 118.675), (-715.121, 105, 118.675), (-721.121, 100, 118.675)]
ps = [(ego_pos[0]-10, ego_pos[1]+2, ego_pos[2]),
(ego_pos[0]-10, ego_pos[1]-2, ego_pos[2]),
(ego_pos[0]-14, ego_pos[1]+5, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]),
(ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]),
(ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])]
ps1 = [(ego_pos[0] - 10, ego_pos[1] + 3, ego_pos[2]),
(ego_pos[0] - 10, ego_pos[1] - 1, ego_pos[2]),
(ego_pos[0] - 14, ego_pos[1] + 5, ego_pos[2]),
(ego_pos[0] + 5, ego_pos[1] - 1, ego_pos[2]),
(ego_pos[0] + 8, ego_pos[1] - 4, ego_pos[2]),
(ego_pos[0] + 10, ego_pos[1] + 9, ego_pos[2]),
(ego_pos[0] + 1, ego_pos[1] + 11, ego_pos[2])]
ps_oldorig = [(ego_pos[0]-7, ego_pos[1]+3, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+4, ego_pos[1]+5, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]),
(ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]),
(ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])]
for i in range(len(tvs)):
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
vehicle.ai_set_aggression(0.5) #0.7)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
start = time.time()
end = time.time()
damage_prev = bng.poll_sensors(vehicle)
with open('lidar_data.csv', 'w') as f:
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
#print(points.tolist())
#print()
#f.write("{}\n".format(points.tolist()))
print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
print("Time passed since scenario begun: {}\n".format(end - start))
if end - start >= 30:
bng.close()
bng.step(30, wait=False)
except Exception as e:
print(e)
finally:
bng.close()
def run_spawn_for_parts_config(
vehicle_model='etk800',
loadfile='C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/front17x8rear17x9.json'
):
global base_filename, default_color, default_scenario, setpoint
global prev_error
global fcam
# setup DNN model + weights
m = Model()
model = m.define_model_BeamNG("BeamNGmodel-5.h5")
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='LOWPRESS',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario, 'highway')
# 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.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(100) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# vehicle.load('C:/Users/merie/Documents/BeamNG.research/vehicles/hopper/pristine.json')
if loadfile:
bng.spawn_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'],
partConfig=loadfile)
else:
bng.spawn_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'],
partConfig='vehicles/hopper/custom.pc')
#bng.set_relative_camera()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
bng.set_steps_per_second(100)
bng.set_deterministic()
totalsecs = 0.0
pitch_traj = []
while totalsecs <= 30:
vehicle.update_vehicle()
camera_state = bng.poll_sensors(vehicle)['front_cam']
camera_state['roll'] = vehicle.state['roll']
camera_state['pitch'] = vehicle.state['pitch']
camera_state['yaw'] = vehicle.state['yaw']
pitch_traj.append(round(math.degrees(vehicle.state['pitch'][0]), 4))
print("roll:{}, pitch:{}, yaw:{}".format(vehicle.state['roll'],
vehicle.state['pitch'],
vehicle.state['yaw']))
bng.step(100)
totalsecs += 1
# print("Camera state:{}".format(camera_state))
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
#get_camera_rot_and_pos([-0.3, 1, 1.0], [0, 0.75, 0], before_state['pos'], before_state['dir'], before_state['up'])
img = m.process_image(image)
before_prediction = model.predict(img)
camera_state["prediction"] = before_prediction
plt.imshow(image)
plt.pause(0.01)
bng.close()
return camera_state, vehicle.state, pitch_traj
def collection_run(speed=11, risk=0.6, num_samples=10000):
global base_filename, training_dir, default_model, setpoint
global spawnpoint, steps_per_second
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario, spawnpoint)
random.seed(1703)
setup_logging()
home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.tech.v0.21.3.0' #
beamng = BeamNGpy('localhost', 64256, home=home, user='******')
scenario = Scenario(default_scenario, 'research_test')
# add barriers and cars to get the ego vehicle to avoid the barriers
add_barriers(scenario)
# add_barrier_cars(scenario)
vehicle = Vehicle('ego_vehicle',
model=default_model,
licence='DATAMKR',
color='White')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle,
pos=spawn_pt['pos'],
rot=None,
rot_quat=spawn_pt['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.hide_hud()
bng.set_nondeterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_second) #
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
bng.switch_vehicle(vehicle)
# ai_line, bng = create_ai_line_from_road(spawn_pt, bng)
ai_line, bng = create_ai_line_from_road_with_interpolation(spawn_pt, bng)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# bng.resume()
start_time = time.time()
# Send random inputs to vehicle and advance the simulation 20 steps
imagecount = 0
timer = 0
traj = []
steering_inputs = []
timestamps = []
kphs = []
with open(f, 'w') as datafile:
datafile.write(
'filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature,wheelspeed\n'
)
return_str = ''
while imagecount < num_samples:
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
full_filename = "{}{}{}.jpg".format(training_dir, base_filename,
imagecount)
qualified_filename = "{}{}.jpg".format(base_filename, imagecount)
steering = sensors['electrics']['steering_input']
dt = sensors['timer']['time'] - timer
if dt > 0:
throttle = throttle_PID(
sensors['electrics']['wheelspeed'] * 3.6, dt)
# print("current kph:{} dt:{} throttle:{}".format(sensors['electrics']['wheelspeed'] * 3.6, dt, throttle))
vehicle.update_vehicle()
# points=[vehicle.state['front']]
# point_colors = [[0, 1, 0, 0.1]]
# spheres=[[vehicle.state['front'][0],vehicle.state['front'][1],vehicle.state['front'][1],0.25]]
# sphere_colors=[[1, 0, 0, 0.8]]
# bng.add_debug_line(points, point_colors,
# spheres=spheres, sphere_colors=sphere_colors,
# cling=True, offset=0.1)
try:
steering_setpt = steering_setpoint(vehicle.state, traj)
if dt > 0:
steering = steering_PID(steering, steering_setpt, dt)
vehicle.control(steering=steering, throttle=throttle)
timer = sensors['timer']['time']
# traj.append(vehicle.state['pos'])
# timestamps.append(timer)
# steering_inputs.append(steering)
# kphs.append(sensors['electrics']['wheelspeed'] * 3.6)
except IndexError as e:
print(e, "exiting...")
print(e.with_traceback())
plot_trajectory(traj, "Car Behavior using AI Script")
plot_inputs(timestamps,
steering_inputs,
title="Steering Inputs by Time")
plot_inputs(timestamps, kphs, title="KPHs by Time")
exit(0)
if timer > 10: # and abs(steering) >= 0.1:
# collect ancillary data
datafile.write(
'{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(
qualified_filename,
str(round(sensors['timer']['time'],
2)), sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],
sensors['electrics']['wheelspeed']))
# save the image
image.save(full_filename)
imagecount += 1
print(f"{imagecount=}")
if sensors['damage']['damage'] > 0:
return_str = "CRASHED at timestep {} speed {}; QUITTING".format(
round(sensors['timer']['time'], 2),
round(sensors['electrics']['wheelspeed'] * 3.6, 3))
print(return_str)
break
# if timer > 300:
# break
bng.step(1, wait=True)
plot_trajectory(traj, "Car Behavior using AI Script")
plot_inputs(timestamps, steering_inputs, title="Steering Inputs by Time")
plot_inputs(timestamps, kphs, title="KPHs by Time")
bng.close()
plot_trajectory(traj, "Car Behavior using AI Script")
return return_str
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)
bng = beamng.open(launch=True)
# Create a scenario in west_coast_usa
scenario = Scenario('west_coast_usa',
'tech_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(visualized=False)
timer = Timer()
# Attach them
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('back_cam', back_camera)
vehicle.attach_sensor('gforces', gforces)
vehicle.attach_sensor('electrics', electrics)
vehicle.attach_sensor('damage', damage)
vehicle.attach_sensor('timer', timer)
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(bng)
# Start BeamNG and enter the main loop
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
# 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)
print('{} seconds passed.'.format(sensors['timer']['time']))
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()
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
