def test_vehicle_move(beamng):
with beamng as bng:
bng.set_deterministic()
scenario = Scenario('smallgrid', 'move_test')
vehicle = Vehicle('test_car', model='etk800')
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
scenario.make(bng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.pause()
vehicle.control(throttle=1)
bng.step(120)
vehicle.update_vehicle()
assert np.linalg.norm(vehicle.state['pos']) > 1
scenario.delete(beamng)
def test_vehicle_spawn(beamng):
scenario = Scenario('smallgrid', 'spawn_test')
vehicle = Vehicle('irrelevant', model='pickup')
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
scenario.make(beamng)
with beamng as bng:
bng.load_scenario(scenario)
bng.start_scenario()
other = Vehicle('relevant', model='etk800')
scenario.add_vehicle(other, pos=(10, 10, 0), rot=(0, 0, 0))
other.update_vehicle()
assert 'pos' in other.state
bng.step(120)
scenario.remove_vehicle(other)
bng.step(600)
assert other.state is None
def collect_data(beamNG_path):
bng = BeamNGpy('localhost', 64256, beamNG_path)
#bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')
scenario = Scenario('GridMap', 'example')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle1 = Vehicle('ego_vehicle',
model='etk800',
licence='PYTHON',
color='Red')
#vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue')
electrics = Electrics()
vehicle1.attach_sensor('electrics', electrics)
pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
# Place files defining our scenario for the simulator to read
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
#SIZE = 50
try:
bng.load_scenario(scenario)
bng.start_scenario()
#vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
vehicle1.ai_set_mode('span')
#collect data
print("\n Position and rotation of car \n ")
# for _ in range(200):
for _ in range(200):
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle1.state['pos'])
directions.append(vehicle1.state['dir'])
print([vehicle1.state['pos'], vehicle1.state['dir']])
#write data into file
# print ("position :",positions)
# print ("position :",directions)
sys_output.print_title(
"\n The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \""
)
write_data(FILE_POS, positions)
write_data(FILE_ROT, directions)
bng.stop_scenario()
bng.close()
time.sleep(2)
finally:
bng.close()
return (positions, directions)
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
car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue')
scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55))
scenario.make(beamng)
bng = beamng.open(launch=True)
bng.load_scenario(scenario)
bng.start_scenario()
vut.ai_set_mode('span')
vut.ai_drive_in_lane(True)
car_1.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}])
for _ in range(240):
sleep(0.1)
vut.update_vehicle()
sensors = bng.poll_sensors(vut)
dmg = sensors['damage']
# Below code snippet is generated form 'detect_obstacle_car' function for car_1
scenario.update()
dist_car_1 = np.linalg.norm(
np.array(vut.state['pos']) - np.array(car_1.state['pos']))
if dist_car_1 < 8:
print('Car Detected')
# Below code snippet is generated form 'ai_stopped' function for car_1
scenario.update()
if sensors['electrics']['values']['wheelspeed'] == 0 or dmg[
'damage'] == 0:
import mmap
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()
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():
global default_model, default_scenario
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
#scenario = Scenario('smallgrid', 'spawn_objects_example')
scenario = Scenario(default_scenario,
'research_test',
description='Random driving for research')
vehicle = Vehicle('ego_vehicle', model=default_model, licence='PYTHON')
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario)
scenario.add_vehicle(vehicle,
pos=spawn['pos'],
rot=spawn['rot'],
rot_quat=spawn['rot_quat'])
scenario.make(beamng)
bng = beamng.open()
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.update_vehicle()
d1 = bng.poll_sensors(vehicle)
cum_list = []
bound = 0.0
for i in range(3):
for _ in range(45):
bound = bound + 0.0 # 0.1
# vehicle.save()
vehicle.update_vehicle()
d2 = bng.poll_sensors(vehicle)
throttle = 1.0
#throttle = random.uniform(0.0, 1.0)
steering = random.uniform(-1 * bound, bound)
brake = 0.0 #random.choice([0, 0, 0, 1])
vehicle.control(throttle=throttle, steering=steering, brake=brake)
pointName = "{}_{}".format(i, _)
cum_list.append(pointName)
vehicle.saveRecoveryPoint(pointName)
bng.step(20)
print("SEGMENT #{}: COMPARE DAMAGE".format(i))
damage_diff = compare_damage(d1, d2)
d1 = d2
# "Back up" 1 second -- load vehicle at that time in that position.
backup_pointName = backup(cum_list, 0.001)
print('recovering to {}'.format(pointName))
loadfile = vehicle.loadRecoveryPoint(backup_pointName)
print('loadfile is {}'.format(loadfile))
bng.pause()
vehicle.update_vehicle()
vehicle.load(loadfile)
#vehicle.load("vehicles/pickup/vehicle.save.json")
bng.resume()
#vehicle.startRecovering()
#time.sleep(1.5)
#vehicle.stopRecovering()
vehicle.update_vehicle()
bng.pause()
time.sleep(2)
# vehicle.load("vehicles/pickup/vehicle.save.json")
bng.resume()
bng.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():
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 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
def run_scenario_ai_version(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0],
parts_config=None):
global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec
global prev_error
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean',
user="******")
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='AI',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = get_spawn_point(default_scenario, default_spawnpoint)
# scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# create vehicle to be chased
chase_vehicle = Vehicle('chase_vehicle',
model='miramar',
licence='CHASEE',
color='Red')
bng.spawn_vehicle(chase_vehicle,
pos=(469.784, 346.391, 144.982),
rot=None,
rot_quat=(-0.0037852677050978, -0.0031219546217471,
-0.78478640317917, 0.61974692344666))
bng.spawn_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'],
partConfig=parts_config)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
#bng.resume()
# perturb vehicle
print("vehicle position before deflation via beamstate:{}".format(
vehicle.get_object_position()))
print("vehicle position before deflation via vehicle state:{}".format(
vehicle.state))
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
plt.imshow(image)
plt.pause(0.01)
vehicle.deflate_tires(deflation_pattern)
bng.step(steps_per_sec * 6)
vehicle.update_vehicle()
# print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
# print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
pitch = vehicle.state['pitch'][0]
roll = vehicle.state['roll'][0]
z = vehicle.state['pos'][2]
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
plt.imshow(image)
plt.pause(0.01)
bng.resume()
vehicle.ai_set_mode('chase')
vehicle.ai_set_target('chase_vehicle')
vehicle.ai_drive_in_lane(True)
damage_prev = None
runtime = 0.0
traj = []
kphs = []
for _ in range(650):
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
damage = bng.poll_sensors(vehicle)['damage']
wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed']
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
runtime = bng.poll_sensors(vehicle)['timer']['time']
vehicle.update_vehicle()
traj.append(vehicle.state['pos'])
kphs.append(ms_to_kph(wheelspeed))
if new_damage > 0.0:
break
bng.step(5)
bng.close()
results = {
'runtime': round(runtime, 3),
'damage': damage,
'kphs': kphs,
'traj': traj,
'pitch': round(pitch, 3),
'roll': round(roll, 3),
"z": round(z, 3),
'final_img': image
}
return results
def test_vehicle_ai(beamng):
with beamng as bng:
bng.set_deterministic()
scenario = Scenario('west_coast_usa', 'ai_test')
vehicle = Vehicle('test_car', model='etk800')
other = Vehicle('other', model='etk800')
pos = [-717.121, 101, 118.675]
scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45))
scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45))
scenario.make(bng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.pause()
vehicle.ai_set_mode('span')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_waypoint('Bridge4_B')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_target('other', mode='chase')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_target('other', mode='flee')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
script = [
{
'x': -735,
'y': 86.7,
'z': 119,
't': 0
},
{
'x': -752,
'y': 70,
'z': 119,
't': 5
},
{
'x': -762,
'y': 60,
'z': 119,
't': 8
},
]
vehicle.ai_set_script(script)
bng.step(600, wait=True)
vehicle.update_vehicle()
ref = [script[1]['x'], script[1]['y'], script[1]['z']]
pos = vehicle.state['pos']
ref, pos = np.array(ref), np.array(pos)
assert np.linalg.norm(ref - pos) < 2.5
scenario.delete(beamng)
def 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')
class BeamNG_ACC_Test():
def __init__(self, **kwargs):
keys = kwargs.keys()
if "test_controller" in keys:
self.controller = kwargs.get("test_controller")
else:
self.controller = ACC_Test()
config = BeamNG_Cruise_Control.Config(KP=CC_P,
KI=CC_I,
KD=CC_D,
target=ACC_CAR_SPEED)
config2 = BeamNG_Cruise_Control.Config(KP=CC_P,
KI=CC_I,
KD=CC_D,
target=FRONT_CAR_SPEED)
self.PID = BeamNG_Cruise_Control.PID_Controller(config=config)
self.PID2 = BeamNG_Cruise_Control.PID_Controller(config=config2)
def setup_bngServer(self):
# Instantiate BeamNGpy instance running the simulator from the given path,
if ('BNG_HOME' in os.environ):
self.bng = BeamNGpy('localhost',
64256,
home=os.environ['BNG_HOME'])
else:
print(
"WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)"
)
self.bng = BeamNGpy(
'localhost',
64256,
home='C:\\Users\\Ingars\\Beamng-unlimited\\trunk')
def setup_BeamNG(self):
# Create a scenario in test map
self.scenario = Scenario('cruise-control', 'example')
# Create an ETK800 with the licence plate 'PID'
self.vehicle = Vehicle('ACC', model='etk800', licence='ACC')
electrics = Electrics()
self.vehicle.attach_sensor('electrics',
electrics) # Cars electric system
# Add it to our scenario at this position and rotation
self.scenario.add_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 0))
self.vehicle2 = Vehicle('FRONT', model='etk800', licence='FRONT')
self.vehicle2.attach_sensor('electrics',
electrics) # Cars electric system
self.scenario.add_vehicle(self.vehicle2,
pos=FRONT_CAR_POS,
rot=(0, 0, 180))
# Place files defining our scenario for the simulator to read
self.scenario.make(self.bng)
# Launch BeamNG.research
self.bng.open()
# Load and start our scenario
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
def runTest(self):
self.bng.restart_scenario()
self.bng.teleport_vehicle(self.vehicle,
pos=ACC_CAR_POS,
rot=(0, 0, 180))
self.run()
def run(self):
self.setupTest()
wanted_t = ACC_CAR_SPEED
wanted_d = WANTED_DISTANCE
soft_brake_d = wanted_d - (wanted_d / 2)
wanted_d_buffer = wanted_d - (wanted_d / 5)
# Wanted distance has a buffer of 25%, to compensate holding same constant speed at bigger distance
deacc_d = wanted_d_buffer + 10
# +10 meters is buffer for starting to decelerate towards front car speed and wanted distance.
# the buffer should be calculated dynamically instead to compensate for different speeds
while self.controller.ended == False:
start_time = datetime.datetime.now()
sensors = self.bng.poll_sensors(self.vehicle)
position = self.vehicle.state['pos']
position2 = self.vehicle2.state['pos']
sensors2 = self.bng.poll_sensors(
self.vehicle2
) # replace with self.vehicle.update_vehicle when not using pre-generated coordinates of vehicle to follow
wheel_speed = sensors['electrics']['values']['wheelspeed']
distance = self.controller.setDistance(position, position2)
front_car_speed = self.controller.getFrontCarSpeed(wheel_speed)
curr_target = self.PID.get_target()
print("distance: " + str(distance))
#FORMULA USED for deacceleration = front_car_speed^2 = wheel_speed^2 + (distance-20)*2*a
#d = (front_car_speed - wheel_speed) / (-0.3 * 2) # 0.3 deacceleration? distance for reducing speed to front cars speed with -0.3 acceleration
#print("d: " + str(d))
if distance < 5: # 5 is manually set as last allowed distance between both cars, will not work good if ACC car is driving too fast. Would be better to calculate it as braking distance.
print("brake1")
self.vehicle.control(brake=1)
self.vehicle.control(throttle=0)
elif distance < soft_brake_d:
print("brake1")
self.vehicle.control(
brake=0.1
) #Softness of brake could also be calculated dynamically to compensate different speeds
self.vehicle.control(throttle=0)
else:
if distance <= wanted_d_buffer:
print("wanted")
if front_car_speed > curr_target + 3.5 or front_car_speed < curr_target - 2: #or front_car_speed < curr_target - 1 // calibrate 3.5 and 2
self.PID.change_target(max(front_car_speed, 0))
elif distance <= deacc_d:
a = (front_car_speed - wheel_speed) / (
(distance - wanted_d_buffer) * 2)
print("a:" + str(a))
self.PID.change_target(a + wheel_speed)
elif curr_target != wanted_t:
self.PID.change_target(wanted_t)
print("throttle1")
value = self.PID.calculate_throttle(wheel_speed)
value = min(1, value)
self.vehicle.control(brake=0)
self.vehicle.control(throttle=value)
#PID for front car
wheel_speed2 = sensors2['electrics']['values']['wheelspeed']
# print("real front:" + str(wheel_speed2))
value2 = self.PID2.calculate_throttle(wheel_speed2)
value2 = min(1, value2)
self.vehicle2.control(brake=0)
self.vehicle2.control(throttle=value2)
elapsed_time = datetime.datetime.now() - start_time
while (elapsed_time.total_seconds() * 1000) < 100:
elapsed_time = datetime.datetime.now() - start_time
elapsed_total = self.controller.calculateElapsed()
# Change front car speed after 10 seconds and after 20 seconds
if elapsed_total.total_seconds() > float(10):
self.PID2.change_target(20)
if elapsed_total.total_seconds() > float(20):
self.PID2.change_target(10)
print("Ending Test")
def close(self):
self.bng.close()
def setupTest(self):
self.vehicle.update_vehicle()
self.controller.last_position = self.vehicle.state['pos']
self.controller.start()
def GenerateTrack(trackLength, sampleRate, show, startBeamNG):
populationSize = 100
maxAcc = 1
times = 20
relNewSize = 0.6
duplicatesThreshold = 0.05
intersectionDelta = 0.01
mutationProb = 0.1
mutationDeviation = 0.01
print("Generating Tracks")
pop = initPop(populationSize, trackLength, maxAcc, 20)
pop = evolve(pop, times, relNewSize, duplicatesThreshold,
intersectionDelta, mutationProb, mutationDeviation, 10)
print("eliminating intersecting tracks")
pop = elminateIntersecting(pop, intersectionDelta)
if show:
plotTrack(pop, 100)
tracks = []
nr = 0
first = True
for track in pop:
track = np.vstack((track, completeAcc(track)))
poly = polysFromAccs(track)
bez = bezFromPolys(poly)
smpl = sampleBezSeries(bez, sampleRate).transpose()
smpl = scaleTrack(smpl, 100, 100)
smpl = np.array(smpl)
smpl = np.vstack((smpl, [smpl[0]]))
if(first):
writeCriteria(smpl)
first = False
tracks.append(smpl)
writeTrack(smpl, nr)
nr += 1
if startBeamNG:
nodes = []
for p in tracks[0]: nodes.append((p[0], p[1], 0, 7))
beamng = BeamNGpy('localhost', 64256)
vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Green')
scenario = Scenario('GridMap_v0422', 'track test')
scenario.add_vehicle(vehicle, pos=(0, 0, -16.64), rot=(0, 0, 180))
road = Road(material='track_editor_C_center', rid='main_road', texture_length=5, looped=True)
road.nodes.extend(nodes)
scenario.add_road(road)
scenario.make(beamng)
beamng.open()
beamng.load_scenario(scenario)
beamng.start_scenario()
vehicle.ai_set_mode('span')
while 1:
vehicle.update_vehicle()
print(vehicle.state['pos'])
sleep(1)
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 = {
'pos': (0, 0, 0.163609),
'speed': 20,
}
node1 = {
'pos': (0, 100, 0.163609),
'speed': 30,
}
script.append(node0)
script.append(node1)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_line(script)
# update the state of vehicle at impact.
for _ in range(100):
time.sleep(0.1)
vehicle.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle
) # Polls the data of all sensors attached to the vehicle
print(vehicle.state['pos'])
if vehicle.state['pos'][1] > 99:
print('free state')
vehicle.control(throttle=0,
steering=0,
brake=0,
parkingbrake=0)
vehicle.update_vehicle()
bng.stop_scenario()
for _ in range(20):
time.sleep(0.1)
bng.load_scenario(scenario)
bng.start_scenario()
node0 = {
'pos': (0, 50, 0.163609),
'speed': 20,
}
node1 = {
'pos': (0, 100, 0.163609),
'speed': 30,
}
script.append(node0)
script.append(node1)
vehicle.ai_set_line(script)
input('Press enter when done...')
finally:
bng.close()
def createCrashSimulation():
print("Crash Simulation")
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_crash_simulation')
road_a = Road('track_editor_C_center', looped=False)
road_b = Road('track_editor_C_center', looped=False)
nodesa = [(30, 0, -4, 4), (0, 0, -4, 4)]
nodesb = [(0, 30, -4, 4), (0, 0, -4, 4)]
road_a.nodes.extend(nodesa)
road_b.nodes.extend(nodesb)
# Create an ETK800 with the licence plate 'PYTHON'
vehicleA = Vehicle('ego_vehicleA', model='etk800', licence='PYTHON')
# Add it to our scenario at this position and rotation
damageS = Damage()
vehicleA.attach_sensor('damagesS', damageS)
scenario.add_vehicle(vehicleA, pos=(30, 0, 0), rot=(0, 0, 90))
# Create an ETK800 with the licence plate 'PYTHON'
vehicleB = Vehicle('ego_vehicleB', model='etk800', licence='PYTHON')
# Add it to our scenario at this position and rotation
damageV = Damage()
vehicleB.attach_sensor('damagesV', damageV)
scenario.add_vehicle(vehicleB, pos=(0, 30, 0), rot=(0, 0, 0))
scenario.add_road(road_a)
scenario.add_road(road_b)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
node0 = {
'pos': (30, 0, 0),
'speed': 0,
}
node1 = {
'pos': (0, 0, 0),
'speed': 20,
}
script = list()
script.append(node0)
script.append(node1)
vehicleA.ai_set_line(script)
node3 = {
'pos': (0, 30, 0),
'speed': 0,
}
node4 = {
'pos': (0, 0, 0),
'speed': 20,
}
script = list()
script.append(node3)
script.append(node4)
vehicleB.ai_set_line(script)
time.sleep(12)
# input('Press enter when done...')
vehicleA.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleA)
damage_striker = sensors['damagesS']
print(sensors['damagesS'])
print(vehicleA.state['pos'])
vehicleB.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleB)
damage_victim = sensors['damagesV']
print(sensors['damagesV'])
print(vehicleB.state['pos'])
multiObjectiveFitnessFunction(123456789, damage_striker,
vehicleA.state['pos'], (0, 0),
damage_victim, vehicleB.state['pos'],
(0, 0))
# multiobjective fitness function.
bng.stop_scenario()
# bng.load_scenario(scenario)
# bng.start_scenario()
#
# print("sleep")
# time.sleep(10)
# print("wake")
#
# node0 = {
# 'pos': (30, 0, 0),
# 'speed': 0,
# }
#
# node1 = {
# 'pos': (0, 0, 0),
# 'speed': 30,
# }
#
# script = list()
# script.append(node0)
# script.append(node1)
#
# vehicleA.ai_set_line(script)
#
# node0 = {
# 'pos': (0, 30, 0),
# 'speed': 0,
# }
#
# node1 = {
# 'pos': (0, 0, 0),
# 'speed': 30,
# }
#
# script = list()
# script.append(node0)
# script.append(node1)
#
# vehicleB.ai_set_line(script)
#
# input('Press enter when done...')
finally:
bng.close()
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
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()
node3 = {
'pos': (collision_points[0][0], collision_points[0][1], 0),
'speed': victim_speeds[0],
}
script = list()
script.append(node2)
script.append(node3)
vehicleVictim.ai_set_line(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] > 306 and vehicleStriker.state[
'pos'][1] > 39:
# 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
def scenario(beamNG_path):
if (beamNG_path == ""):
beamNG_path = 'D:/BeamNGReasearch/Unlimited_Version/trunk'
bng = BeamNGpy('localhost', 64256, beamNG_path)
#scenario = Scenario('west_coast_usa', 'example')
scenario = Scenario('GridMap', 'example')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle1 = Vehicle('ego_vehicle', model='etk800', licence='PYTHON', color='Red')
vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue')
electrics = Electrics()
vehicle1.attach_sensor('electrics', electrics)
pos2 = (-13.04469108581543, -107.0409164428711, 0.202297180891037)
pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
rot2 = (0.0009761620895005763, -0.9999936819076538, -0.0034209610894322395)
rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2)
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
SIZE = 500
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
vehicle1.ai_set_mode('span')
vehicle2.ai_set_mode('chase')
#collect data
positions = list()
directions = list()
wheel_speeds = list()
for _ in range(100):
time.sleep(0.1)
vehicle1.update_vehicle() # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicle1) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle1.state['pos'])
directions.append(vehicle1.state['dir'])
wheel_speeds.append(sensors['electrics']['values']['wheelspeed'])
print('The Final result - position :')
print(positions)
print('The Final result - direction :')
print(directions)
print('The Final result - speed :')
print(wheel_speeds)
bng.stop_scenario()
bng.close()
finally:
bng.close()
# display(sensors['electrics']['values']['brake'])
# print('The Damage is:')
# display(sensors['damages'])
# display(damage)
positions = list()
directions = list()
wheel_speeds = list()
throttles = list()
brakes = list()
damages = list()
for _ in range(150):
time.sleep(0.1)
vehicle.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle.state['pos'])
directions.append(vehicle.state['dir'])
wheel_speeds.append(sensors['electrics']['values']['wheelspeed'])
throttles.append(sensors['electrics']['values']['throttle'])
brakes.append(sensors['electrics']['values']['brake'])
damages.append(sensors['damages'])
bng.close()
#print(wheel_speeds)
# print(directions)
# print(damages[-1])
def launch(beamNGPath, car1, car2, speed_car2):
crash = False
dist_2car = 20
speed_car2 = int(speed_car2)
bng = BeamNGpy('localhost', 64256, beamNG_path)
#bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')
scenario = Scenario('GridMap', 'example')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle1 = Vehicle('ego_vehicle',
model='etk800',
licence='PYTHON',
color='Red')
vehicle2 = Vehicle('vehicle',
model='etk800',
licence='CRASH',
color='Blue')
electrics1 = Electrics()
electrics2 = Electrics()
vehicle1.attach_sensor('electrics1', electrics1)
vehicle2.attach_sensor('electrics2', electrics2)
#position to try drive then teleport
#-365.2436828613281, -214.7460479736328, 1.2118444442749023], [0.9762436747550964, 0.20668958127498627, 0.0650215595960617]]
#[[-362.4477844238281, -214.16226196289062, 1.32931387424469], [0.9824153780937195, 0.1852567195892334, 0.023236412554979324]]
pos2 = (25.75335693359375, -127.78406524658203, 0.2072899490594864)
pos1 = (-88.8136978149414, -261.0204162597656, 0.20253072679042816)
#pos_tel1 = (53.35311508178711, -139.84017944335938, 0.20729705691337585) #change this
#pos_tel2 = (75.94310760498047, -232.62135314941406, 0.20568031072616577) #change this
pos_tel1 = car1[0]
pos_tel2 = car2[0]
rot2 = (0.9298766851425171, -0.36776003241539, 0.009040255099534988)
rot1 = (-0.9998571872711182, 0.016821512952446938, -0.0016229493776336312)
# rot_tel1= (0.8766672611236572, -0.4810631573200226, 0.005705998744815588) #change this
# rot_tel2 = (-0.896364688873291, -0.4433068335056305, -0.0030648468527942896) #change this
rot_tel1 = car1[1]
rot_tel2 = car2[1]
#initial postion of two car.
# run 2cars on particular road until they reach particular speed which satisfies the condisiton of the given testcase
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2)
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle1.ai_set_speed(10, mode='set')
vehicle1.ai_set_mode('span')
vehicle2.ai_set_speed(speed_car2,
mode='set') ##change this param of speed
vehicle2.ai_set_mode('chase')
sys_output.print_sub_tit("Initial Position and rotation of car")
print("\nInitial Position and rotation of car1 %s %s " %
(pos1, rot1))
print("\nInitial Position and rotation of car2 %s %s " %
(pos2, rot2))
sys_output.print_sub_tit(
"\n when speed of car 1 rearch 10 and speed car 2 reach %s. Two cars are teleport to new locations."
% (speed_car2))
for _ in range(450):
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
vehicle2.update_vehicle()
sensors1 = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
sensors2 = bng.poll_sensors(vehicle2)
speed1 = sensors1['electrics1']['values']['wheelspeed']
speed2 = sensors2['electrics2']['values']['wheelspeed']
print("speed of car 1", speed1)
print("speed of car 2", speed2)
#print([vehicle1.state['pos'],vehicle1.state['dir']])
if speed1 > 9 and speed2 > speed_car2 - 1: #change speed here
bng.teleport_vehicle(vehicle1, pos_tel1, rot_tel1)
bng.teleport_vehicle(vehicle2, pos_tel2, rot_tel2)
sys_output.print_sub_tit("Teleport 2 car to new location")
print("\n Car1 : " + str(pos_tel1) + ", " + str(rot_tel1))
print("\n Car2 : " + str(pos_tel2) + ", " + str(rot_tel2))
print("\n Distance between two cars: " +
str(distance.euclidean(pos_tel1, pos_tel2)))
break
# if speed > 19:
# bng.teleport_vehicle(vehicle1,pos_tel,rot_tel )
# break
for _ in range(100):
#pos1 = []
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
vehicle2.update_vehicle()
sensors1 = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
sensors2 = bng.poll_sensors(vehicle2)
speed1 = sensors1['electrics1']['values']['wheelspeed']
speed2 = sensors2['electrics2']['values']['wheelspeed']
#pos1.append(vehicle1.state['pos'])
#pos2.append(vehicle2.state['pos'])
dist_2car = distance.euclidean(vehicle1.state['pos'],
vehicle2.state['pos'])
if dist_2car < 5: #or int(speed2)== 0 :
crash = True
print(
"\n Failed because distance of two cars are less than 5")
break
print("\n speed1 %s, speed2: %s, distance: %s" %
(speed1, speed2, dist_2car))
if int(speed2) == 0:
print("\n Pass because car2 stoped")
break
bng.stop_scenario()
bng.close()
finally:
bng.close()
return crash
