def test_vehicle_move(beamng):
with beamng as bng:
bng.set_steps_per_second(50)
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, wait=True)
vehicle.poll_sensors()
assert np.linalg.norm(vehicle.sensors['state'].data['pos']) > 1
scenario.delete(beamng)
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_electrics(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'electrics_test')
vehicle = Vehicle('test_car', model='etk800')
electrics = Electrics()
vehicle.attach_sensor('electrics', electrics)
scenario.add_vehicle(vehicle, pos=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(120)
vehicle.control(throttle=1.0)
bng.step(360)
vehicle.poll_sensors()
assert electrics.data['airspeed'] > 0
assert electrics.data['wheelspeed'] > 0
assert electrics.data['throttle_input'] > 0
direction,
fov,
resolution,
near_far=(0.5, 300),
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
beamng = BeamNGpy('localhost', 64256)
bng = beamng.open(launch=False)
bng.set_deterministic()
bng.set_steps_per_second(MAX_FPS)
bng.connect_vehicle(vehicle)
bng.pause()
while True:
bng.step(SIMULATION_STEP)
sensors = bng.poll_sensors(vehicle)
speed = math.sqrt(vehicle.state['vel'][0] * vehicle.state['vel'][0] +
vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Retrieve image and preprocess it
imageData = preprocess(sensors['front_cam']['colour'], brightness)
# Retrieve controls based on camera-image
controls = controller.getControl(imageData, speed)
# Apply controls
vehicle.control(throttle=controls.throttle, steering=controls.steering)
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()
colour=True)
scenario.add_camera(cam, 'cam')
scenario.make(beamng)
bng = beamng.open()
bng.set_deterministic()
bng.load_scenario(scenario)
bng.start_scenario()
meshes = scenario.find_procedural_meshes()
ring_pos = None
for mesh in meshes:
if mesh.name == 'pyring':
ring_pos = np.array(mesh.position)
for i in range(5)[1:]:
vehicle.control(throttle=i / 4)
bng.step(150)
scenario.update()
distance = np.linalg.norm(np.array(vehicle.state['pos']) - ring_pos)
while distance > 5:
scenario.update()
distance = np.linalg.norm(np.array(vehicle.state['pos']) - ring_pos)
frames = scenario.render_cameras()
bng.close()
plt.figure(figsize=(20, 20))
imshow(np.asarray(frames['cam']['colour'].convert('RGB')))
rot=(0, 0, 90))
#add vehicle as Obstacle
vehicle1 = Vehicle('test_vehicle', model='etk800', licence='RED', color='Blue')
scenario.add_vehicle(vehicle1,
pos=(-9.66595, -90.7465, -3.45737),
rot=(0, 0, 180))
scenario.make(beamng)
bng = beamng.open()
bng.load_scenario(scenario)
#set the ego vehicle to AI mode
vehicle.ai_set_mode('Manual')
vehicle.ai_set_waypoint('DecalRoad5248_4')
vehicle1.control(throttle=0.44)
bng.start_scenario()
# After loading, the simulator waits for further input to actually start
positions = list()
directions = list()
wheel_speeds = list()
speed = list()
brakes = list()
damage1 = list()
vehicle.update_vehicle()
sensors = bng.poll_sensors(vehicle)
print('The vehicle position is:')
print(vehicle.state['pos'])
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
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 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()
class Simulation(object):
def __init__(self) -> None:
super().__init__()
thread = Thread(target=self._intervene)
thread.start()
self.step = 0
self.dist_driven = 0
self.done = False
self.last_action = (0.0, 0.0)
self.bng = BeamNGpy('localhost', 64257, home=BEAMNG_HOME)
self.scenario = Scenario('train', 'train', authors='Vsevolod Tymofyeyev',
description='Reinforcement learning')
self.road = TrainingRoad(ASFAULT_PREFAB)
self.road.calculate_road_line()
spawn_point = self.road.spawn_point()
self.last_pos = spawn_point.pos()
self.scenario.add_road(self.road.asphalt)
self.scenario.add_road(self.road.mid_line)
self.scenario.add_road(self.road.left_line)
self.scenario.add_road(self.road.right_line)
self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='RL FTW', color='Red')
front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV,
resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
colour=True, depth=False, annotation=False)
self.vehicle.attach_sensor("front_camera", front_camera)
self.scenario.add_vehicle(self.vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
self.scenario.make(self.bng)
prefab_path = self.scenario.get_prefab_path()
update_prefab(prefab_path)
self.bng.open()
self.bng.set_deterministic()
self.bng.set_steps_per_second(SPS)
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
# self.bng.hide_hud()
self.bng.pause()
def _intervene(self):
while True:
a = input()
self.done = not self.done
def take_action(self, action):
steering, throttle = action
steering = steering.item()
throttle = throttle.item()
self.last_action = action
self.step += 1
self.vehicle.control(steering=steering, throttle=throttle, brake=0.0)
self.bng.step(STEPS_INTERVAL)
def _reward(self, done, dist):
steering = self.last_action[0]
throttle = self.last_action[1]
velocity = self.velocity() # km/h
if not done:
reward = REWARD_STEP + THROTTLE_REWARD_WEIGHT * throttle #- MID_DIST_PENALTY_WEIGHT * dist
else:
reward = REWARD_CRASH - CRASH_SPEED_WEIGHT * throttle
return reward
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
image = sensors['front_camera']['colour'].convert("RGB")
image = np.array(image)
r = ROI
# Cut to the relevant region
image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])]
# Convert to BGR
state = image[:, :, ::-1]
#done = self.done
pos = self.vehicle.state['pos']
dir = self.vehicle.state['dir']
self.refresh_dist(pos)
self.last_pos = pos
dist = self.road.dist_to_center(self.last_pos)
#velocity = self.velocity()
done = dist > MAX_DIST #or velocity > MAX_VELOCITY
reward = self._reward(done, dist)
return state, reward, done, {}
def velocity(self):
state = self.vehicle.state
velocity = np.linalg.norm(state["vel"])
return velocity * 3.6
def position(self):
return self.vehicle.state["pos"]
def refresh_dist(self, pos):
pos = np.array(pos)
last_pos = np.array(self.last_pos)
dist = np.linalg.norm(pos - last_pos)
self.dist_driven += dist
def close_connection(self):
self.bng.close()
def reset(self):
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.poll_sensors(self.vehicle)
self.dist_driven = 0
self.step = 0
self.done = False
current_pos = self.vehicle.state['pos']
closest_point = self.road.closest_waypoint(current_pos)
#closest_point = self.road.random_waypoint()
self.bng.teleport_vehicle(vehicle=self.vehicle, pos=closest_point.pos(), rot=closest_point.rot())
self.bng.pause()
# TODO delete
def wait(self):
from client.aiExchangeMessages_pb2 import SimStateResponse
return SimStateResponse.SimState.RUNNING
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()
class WCARaceGeometry(gym.Env):
sps = 50
rate = 5
front_dist = 800
front_step = 100
trail_dist = 104
trail_step = 13
starting_proj = 1710
max_damage = 100
def __init__(self, host='localhost', port=64256):
self.steps = WCARaceGeometry.sps // WCARaceGeometry.rate
self.host = host
self.port = port
self.action_space = self._action_space()
self.observation_space = self._observation_space()
self.episode_steps = 0
self.spine = None
self.l_edge = None
self.r_edge = None
self.polygon = None
front_factor = WCARaceGeometry.front_dist / WCARaceGeometry.front_step
trail_factor = WCARaceGeometry.trail_dist / WCARaceGeometry.trail_step
self.front = lambda step: +front_factor * step
self.trail = lambda step: -trail_factor * step
self.bng = BeamNGpy(self.host, self.port)
self.vehicle = Vehicle('racecar',
model='sunburst',
licence='BEAMNG',
colour='red',
partConfig='vehicles/sunburst/hillclimb.pc')
electrics = Electrics()
damage = Damage()
self.vehicle.attach_sensor('electrics', electrics)
self.vehicle.attach_sensor('damage', damage)
scenario = Scenario('west_coast_usa', 'wca_race_geometry_v0')
scenario.add_vehicle(self.vehicle,
pos=(394.5, -247.925, 145.25),
rot=(0, 0, 90))
scenario.make(self.bng)
self.bng.open(launch=True)
self.bng.set_deterministic()
self.bng.set_steps_per_second(WCARaceGeometry.sps)
self.bng.load_scenario(scenario)
self._build_racetrack()
self.observation = None
self.last_observation = None
self.last_spine_proj = None
self.bng.start_scenario()
self.bng.pause()
def __del__(self):
self.bng.close()
def _build_racetrack(self):
roads = self.bng.get_roads()
track = roads['race_ref']
l_vtx = []
s_vtx = []
r_vtx = []
for right, middle, left in track:
r_vtx.append(right)
s_vtx.append(middle)
l_vtx.append(left)
self.spine = LinearRing(s_vtx)
self.r_edge = LinearRing(r_vtx)
self.l_edge = LinearRing(l_vtx)
r_vtx = [v[0:2] for v in r_vtx]
l_vtx = [v[0:2] for v in l_vtx]
self.polygon = Polygon(l_vtx, holes=[r_vtx])
def _action_space(self):
action_lo = [-1., -1.]
action_hi = [+1., +1.]
return gym.spaces.Box(np.array(action_lo),
np.array(action_hi),
dtype=float)
def _observation_space(self):
# n vertices of left and right polylines ahead and behind, 3 floats per
# vtx
scope = WCARaceGeometry.trail_step + WCARaceGeometry.front_step
obs_lo = [
-np.inf,
] * scope * 3
obs_hi = [
np.inf,
] * scope * 3
obs_lo.extend([
-np.inf, # Distance to left edge
-np.inf, # Distance to right edge
-2 * np.pi, # Inclination
-2 * np.pi, # Angle
-2 * np.pi, # Vertical angle
-np.inf, # Spine speed
0, # RPM
-1, # Gear
0, # Throttle
0, # Brake
-1.0, # Steering
0, # Wheel speed
-np.inf, # Altitude
])
obs_hi.extend([
np.inf, # Distance to left edge
np.inf, # Distance to right edge
2 * np.pi, # Inclincation
2 * np.pi, # Angle
2 * np.pi, # Vertical angle
np.inf, # Spine speed
np.inf, # RPM
8, # Gear
1.0, # Throttle
1.0, # Brake
1.0, # Steering
np.inf, # Wheel speed
np.inf, # Altitude
])
return gym.spaces.Box(np.array(obs_lo), np.array(obs_hi), dtype=float)
def _make_commands(self, action):
brake = 0
throttle = action[1]
steering = action[0]
if throttle < 0:
brake = -throttle
throttle = 0
self.vehicle.control(steering=steering, throttle=throttle, brake=brake)
def _project_vehicle(self, pos):
r_proj = self.r_edge.project(pos)
r_proj = self.r_edge.interpolate(r_proj)
l_proj = self.l_edge.project(r_proj)
l_proj = self.l_edge.interpolate(l_proj)
s_proj = self.spine.project(r_proj)
s_proj = self.spine.interpolate(s_proj)
return l_proj, s_proj, r_proj
def _get_vehicle_angles(self, vehicle_pos, spine_seg):
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
spine_angle = np.arctan2(spine_end[1] - spine_beg[1],
spine_end[0] - spine_beg[0])
vehicle_angle = self.vehicle.state['dir'][0:2]
vehicle_angle = np.arctan2(vehicle_angle[1], vehicle_angle[0])
vehicle_angle = normalise_angle(vehicle_angle - spine_angle)
vehicle_angle -= np.pi
elevation = np.arctan2(spine_beg[2] - spine_end[2], spine_seg.length)
vehicle_elev = self.vehicle.state['dir']
vehicle_elev = np.arctan2(vehicle_elev[2],
np.linalg.norm(vehicle_elev))
return vehicle_angle, vehicle_elev, elevation
def _wrap_length(self, target):
length = self.spine.length
while target < 0:
target += length
while target > length:
target -= length
return target
def _gen_track_scope_loop(self, it, fn, base, s_scope, s_width):
for step in it:
distance = base + fn(step)
distance = self._wrap_length(distance)
s_proj = self.spine.interpolate(distance)
s_scope.append(s_proj)
l_proj = self.l_edge.project(s_proj)
l_proj = self.l_edge.interpolate(l_proj)
r_proj = self.r_edge.project(s_proj)
r_proj = self.r_edge.interpolate(r_proj)
width = l_proj.distance(r_proj)
s_width.append(width)
def _gen_track_scope(self, pos, spine_seg):
s_scope = []
s_width = []
base = self.spine.project(pos)
it = range(WCARaceGeometry.trail_step, 0, -1)
self._gen_track_scope_loop(it, self.trail, base, s_scope, s_width)
it = range(1)
self._gen_track_scope_loop(it, lambda x: x, base, s_scope, s_width)
it = range(WCARaceGeometry.front_step + 1)
self._gen_track_scope_loop(it, self.front, base, s_scope, s_width)
s_proj = self.spine.interpolate(base)
offset = (-s_proj.x, -s_proj.y, -s_proj.z)
s_line = LineString(s_scope)
s_line = affinity.translate(s_line, *offset)
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
direction = [spine_end[i] - spine_beg[i] for i in range(3)]
angle = np.arctan2(direction[1], direction[0]) + np.pi / 2
s_line = affinity.rotate(s_line,
-angle,
origin=(0, 0),
use_radians=True)
ret = list()
s_scope = s_line.coords
for idx in range(1, len(s_scope) - 1):
curvature = calculate_curvature(s_scope, idx)
inclination = calculate_inclination(s_scope, idx)
width = s_width[idx]
ret.append(curvature)
ret.append(inclination)
ret.append(width)
return ret
def _spine_project_vehicle(self, vehicle_pos):
proj = self.spine.project(vehicle_pos) - WCARaceGeometry.starting_proj
if proj < 0:
proj += self.spine.length
proj = self.spine.length - proj
return proj
def _get_spine_speed(self, vehicle_pos, vehicle_dir, spine_seg):
spine_beg = spine_seg.coords[0]
future_pos = Point(vehicle_pos.x + vehicle_dir[0],
vehicle_pos.y + vehicle_dir[1],
vehicle_pos.z + vehicle_dir[2])
spine_end = self.spine.project(future_pos)
spine_end = self.spine.interpolate(spine_end)
return spine_end.distance(Point(*spine_beg))
def _make_observation(self, sensors):
electrics = sensors['electrics']['values']
vehicle_dir = self.vehicle.state['dir']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
spine_beg = self.spine.project(vehicle_pos)
spine_end = spine_beg
spine_end += WCARaceGeometry.front_dist / WCARaceGeometry.front_step
spine_beg = self.spine.interpolate(spine_beg)
spine_end = self.spine.interpolate(spine_end)
spine_seg = LineString([spine_beg, spine_end])
spine_speed = self._get_spine_speed(vehicle_pos, vehicle_dir,
spine_seg)
l_dist = self.l_edge.distance(vehicle_pos)
r_dist = self.r_edge.distance(vehicle_pos)
angle, vangle, elevation = self._get_vehicle_angles(
vehicle_pos, spine_seg)
l_proj, s_proj, r_proj = self._project_vehicle(vehicle_pos)
s_scope = self._gen_track_scope(vehicle_pos, spine_seg)
obs = list()
obs.extend(s_scope)
obs.append(l_dist)
obs.append(r_dist)
obs.append(elevation)
obs.append(angle)
obs.append(vangle)
obs.append(spine_speed)
obs.append(electrics['rpm'])
obs.append(electrics['gearIndex'])
obs.append(electrics['throttle'])
obs.append(electrics['brake'])
obs.append(electrics['steering'])
obs.append(electrics['wheelspeed'])
obs.append(electrics['altitude'])
return np.array(obs)
def _compute_reward(self, sensors):
damage = sensors['damage']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
if damage['damage'] > WCARaceGeometry.max_damage:
return -1, True
if not self.polygon.contains(Point(vehicle_pos.x, vehicle_pos.y)):
return -1, True
score, done = -1, False
spine_proj = self._spine_project_vehicle(vehicle_pos)
if self.last_spine_proj is not None:
diff = spine_proj - self.last_spine_proj
if diff >= -0.10:
if diff < 0.5:
return -1, False
else:
score, done = diff / self.steps, False
elif np.abs(diff) > self.spine.length * 0.95:
score, done = 1, True
else:
score, done = -1, True
self.last_spine_proj = spine_proj
return score, done
def reset(self):
self.episode_steps = 0
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.restart_scenario()
self.bng.step(30)
self.bng.pause()
self.vehicle.set_shift_mode(3)
self.vehicle.control(gear=2)
sensors = self.bng.poll_sensors(self.vehicle)
self.observation = self._make_observation(sensors)
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
self.last_spine_proj = self._spine_project_vehicle(vehicle_pos)
return self.observation
def advance(self):
self.bng.step(self.steps, wait=False)
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
new_observation = self._make_observation(sensors)
return new_observation, sensors
def step(self, action):
action = [*np.clip(action, -1, 1), action[0], action[1]]
action = [float(v) for v in action]
self.episode_steps += 1
self._make_commands(action)
self.advance()
new_observation, sensors = self.observe()
if self.observation is not None:
self.last_observation = self.observation
self.observation = new_observation
score, done = self._compute_reward(sensors)
print((' A: {:5.2f} B: {:5.2f} '
' S: {:5.2f} T: {:5.2f} R: {:5.2f}').format(
action[2], action[3], action[0], action[1], score))
# if done:
# self.bng.step(WCARaceGeometry.sps * 1)
return self.observation, score, done, {}
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()
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 test_imu(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'vehicle_state_test')
vehicle = Vehicle('test_car', model='etk800')
imu_pos = IMU(pos=(0.73, 0.51, 0.8), debug=True)
imu_node = IMU(node=0, debug=True)
vehicle.attach_sensor('imu_pos', imu_pos)
vehicle.attach_sensor('imu_node', imu_node)
scenario.add_vehicle(vehicle, pos=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(20)
pax, pay, paz, pgx, pgy, pgz = [], [], [], [], [], []
for _ in range(30):
# Stand still, sample IMU
bng.step(60)
vehicle.poll_sensors()
pax.append(imu_pos.data['aX'])
pay.append(imu_pos.data['aY'])
paz.append(imu_pos.data['aZ'])
pgx.append(imu_pos.data['gX'])
pgy.append(imu_pos.data['gY'])
pgz.append(imu_pos.data['gZ'])
# Some slight movement is bound to happen since the engine is one and
# the vehicle isn't perfectly stable; hence no check for == 0
assert np.mean(pax) < 1
assert np.mean(pay) < 1
assert np.mean(paz) < 1
assert np.mean(pgx) < 1
assert np.mean(pgy) < 1
assert np.mean(pgz) < 1
pax, pay, paz, pgx, pgy, pgz = [], [], [], [], [], []
nax, nay, naz, ngx, ngy, ngz = [], [], [], [], [], []
for _ in range(30):
# Drive randomly, sample IMU
t = random.random() * 2 - 1
s = random.random() * 2 - 1
vehicle.control(throttle=t, steering=s)
bng.step(60)
vehicle.poll_sensors()
pax.append(imu_pos.data['aX'])
pay.append(imu_pos.data['aY'])
paz.append(imu_pos.data['aZ'])
pgx.append(imu_pos.data['gX'])
pgy.append(imu_pos.data['gY'])
pgz.append(imu_pos.data['gZ'])
nax.append(imu_node.data['aX'])
nay.append(imu_node.data['aY'])
naz.append(imu_node.data['aZ'])
ngx.append(imu_node.data['gX'])
ngy.append(imu_node.data['gY'])
ngz.append(imu_node.data['gZ'])
for arr in [pax, pay, paz, pgx, pgy, pgz]:
assert np.max(arr) > 0.01
assert np.min(arr) < -0.01
# See if IMU at different position ended up with different measurements
for parr, narr in zip([pax, pay, paz, pgx, pgy, pgz],
[nax, nay, naz, ngx, ngy, ngz]):
assert np.mean(parr) != np.mean(narr)
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()
class BeamNG_Cruise_Controller_Test():
def __init__(self, **kwargs):
keys = kwargs.keys()
if "test_controller" in keys:
self.controller = kwargs.get("test_controller")
else:
self.controller = PID_Controller_Test()
if "testing_times" in keys:
self.testing_times = kwargs.get("testing_times")
else:
self.testing_times = 1
if "test_name" in keys:
self.test_name = kwargs.get("test_name")
else:
self.test_name = "test"
if "targets" in keys:
self.targets = kwargs.get("targets")
else:
self.targets = [0]
def setup_bngServer(self):
# Instantiate BeamNGpy instance running the simulator from the given path,
if ('BNG_HOME' in os.environ):
self.bng = BeamNGpy('localhost',
64256,
home=os.environ['BNG_HOME'])
else:
print(
"WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)"
)
self.bng = BeamNGpy('localhost', 64256, home='')
def setup_BeamNG(self):
# Create a scenario in test map
scenario = Scenario('cruise-control', 'example')
# Create an ETK800 with the licence plate 'PID'
self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='PID')
electrics = Electrics()
self.vehicle.attach_sensor('electrics',
electrics) # Cars electric system
# Add it to our scenario at this position and rotation
scenario.add_vehicle(self.vehicle,
pos=(406.787, 1115.518, 0),
rot=(0, 0, 0))
# Place files defining our scenario for the simulator to read
scenario.make(self.bng)
# Launch BeamNG.research
self.bng.open()
# Load and start our scenario
self.bng.load_scenario(scenario)
self.bng.start_scenario()
def runTest(self, test_K_values, test_name):
for test_K_value in test_K_values:
KP_in, KI_in, KD_in = map(float, test_K_value)
if KP_in == 0 and KI_in == 0 and KD_in == 0:
config = Config()
controller = On_Off_Controller(config=config)
else:
config = Config(KP=KP_in, KI=KI_in, KD=KD_in)
controller = PID_Controller(config=config)
self.controller.controller = controller
self.test_name = str(test_name) + "_" + str(KP_in) + "_" + str(
KI_in) + "_" + str(KD_in)
self.runTestForPID()
def runTestForPID(self):
for speed in self.targets:
self.controller.setTarget(speed)
self.controller.setTime(float(30))
#self.runTestOfType("up_{0}_".format(speed), (406.787, 700.517, 0.214829), (0, 0, 0))
self.runTestOfType("straight_{0}_".format(speed),
(406.787, 715.517, 0.214829), (0, 0, 180))
#self.runTestOfType("down_{0}_".format(speed), (406.787, 304.896, 100.211), (0, 0, 180))
def runTestOfType(self, type, pos, rot):
i = 1
while i <= self.testing_times:
self.controller.newLogFile("../logs/" + self.test_name + "_" +
str(type) + "_" + str(i) + ".txt")
self.bng.restart_scenario()
self.bng.teleport_vehicle(self.vehicle, pos=pos, rot=rot)
self.run()
i += 1
def run(self):
self.controller.start()
while self.controller.ended == False:
start_time = datetime.datetime.now()
sensors = self.bng.poll_sensors(self.vehicle)
wheel_speed = sensors['electrics']['values']['wheelspeed']
value = self.controller.calculate_speed_with_logs(wheel_speed)
if value <= 0:
value = max(-1, value - 0.1) * -1
self.vehicle.control(brake=value)
self.vehicle.control(throttle=0)
else:
value = min(1, value)
self.vehicle.control(brake=0)
self.vehicle.control(throttle=value)
elapsed_time = datetime.datetime.now() - start_time
while (elapsed_time.total_seconds() * 1000) < 100:
elapsed_time = datetime.datetime.now() - start_time
print("Ending Test")
def close(self):
self.bng.close()
def 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 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()
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
sensors = bng.poll_sensors(
vehicleVictim
) # Polls the data of all sensors attached to the vehicle
#print(vehicleStriker.state['pos'])
if vehicleVictim.state['pos'][0] > 306 and vehicleVictim.state[
'pos'][1] > 39:
#print('free state')
vehicleVictim.control(throttle=0,
steering=0,
brake=0,
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
