def main():
bng_home = os.environ['BEAMNG_HOME']
road = TrainingRoad(ASFAULT_PREFAB)
road.calculate_road_line(back=True)
bng = BeamNGpy('localhost', 64256, home=bng_home)
scenario = Scenario('smallgrid', 'train')
scenario.add_road(road.asphalt)
scenario.add_road(road.mid_line)
scenario.add_road(road.left_line)
scenario.add_road(road.right_line)
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
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)
vehicle.attach_sensor("front_camera", front_camera)
# Add it to our scenario at this position and rotation
spawn_point = road.spawn_point()
scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
# Place files defining our scenario for the simulator to read
scenario.make(bng)
prefab_path = scenario.get_prefab_path()
update_prefab(prefab_path)
bng.open()
bng.set_nondeterministic()
bng.set_steps_per_second(60)
# Load and start our scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('span')
vehicle.ai_set_speed(5)
vehicle.ai_set_line([{
'pos': node.pos(),
'speed': 10
} for node in road.road_line])
number_of_images = 0
while number_of_images < 9000:
bng.poll_sensors(vehicle)
number_of_images += 1
#print(number_of_images)
bng.step(1)
sensors = bng.poll_sensors(vehicle)
image = sensors['front_camera']['colour'].convert('RGB')
image = np.array(image)
image = image[:, :, ::-1]
dist = road.dist_to_center(vehicle.state['pos'])
cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image)
bng.close()
import mmap
scenario = Scenario('drivebuild', scenario_name)
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
# Has to be the correct folder.
destination_path = 'C:\\Users\\fraun\\Documents\\BeamNG\\levels\\drivebuild\\scenarios'
scenario.path = Path(destination_path)
prefab_path = Path(destination_path).joinpath(Path(scenario_name + ".prefab"))
cov_collector = CoverageCollector(vehicle, bng, prefab_path)
bng.open()
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_speed(20, "limit")
vehicle.ai_set_mode("span")
# Data collecting loop. Collects every three steps data.
counter = 0
last_counter = 0
steps = 2
while counter < 300:
if counter > last_counter + steps:
cov_collector.collect()
counter += 1
if counter % 10 == 0:
print(counter)
# bng.stop_scenario() # not working?
# bng.restart_scenario()
bng.close()
def main() -> None:
from beamngpy import BeamNGpy, Scenario, Vehicle, beamngcommon
from beamngpy.sensors import Damage, Camera, Electrics
from time import sleep
bng = BeamNGpy("localhost", 64523)
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Red')
scenario = Scenario(
"west_coast_usa",
"DamageSensorTest",
authors="Stefan Huber",
description="Test usage and check output of the damage sensor")
direction = (0, 1, 0)
fov = 120
resolution = (320, 160)
y, z = (1.7, 1.0)
cam_center = Camera((-0.3, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
cam_left = Camera((-1.3, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
cam_right = Camera((0.4, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
#cameras_array = [camera_center, camera_left, camera_right]
ego_vehicle.attach_sensor('cam_center', cam_center)
ego_vehicle.attach_sensor('cam_left', cam_left)
ego_vehicle.attach_sensor('cam_right', cam_right)
ego_vehicle.attach_sensor("electrics", Electrics())
#electrics_data = Electrics.encode_vehicle_request(Electrics)
#print(electrics_data)
#scenario.add_vehicle(ego_vehicle,pos=(-717.121, 101, 118.675), rot=(0, 0, 45))
scenario.add_vehicle(ego_vehicle,
pos=(-725.7100219726563, 554.3270263671875,
121.0999984741211),
rot=(0, 0, 45))
scenario.make(bng)
bng.open(launch=True)
def save_image(data, ind, cam_name):
img = data[cam_name]['colour'].convert('RGB')
file_name = str(ind) + "_" + cam_name + ".jpg"
filepath = os.path.join('C:/Users/HP/Documents/Data_Log/10_lap_log',
file_name)
img.save(filepath)
return file_name
def save(data, ind):
cam_left_file_name = save_image(data, ind, 'cam_left')
cam_right_file_name = save_image(data, ind, 'cam_right')
cam_center_file_name = save_image(data, ind, 'cam_center')
steering_in_value = data['electrics']['values']['steering_input']
steering_value = data['electrics']['values']['steering']
throttle_in_value = data['electrics']['values']['throttle_input']
throttle_value = data['electrics']['values']['throttle']
clutch_value = data['electrics']['values']['clutch']
clutch_in_value = data['electrics']['values']['clutch_input']
wheel_speed_value = data['electrics']['values']['wheelspeed']
rpmspin_value = data['electrics']['values']['rpmspin']
#add here
print(cam_left_file_name, cam_right_file_name, cam_center_file_name,
steering_in_value, steering_value, throttle_in_value,
throttle_value, clutch_in_value, clutch_value, rpmspin_value,
wheel_speed_value)
print()
temp_df = temp_dataframe()
temp_df.loc[0] = [
cam_left_file_name, cam_right_file_name, cam_center_file_name,
steering_in_value, steering_value, throttle_in_value,
throttle_value, clutch_in_value, clutch_value, wheel_speed_value,
rpmspin_value
] # modify
#append with existing and save
df_orig = pd.read_csv('my_data_lap3.csv')
df_orig = pd.concat([df_orig, temp_df])
df_orig.to_csv('my_data_lap3.esc', index=False)
del [[df_orig, temp_df]]
def temp_dataframe():
df1 = pd.DataFrame(columns=[
'cam_left', 'cam_right', 'cam_centre', 'steering_in_value',
'steering_value', 'throttle_in_value', 'throttle_value',
'clutch_in_value', 'clutch_value', 'wheelspeed_value',
'rpmspin_value'
]) # modify
return df1
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.start_scenario()
bng.hide_hud()
ego_vehicle.ai_drive_in_lane(lane=True)
ego_vehicle.ai_set_mode('span')
ego_vehicle.ai_set_speed(10)
ind = 0
while True:
sensor_data = bng.poll_sensors(ego_vehicle)
save(sensor_data, ind)
ind += 1
finally:
bng.close()
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()
def main(MAX_SPEED):
setup_logging()
# Gains to port TORCS actuators to BeamNG
# steering_gain = translate_steering()
acc_gain = 0.5 # 0.4
brake_gain = 1.0
# BeamNG images are too bright for DeepDrive
brightness = 0.4
# Set up first vehicle
# ! A vehicle with the name you specify here has to exist in the scenario
vehicle = Vehicle('egovehicle')
# Set up sensors
resolution = (280, 210)
# Original Settings
#pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below
# 0.4 is inside
pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below
direction = (0, 1, 0)
# direction = (180, 0, 180)
# FOV 60, MAX_SPEED 100, 20 (3) Hz fails
# FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
# FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
# FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
# FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
# FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
fov = 60
# MAX_SPEED = 70
MAX_FPS = 60
SIMULATION_STEP = 6
# Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
#
# WORKING SETTINGS: 20 Freq, 90 FOV.
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
# Setup the SHM with DeepDrive
# Create shared memory object
Memory = sd.CSharedMemory(TargetResolution=[280, 210])
# Enable Pause-Mode
Memory.setSyncMode(True)
Memory.Data.Game.UniqueRaceID = int(time.time())
print("Setting Race ID at ", Memory.Data.Game.UniqueRaceID)
# Setting Max_Speed for the Vehicle.
# TODO What's this? Maybe some hacky way to pass a parameter which is not supposed to be there...
Memory.Data.Game.UniqueTrackID = int(MAX_SPEED)
# Speed is KM/H
print("Setting speed at ", Memory.Data.Game.UniqueTrackID)
# Default for AsFault
Memory.Data.Game.Lanes = 1
# By default the AI is in charge
Memory.Data.Control.IsControlling = 1
deep_drive_engaged = True
STATE = "NORMAL"
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset ! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
# Not sure this will have any effect
Memory.indicateWrite()
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset Again! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Connect to running beamng
beamng = BeamNGpy(
'localhost',
64256,
home='C://Users//Alessio//BeamNG.research_unlimited//trunk')
bng = beamng.open(launch=False)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(MAX_FPS) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# Road interface is not available in BeamNG.research yet
# Get the road map from the level
# roads = bng.get_roads()
# # find the actual road. Dividers lane markings are all represented as roads
# theRoad = None
# for road in enumerate(roads):
# # ((left, centre, right), (left, centre, right), ...)
# # Compute the width of the road
# left = Point(road[0][0])
# right = Point(road[0][1])
# distance = left.distance( right )
# if distance < 2.0:
# continue
# else:
# theRoad = road;
# break
#
# if theRoad is None:
# print("WARNING Cannot find the main road of the map")
while True:
# Resume the execution
# 6 steps correspond to 10 FPS with a resolution of 60FPS
# 5 steps 12 FPS
# 3 steps correspond to 20 FPS
bng.step(SIMULATION_STEP)
# Retrieve sensor data and show the camera data.
sensors = bng.poll_sensors(vehicle)
# print("vehicle.state", vehicle.state)
# # TODO: Is there a way to query for the speed directly ?
speed = math.sqrt(
vehicle.state['vel'][0] * vehicle.state['vel'][0] +
vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Speed is M/S ?
# print("Speed from BeamNG is: ", speed, speed*3.6)
imageData = preprocess(sensors['front_cam']['colour'], brightness)
Height, Width = imageData.shape[:2]
# print("Image size ", Width, Height)
# TODO Size of image should be right since the beginning
Memory.write(Width, Height, imageData, speed)
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Wait for the control commands to send to the vehicle
# This includes a sleep and will be unlocked by writing data to it
Memory.waitOnRead()
# TODO Assumption. As long as the car is out of the road for too long this value stays up
if Memory.Data.Control.Breaking == 3.0:
if STATE != "DISABLED":
print(
"Abnormal situation detected. Disengage DeepDrive and enable BeamNG AI"
)
vehicle.ai_set_mode("manual")
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_speed(MAX_SPEED)
vehicle.ai_set_waypoint("waypoint_goal")
deep_drive_engaged = False
STATE = "DISABLED"
elif Memory.Data.Control.Breaking == 2.0:
if STATE != "GRACE":
print("Grace period. Deep Driving still disengaged")
vehicle.ai_set_mode("manual")
vehicle.ai_set_waypoint("waypoint_goal")
# vehicle.ai_drive_in_lane(True)
STATE = "GRACE"
else:
if STATE != "NORMAL":
print("DeepDrive re-enabled")
# Disable BeamNG AI driver
vehicle.ai_set_mode("disabled")
deep_drive_engaged = True
STATE = "NORMAL"
# print("State ", STATE, "Memory ",Memory.Data.Control.Breaking )
if STATE == "NORMAL":
vehicle.ai_set_mode("disabled")
# Get commands from SHM
# Apply Control - not sure cutting at 3 digit makes a difference
steering = round(
translate_steering(Memory.Data.Control.Steering), 3)
throttle = round(Memory.Data.Control.Accelerating * acc_gain,
3)
brake = round(Memory.Data.Control.Breaking * brake_gain, 3)
# Apply commands
vehicle.control(throttle=throttle,
steering=steering,
brake=brake)
#
# print("Suggested Driving Actions: ")
# print(" Steer: ", steering)
# print(" Accel: ", throttle)
# print(" Brake: ", brake)
finally:
bng.close()
def main():
global base_filename, training_dir, default_model, setpoint
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario)
random.seed(1703)
setup_logging()
#beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean',
user='******')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=default_model,
licence='RED',
color='Red')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle,
pos=spawn_pt['pos'],
rot=None,
rot_quat=spawn_pt['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
images = []
#bng.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_aggression(0.3)
vehicle.ai_set_speed(11.5, mode='set')
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
# Send random inputs to vehice and advance the simulation 20 steps
imagecount = 0
wheelvel = [0.1, 0.1, 0.1]
vels = []
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'
)
#for _ in range(1024):
for _ in range(32768):
#throttle = 1.0 #random.uniform(0.0, 1.0)
#steering = random.uniform(-1.0, 1.0)
#brake = random.choice([0, 0, 0, 1])
#vehicle.control(throttle=throttle)
# collect images
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
imagecount += 1
filename = "{}{}.bmp".format(base_filename, imagecount)
# collect ancillary data
datafile.write(
'{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(
filename, str(
round(sensors['timer']['time'],
2)), sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],
sensors['electrics']['wheelspeed']))
# save the image
image.save(filename)
vels.append(sensors['electrics']['wheelspeed'])
# step sim forward
bng.step(20)
print('{} seconds passed.'.format(
str(round(sensors['timer']['time'], 2))))
# check for stopping condition
for i in range(len(wheelvel) - 1):
wheelvel[i] = wheelvel[i + 1]
wheelvel[2] = float(sensors['electrics']['wheelspeed'])
#print("wheelspeed:{}".format(sensors['electrics']['wheelspeed']))
#print('wheelvel = {}'.format(sum(wheelvel) / 3.0 ))
print("average velocity: {}".format(sum(vels) / float(len(vels))))
if sum(wheelvel) / 3.0 == 0.0:
print("avg wheelspeed is zero; exiting...")
bng.close()
break
def getOob(oobList, test, port):
# Gains to port TORCS actuators to BeamNG
# steering_gain = translate_steering()
acc_gain = 0.5 # 0.4
brake_gain = 1.0
# BeamNG images are too bright for DeepDrive
brightness = 0.4
# Set up first vehicle
# ! A vehicle with the name you specify here has to exist in the scenario
vehicle = Vehicle('egovehicle')
scenario = Scenario('asfault', 'asfault', True)
# Set up sensors
resolution = (280, 210)
# Original Settings
#pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below
# 0.4 is inside
pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below
direction = (0, 1, 0)
# direction = (180, 0, 180)
# FOV 60, MAX_SPEED 100, 20 (3) Hz fails
# FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
# FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
# FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
# FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
# FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
fov = 60
# MAX_SPEED = 70
MAX_FPS = 60
SIMULATION_STEP = 6
# Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
#
# WORKING SETTINGS: 20 Freq, 90 FOV.
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
with open(pathToScenarioBeamng + '/asfault.json', 'r') as f:
jsonScenario = json.load(f)
with open(pathToScenarioBeamng + '/asfault.lua', 'r') as f:
luaScenario = f.readlines()
with open(pathToScenarioBeamng + '/asfault.prefab', 'r') as f:
prefabScenario = f.readlines()
with open(pathToScenarioDocuments + '/asfault.json', 'w') as outfile:
json.dump(jsonScenario, outfile)
with open(pathToScenarioDocuments + '/asfault.lua', 'w') as outfile:
outfile.writelines(luaScenario)
with open(pathToScenarioDocuments + '/asfault.prefab', 'w') as outfile:
outfile.writelines(prefabScenario)
deep_drive_engaged = True
STATE = "NORMAL"
notFinished = True
portCount = 1
multiplier = 2
turtleMode = False
turtleSpeed = 10
offRoad = False
oobSegKey = None
oobSearch = False
print(oobList)
speedList = []
currentSpeed = oobList[0][0][0]
for speed in oobList[0][0]:
speedList.append(speed)
if speed > currentSpeed:
currentSpeed = speed
currentSpeed = currentSpeed * 3.6
waypointList = []
resultList = []
oobWaypoint = []
newOobWaypointList = []
for key in oobList[0][1]:
waypointList.append(key)
print(waypointList)
for waypoint in waypointList:
for key in waypointList:
print(key)
for k in key:
oobWaypoint.append(k)
resultList.append(k)
while notFinished:
# Connect to running beamng
setup_logging()
beamng = BeamNGpy('localhost', port)
bng = beamng.open(launch=True)
scenario.make(bng)
bng.load_scenario(scenario)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle, port + portCount)
bng.start_scenario()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
testDic = RoadTest.to_dict(test)
parentage = testDic['network']['parentage']
for parent in parentage:
roadKeys = []
for keys in parentage:
for key in keys:
roadKeys.append(key)
nodesDict = testDic['network']['nodes']
nodes = []
for node in nodesDict:
nodes.append(node)
vehicle.update_vehicle()
oobCount = 0
inOob = False
vehicle.ai_set_speed(70)
if oobSearch:
if turtleMode:
currentSpeed = turtleSpeed
else:
currentSpeed = currentSpeed - 10
print(currentSpeed)
running = True
while running:
# Resume the execution
# 6 steps correspond to 10 FPS with a resolution of 60FPS
# 5 steps 12 FPS
# 3 steps correspond to 20 FPS
#bng.step(SIMULATION_STEP)
# # TODO: Is there a way to query for the speed directly ?
#speed = math.sqrt(vehicle.state['vel'][0] * vehicle.state['vel'][0] + vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Speed is M/S ?
# print("Speed from BeamNG is: ", speed, speed*3.6)
#vehicle.ai_drive_in_lane(True)
vehicle.update_vehicle()
tup = get_segment(test, vehicle.state)
currentSegmentDict = None
currentSegKey = None
if tup is not None:
currentSegmentDict = NetworkNode.to_dict(tup)
currentSegKey = currentSegmentDict['key']
if offRoad:
if oobSegKey in resultList:
index = resultList.index(oobSegKey)
lostSeg = resultList.pop(index)
print('resList without seg', resultList)
print(lostSeg)
if oobSegKey not in newOobWaypointList:
newOobWaypointList.append(oobSegKey)
print('new OOB list', newOobWaypointList)
offRoad = False
if inOob and off_track(test, vehicle.state):
print('off track')
offRoad = True
oobCount = oobCount + 1
if currentSegKey in oobWaypoint:
vehicle.ai_drive_in_lane(True)
inOob = True
oobSegKey = currentSegKey
vehicle.ai_set_speed(currentSpeed)
else:
inOob = False
vehicle.ai_set_speed(70)
vehicle.ai_set_waypoint('waypoint_goal')
vehicle.ai_drive_in_lane(True)
if goal_reached(test, vehicle.state):
print(turtleMode)
for res in resultList:
segOobDict = testDic['execution']['seg_oob_count']
segOobDict[res] = currentSpeed / 3.6
if oobCount == 0:
testDic['execution']['oobs'] = 0
print('waypointlist', newOobWaypointList)
print('resultlist', resultList)
test = RoadTest.from_dict(testDic)
return test
else:
running = False
portCount = portCount + 1
multiplier = multiplier + 1
oobSearch = True
oobWaypoint = newOobWaypointList
newOobWaypointList = []
if currentSpeed < 20:
print(currentSpeed)
print('NewoobWay: ', newOobWaypointList)
print('resultlist: ', resultList)
if turtleMode:
for res in oobWaypoint:
print(res)
segOobDict = testDic['execution'][
'seg_oob_count']
segOobDict[res] = 12
testDic['execution']['oobs'] = 0
test = RoadTest.from_dict(testDic)
print(segOobDict)
print(resultList)
running = False
oobCount = 0
return test
else:
turtleMode = True
print('mode on')
bng.close()
finally:
if oobCount == 0:
bng.close()
else:
print('restart')
def main():
"""
Generate a bunch of images by driving along a predefined sequence of points, while capturing camera images
to JPG files.
:return: (None)
"""
bng_home = os.environ['BEAMNG_HOME']
road = TrainingRoad(ASFAULT_PREFAB)
road.calculate_road_line(back=True)
bng = BeamNGpy('localhost', 64256, home=bng_home)
scenario = Scenario('smallgrid', 'train')
# Asphalt and lines are actually considered as differently colored roads by beamngpy.
scenario.add_road(road.asphalt)
scenario.add_road(road.mid_line)
scenario.add_road(road.left_line)
scenario.add_road(road.right_line)
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
# Create a dash cam that is somewhat down-sloped.
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)
vehicle.attach_sensor("front_camera", front_camera)
# Get a spawn point and initial rotation of the vehicle.
spawn_point = road.spawn_point()
scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
# Place files defining our scenario for the simulator to read.
scenario.make(bng)
prefab_path = scenario.get_prefab_path()
update_prefab(prefab_path)
bng.open()
bng.set_nondeterministic()
bng.set_steps_per_second(60)
# Load and start our scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('span')
vehicle.ai_set_speed(5)
vehicle.ai_set_line([{
'pos': node.pos(),
'speed': 10
} for node in road.road_line])
number_of_images = 0
while number_of_images < NUMBER_OF_IMAGES:
bng.poll_sensors(vehicle)
number_of_images += 1
bng.step(1)
sensors = bng.poll_sensors(vehicle)
image = sensors['front_camera']['colour'].convert('RGB')
image = np.array(image)
image = image[:, :, ::-1]
dist = road.dist_to_center(vehicle.state['pos'])
cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image)
bng.close()
def testrun(speed=11, risk=0.6):
global base_filename, training_dir, default_model, setpoint
global spawnpoint
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario, spawnpoint)
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean',
user='******')
scenario = Scenario(default_scenario, 'research_test')
# add barriers and cars to get the ego vehicle to avoid the barriers
add_barriers(scenario)
# add_barrier_cars(scenario)
vehicle = Vehicle('ego_vehicle',
model=default_model,
licence='RED',
color='White')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle,
pos=spawn_pt['pos'],
rot=None,
rot_quat=spawn_pt['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
#bng.hide_hud()
bng.set_nondeterministic() # Set simulator to be deterministic
bng.set_steps_per_second(36) #
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
bng.switch_vehicle(vehicle)
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(False)
vehicle.ai_set_speed(speed, mode='set')
vehicle.ai_set_aggression(risk)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# bng.resume()
start_time = time.time()
# Send random inputs to vehicle and advance the simulation 20 steps
imagecount = 0
with open(f, 'w') as datafile:
datafile.write(
'filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature,wheelspeed\n'
)
reached_start = False
vels = []
vel_dict = {}
while imagecount < 10000:
# collect images
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
full_filename = "{}{}{}.jpg".format(training_dir, base_filename,
imagecount)
qualified_filename = "{}{}.jpg".format(base_filename, imagecount)
# collect ancillary data
datafile.write(
'{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(
qualified_filename, str(
round(sensors['timer']['time'],
2)), sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],
sensors['electrics']['wheelspeed']))
if sensors['timer']['time'] > 10:
kph = sensors['electrics']['wheelspeed'] * 3.6
vels.append(kph)
vel_dict[sensors['timer']['time']] = kph
if distance(spawn_pt['pos'], vehicle.state['pos']
) < 5 and sensors['timer']['time'] > 10:
reached_start = True
break
if sensors['damage']['damage'] > 0:
print("CRASHED; QUITTING")
break
# save the image
image.save(full_filename)
imagecount += 1
# plt.title("90 degrees FOV")
# plt.imshow(image)
# plt.pause(0.01)
# plt.title("120 degrees FOV")
# plt.imshow(sensors['back_cam']['colour'].convert('RGB'))
# plt.pause(0.01)
# step sim forward
bng.step(1, wait=True)
wheelspeed_avg = round((sum(vels) / float(len(vels))), 3)
wheelspeed_var = round(np.var(vels), 3)
return_str = ''
if sensors['damage']['damage'] > 0:
# print("QUIT DUE TO CRASH VALUE {}".format(sensors['damage']['damage']))
return_str = "QUIT DUE TO CRASH VALUE {}".format(
sensors['damage']['damage'])
print(return_str)
maxtime = from_val_get_key(vel_dict, max(vels))
mintime = from_val_get_key(vel_dict, min(vels))
info = "IMAGE COUNT:{}\nSIM TIME:{} WALLCLOCK TIME:{}\nWHEELSPEED AVG:{} VAR:{} \nMAX:{} @ timestep {} MIN:{} @ timestep {} ".format(
imagecount, str(round(sensors['timer']['time'], 2)),
time.time() - start_time, wheelspeed_avg, wheelspeed_var,
round(max(vels), 3), round(maxtime, 3), round(min(vels), 3),
round(mintime, 3))
print("SPEED:{} RISK:{}".format(speed, risk))
print(info)
return_str = "{}\n{}".format(return_str, info)
bng.close()
return return_str
def 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
