def setup_beamng(traffic=2):
global sp, beamng_home, beamng_user
setup_logging()
beamng = BeamNGpy('localhost', 64256, home=beamng_home, user=beamng_user)
scenario = Scenario(
'west_coast_usa',
'lidar_tour',
description=
'Tour through highway with variable traffic vehicles gathering '
'Lidar data')
# setup vehicle
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='LIDAR',
color='Red')
vehicle = setup_sensors(vehicle)
# setup vehicle poses
lane = random.choice([1, 2, 3, 4])
ego_sp = spawn_point(sp, lane)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
lane = ego_sp['lane']
# add vehicles to scenario
# print("adding vehicle to scenario...")
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles(traffic)
ps = generate_vehicle_positions(ego_pos, ego_rot_quat, lane, tvs)
for i in range(len(tvs)):
print("adding vehicle {}...".format(i))
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
print("making scenario...")
scenario.make(beamng)
print("opening beamNG...")
bng = beamng.open(launch=True)
st = time.time()
print("loading scenario...")
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
#print("Bounding box: {}".format(vehicle.get_bbox()))
bng.hide_hud()
print("starting scenario...")
bng.start_scenario()
vehicle.ai_set_mode('traffic')
#"DecalRoad31765_8"
vehicle.ai_drive_in_lane(False)
vehicle.ai_set_aggression(2.0)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
bng.pause()
return vehicle, bng
electrics = Electrics()
damage = Damage()
vut.attach_sensor('electrics', electrics)
vut.attach_sensor('damage', damage)
scenario.add_vehicle(vut, pos=(-198.5, -164.189, 119.7), rot=(0, 0, -126.25))
car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue')
scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55))
scenario.make(beamng)
bng = beamng.open(launch=True)
bng.load_scenario(scenario)
bng.start_scenario()
vut.ai_set_mode('span')
vut.ai_drive_in_lane(True)
car_1.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}])
for _ in range(240):
sleep(0.1)
vut.update_vehicle()
sensors = bng.poll_sensors(vut)
dmg = sensors['damage']
# Below code snippet is generated form 'detect_obstacle_car' function for car_1
scenario.update()
dist_car_1 = np.linalg.norm(
np.array(vut.state['pos']) - np.array(car_1.state['pos']))
if dist_car_1 < 8:
def main():
global base_filename, training_dir, default_model
f = setup_dir(training_dir)
spawn_pt = spawn_point(default_scenario)
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost', 64256, home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle', model=default_model,
licence='RED', color='Red')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle, pos=spawn_pt['pos'], rot=None, rot_quat=spawn_pt['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
images = []
bng.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('span')
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_aggression(0.1)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# Send random inputs to vehice and advance the simulation 20 steps
imagecount = 0
wheelvel = [0.1, 0.1, 0.1]
with open(f, 'w') as datafile:
datafile.write('filename,timestamp,steering_input,throttle_input,brake_input,driveshaft,engine_load,fog_lights,fuel,'
'lowpressure,oil,oil_temperature,parkingbrake,rpm,water_temperature\n')
#for _ in range(1024):
for _ in range(32768):
#throttle = 1.0 #random.uniform(0.0, 1.0)
#steering = random.uniform(-1.0, 1.0)
#brake = random.choice([0, 0, 0, 1])
#vehicle.control(throttle=throttle)
# collect images
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
imagecount += 1
filename = "{}{}.bmp".format(base_filename, imagecount)
# collect ancillary data
datafile.write('{},{},{},{},{},{},{},{},{},{},{},{},{},{}\n'.format(filename,
str(round(sensors['timer']['time'], 2)),
sensors['electrics']['steering_input'],
sensors['electrics']['throttle_input'],
sensors['electrics']['brake_input'],
sensors['electrics']['driveshaft'],
sensors['electrics']['engine_load'],
sensors['electrics']['fog_lights'],
sensors['electrics']['fuel'],
sensors['electrics']['lowpressure'],
sensors['electrics']['oil'],
sensors['electrics']['oil_temperature'],
sensors['electrics']['parkingbrake'],
sensors['electrics']['rpm'],
sensors['electrics']['water_temperature'],))
# save the image
image.save(filename)
# step sim forward
bng.step(20)
print('{} seconds passed.'.format(str(round(sensors['timer']['time'], 2))))
# check for stopping condition
for i in range(len(wheelvel)-1):
wheelvel[i] = wheelvel[i+1]
wheelvel[2] = float(sensors['electrics']['wheelspeed'])
print('wheelvel = {}'.format(sum(wheelvel) / 3.0 ))
if sum(wheelvel) / 3.0 == 0.0:
print("avg wheelspeed is zero; exiting...")
bng.close()
break
def main() -> 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()
scenario_name = SCENARIO_BASE_NAME + str(i)
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()
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 run_scenario(traffic=2, run_number=0):
global sp
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario('west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='LIDAR',
color='Red')
vehicle = setup_sensors(vehicle)
lidar = setup_lidar('hood')
vehicle.attach_sensor('lidar', lidar)
ego_sp = spawn_point(sp)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles(traffic)
ps = generate_vehicle_positions(ego_pos, ego_rot_quat, tvs)
for i in range(len(tvs)):
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
#bng.set_deterministic()
#print("Bounding box: {}".format(vehicle.get_bbox()))
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
vehicle.ai_set_aggression(0.9)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
start = time.time()
end = time.time()
damage_prev = None
with open(
'H:/traffic_traces/traffic_lidar_data_{}traffic_run{}.csv'.
format(traffic, run_number), 'w') as f:
f.write(
"TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH,EXTERNAL_VEHICLES \n"
)
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
damage = sensors['damage']
v_state = vehicle.state
#print("vehicle_state = {}".format(v_state))
ai_state = vehicle.ai_get_state()
#print("ai_state = {}".format(ai_state))
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
if new_damage > 0:
print("new damage = {}".format(new_damage))
f.write("{},{},{},{},{},{},{}\n".format(
_ * 0.5, v_state['pos'], v_state['dir'], v_state['vel'],
points.tolist(), str(new_damage), traffic))
#print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
#print("Time passed since scenario begun: {}\n".format(end - start))
print()
if end - start >= 45:
bng.close()
bng.step(30, wait=False)
except Exception as e:
print(e)
finally:
bng.close()
def main() -> None:
# Read CommonRoad scenario
cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \
.open()
if cr_planning_problem_set:
for _, pp in cr_planning_problem_set.planning_problem_dict.items():
cr_planning_problem = pp
break # Read only the first one
else:
raise Exception(
"The given CommonRoad scenario does not define a planning problem."
)
# Setup BeamNG
bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path)
bng_scenario = Scenario(
bng_scenario_environment,
bng_scenario_name,
authors='Stefan Huber',
description='Simple visualization of the CommonRoad scenario ' +
cr_scenario_name)
# Add lane network
lanes = cr_scenario.lanelet_network.lanelets
for lane in lanes:
lane_nodes = []
for center in lane.center_vertices:
lane_nodes.append((center[0], center[1], 0,
4)) # FIXME Determine appropriate width
road = Road(cr_road_material)
road.nodes.extend(lane_nodes)
bng_scenario.add_road(road)
# Add ego vehicle
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Cornflowerblue')
ego_init_state = cr_planning_problem.initial_state
ego_init_state.position[0] = 82.8235
ego_init_state.position[1] = 31.5786
add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state,
etk800_z_offset)
obstacles_to_move = dict()
# Add truck
semi = Vehicle('truck', model='semi', color='Red')
semi_init_state = cr_scenario.obstacle_by_id(206).initial_state
add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state,
semi_z_offset)
obstacles_to_move[206] = semi
# Add truck trailer
tanker_init_state = copy(semi_init_state)
tanker_init_state.position += [6, 3]
add_vehicle_to_bng_scenario(
bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'),
tanker_init_state, tanker_z_offset)
# Add other traffic participant
opponent = Vehicle('opponent',
model='etk800',
licence='VS',
color='Cornflowerblue')
add_vehicle_to_bng_scenario(bng_scenario, opponent,
cr_scenario.obstacle_by_id(207).initial_state,
etk800_z_offset)
obstacles_to_move[207] = opponent
# Add static obstacle
obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape
obstacle_pos = obstacle_shape.center
obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset
obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset)
for w in range(3):
for h in range(3):
for d in range(2):
obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d),
model='haybale',
color='Red')
haybale_pos_diff = obstacle_pos \
+ pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \
+ pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad)
bng_scenario.add_vehicle(obstacle,
pos=(haybale_pos_diff[0],
haybale_pos_diff[1], h * 1),
rot=(0, 0, obstacle_rot_deg))
bng_scenario.make(bng)
# Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported)
prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment,
'scenarios', bng_scenario_name + '.prefab')
lines = open(prefab_path, 'r').readlines()
for i in range(len(lines)):
if 'overObjects' in lines[i]:
lines[i] = lines[i].replace('0', '1')
open(prefab_path, 'w').writelines(lines)
# Start simulation
bng.open(launch=True)
try:
bng.load_scenario(bng_scenario)
bng.start_scenario()
bng.pause()
bng.display_gui_message(
"The scenario is fully prepared and paused. You may like to position the camera first."
)
delay_to_resume = 15
input("Press enter to continue the simulation... You have " +
str(delay_to_resume) +
" seconds to switch back to the BeamNG window.")
sleep(delay_to_resume)
bng.resume()
for id, obstacle in obstacles_to_move.items():
obstacle.ai_drive_in_lane(False)
obstacle.ai_set_line(
generate_node_list(cr_scenario.obstacle_by_id(id)))
ego_vehicle.ai_drive_in_lane(False)
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit')
speed = 65 / 3.6
ego_vehicle.ai_set_line([{
'pos': ego_vehicle.state['pos']
}, {
'pos': (129.739, 56.907, etk800_z_offset),
'speed': speed
}, {
'pos': (139.4, 62.3211, etk800_z_offset),
'speed': speed
}, {
'pos': (149.442, 64.3655, etk800_z_offset),
'speed': speed
}, {
'pos': (150.168, 63.3065, etk800_z_offset),
'speed': speed
}, {
'pos': (188.495, 78.8517, etk800_z_offset),
'speed': speed
}])
# FIXME Setting the speed does not work as expected
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set')
input("Press enter to end simulation...")
finally:
bng.close()
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():
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
scenario = Scenario(
'west_coast_usa',
'lidar_tour',
description='Tour through the west coast gathering Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR')
vehicle = setup_sensors(vehicle)
lidar = setup_lidar('hood')
vehicle.attach_sensor('lidar', lidar)
spawn_pt = spawn_point()
ego_pos = spawn_pt['pos']
ego_rot_quat = spawn_pt['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles()
scenario.make(beamng)
bng = beamng.open(launch=True)
#try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_set_aggression(0.5)
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
bng.switch_vehicle(vehicle)
damage_prev = None
start = time.time()
end = time.time()
bng.pause()
print("Bounding box: {}".format(vehicle.get_bbox()))
with open('lidar_data.csv', 'w') as f:
f.write(
"TIMESTAMP,VEHICLE_POSITION,VEHICLE_ORIENTATION,VELOCITY,LIDAR,CRASH\n"
)
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
damage = sensors['damage']
v_state = vehicle.state
print("vehicle_state = {}".format(v_state))
print("vehicle_state[pos] = {}".format(v_state['pos']))
print("vehicle_state[dir] = {}".format(v_state['dir']))
print("Vehicle bounding box:{}".format(vehicle.get_bbox()))
#ai_state = vehicle.ai_get_state()
#print("ai_state = {}".format(ai_state))
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
#print("new damage = {}".format(new_damage))
print()
f.write("{},{},{},{},{},{}\n".format(_ * 0.5, v_state['pos'],
v_state['dir'],
v_state['vel'],
points.tolist(),
str(new_damage)))
#bng.step(30, wait=False)
bng.step(30)
#print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
#print("Time passed since scenario begun: {}\n".format(end - start))
#screenrecord()
if end - start >= 30:
#bng.close()
continue
class DataCollector:
def __init__(self):
self.collected_data = {"image_name": [], "steering": []}
self.scenario = Scenario(config.get("data_collecting.scenario_level"),
config.get("data_collecting.scenario_name"))
self.vehicle = Vehicle(
'ego_vehicle',
model=config.get("data_collecting.vehicle_model"),
licence='PYTHON')
self.bng = BeamNGpy(config.get("beamNG.host"),
int(config.get("beamNG.port")),
home=config.get("beamNG.home"))
def launch_beam_ng(self, mode="manual_mode"):
# Create an ETK800 with the licence plate 'PYTHON'
electrics = Electrics()
# attach to get steering angles
self.vehicle.attach_sensor('electrics', electrics)
# attach to get images from camera
self.vehicle.attach_sensor('front_cam', self.create_camera_sensor())
# Add it to our scenario at this position and rotation
# self.scenario.add_vehicle(self.vehicle)
self.scenario.add_vehicle(
self.vehicle,
pos=tuple(map(float,
config.get("data_collecting.pos").split(","))),
rot_quat=tuple(
map(float,
config.get("data_collecting.rot_quat").split(","))))
# 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()
if mode == "ai_mode":
# Make the vehicle's AI span the map
self.vehicle.ai_drive_in_lane(True)
self.vehicle.ai_set_mode('span')
def save_image_manually(self, cam_name='front_cam'):
img = self.bng.poll_sensors(self.vehicle)[cam_name]['colour']
steering = self.bng.poll_sensors(self.vehicle)['electrics']['steering']
self.save_data(img, steering)
@staticmethod
def create_camera_sensor(
pos=(-0.3, 2, 1.0), direction=(0, 1, 0), fov=100, res=None):
# Set up camera sensor
resolution = res
if res is None:
resolution = (int(config.get("data_collecting.image_width")),
int(config.get("data_collecting.image_height")))
pos = pos
direction = direction
fov = fov
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
return front_camera
def save_data(self, img, steering, i: str = "0"):
file_name = str(int(time.time())) + i + ".jpg"
try:
image_path = definitions.ROOT_DIR + config.get(
'data_collecting.data_path') + file_name
imageio.imwrite(image_path, np.asarray(img.convert('RGB')), "jpg")
self.collected_data["image_name"].append(file_name)
self.collected_data["steering"].append(steering)
except Exception as ex:
logger.info(f"Error while saving data -- {ex}")
raise Exception
def collect_data(self, number_of_images: int, mode="manual_mode"):
self.launch_beam_ng(mode)
logger.info("Start after 3 seconds...")
time.sleep(5)
logger.info(
f"Start collecting {config.get('data_collecting.number_of_images')} training images"
)
i = 0
exit_normally = True
try:
while i < number_of_images:
# image is training image and steering is label
img = self.bng.poll_sensors(
self.vehicle)['front_cam']['colour']
steering = self.bng.poll_sensors(
self.vehicle)['electrics']['steering']
logger.info(f"Saving data {i + 1} ...")
self.save_data(img, steering, str(i))
logger.info("Saved data successfully")
i = i + 1
time.sleep(int(config.get("data_collecting.sleep")))
except Exception as ex:
exit_normally = False
logger.info(f"Error while collecting data {ex}")
finally:
self.bng.close()
return exit_normally
def save_csv_data(self):
logger.info("Start saving csv file......")
csv_path = definitions.ROOT_DIR + config.get(
'data_collecting.csv_path')
df = pd.DataFrame(self.collected_data,
columns=['image_name', 'steering'])
if not os.path.isfile(csv_path):
df.to_csv(csv_path, index=False, header=True)
else: # else it exists so append without writing the header
df.to_csv(csv_path, index=False, mode='a', header=False)
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 main():
global sp
setup_logging()
beamng = BeamNGpy('localhost', 64256, home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario('west_coast_usa', 'lidar_tour',
description='Tour through the west coast gathering '
'Lidar data')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='LIDAR', color='Red')
lidar = Lidar()
lidar.__init__(offset=(0, 0, 1.7), direction=(-0.707, -0.707, 0), vres=32,
vangle=0.01, rps=2200000, hz=20, angle=360, max_dist=200,
visualized=True)
vehicle.attach_sensor('lidar', lidar)
ego_sp = spawn_point(sp)
ego_pos = ego_sp['pos']
ego_rot_quat = ego_sp['rot_quat']
scenario.add_vehicle(vehicle, pos=ego_pos, rot=None, rot_quat=ego_rot_quat)
tvs = traffic_vehicles()
#ps = [(-722.121, 103, 118.675), (-714.121, 101, 118.675), (-715.121, 105, 118.675), (-721.121, 100, 118.675)]
ps = [(ego_pos[0]-10, ego_pos[1]+2, ego_pos[2]),
(ego_pos[0]-10, ego_pos[1]-2, ego_pos[2]),
(ego_pos[0]-14, ego_pos[1]+5, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]),
(ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]),
(ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])]
ps1 = [(ego_pos[0] - 10, ego_pos[1] + 3, ego_pos[2]),
(ego_pos[0] - 10, ego_pos[1] - 1, ego_pos[2]),
(ego_pos[0] - 14, ego_pos[1] + 5, ego_pos[2]),
(ego_pos[0] + 5, ego_pos[1] - 1, ego_pos[2]),
(ego_pos[0] + 8, ego_pos[1] - 4, ego_pos[2]),
(ego_pos[0] + 10, ego_pos[1] + 9, ego_pos[2]),
(ego_pos[0] + 1, ego_pos[1] + 11, ego_pos[2])]
ps_oldorig = [(ego_pos[0]-7, ego_pos[1]+3, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+4, ego_pos[1]+5, ego_pos[2]),
(ego_pos[0]+5, ego_pos[1]-1, ego_pos[2]),
(ego_pos[0]+8, ego_pos[1]-4, ego_pos[2]),
(ego_pos[0]+10, ego_pos[1]+9, ego_pos[2]),
(ego_pos[0]+1, ego_pos[1]+11, ego_pos[2])]
for i in range(len(tvs)):
scenario.add_vehicle(tvs[i], pos=ps[i], rot_quat=ego_rot_quat)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.hide_hud()
bng.start_scenario()
vehicle.ai_set_mode('traffic')
vehicle.ai_drive_in_lane(True)
#vehicle.ai_set_speed(16, mode='limit')
#vehicle.ai_set_target('traffic')
vehicle.ai_set_aggression(0.5) #0.7)
bng.start_traffic(tvs)
bng.switch_vehicle(vehicle)
start = time.time()
end = time.time()
damage_prev = bng.poll_sensors(vehicle)
with open('lidar_data.csv', 'w') as f:
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
points = sensors['lidar']['points']
#print(points.tolist())
#print()
#f.write("{}\n".format(points.tolist()))
print("Time passed since last step: {}".format(time.time() - end))
end = time.time()
print("Time passed since scenario begun: {}\n".format(end - start))
if end - start >= 30:
bng.close()
bng.step(30, wait=False)
except Exception as e:
print(e)
finally:
bng.close()
def run_sim(nodes, ai_aggression, street_1: DecalRoad, street_2: DecalRoad):
waypoint_goal = BeamNGWaypoint('waypoint_goal',
get_node_coords(street_1.nodes[-1]))
maps.beamng_map.generated().write_items(street_1.to_json() + '\n' +
waypoint_goal.to_json() + '\n' +
street_2.to_json())
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('tig', 'tigscenario')
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
sim_data_collector = TrainingDataCollectorAndWriter(
vehicle, beamng, street_1)
scenario.add_vehicle(vehicle,
pos=get_node_coords(street_1.nodes[0]),
rot=get_rotation(street_1))
scenario.make(beamng)
beamng.open()
beamng.set_deterministic()
beamng.load_scenario(scenario)
beamng.pause()
beamng.start_scenario()
vehicle.ai_set_aggression(ai_aggression)
vehicle.ai_drive_in_lane(True)
# vehicle.ai_set_speed(25.0 / 4)
vehicle.ai_set_waypoint(waypoint_goal.name)
# vehicle.ai_set_mode("manual")
# sleep(5)
steps = 5
print(nodes)
print(beamng.get_road_edges("street_1"))
def start():
for idx in range(1000):
if (idx * 0.05 * steps) > 3.:
sim_data_collector.collect_and_write_current_data()
dist = distance(sim_data_collector.last_state.pos,
waypoint_goal.position)
if dist < 15.0:
beamng.resume()
break
# one step is 0.05 seconds (5/100)
beamng.step(steps)
try:
start()
finally:
beamng.close()
def run_scenario_ai_version(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0],
parts_config=None):
global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec
global prev_error
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean',
user="******")
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='AI',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = get_spawn_point(default_scenario, default_spawnpoint)
# scenario.add_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# create vehicle to be chased
chase_vehicle = Vehicle('chase_vehicle',
model='miramar',
licence='CHASEE',
color='Red')
bng.spawn_vehicle(chase_vehicle,
pos=(469.784, 346.391, 144.982),
rot=None,
rot_quat=(-0.0037852677050978, -0.0031219546217471,
-0.78478640317917, 0.61974692344666))
bng.spawn_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'],
partConfig=parts_config)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
#bng.resume()
# perturb vehicle
print("vehicle position before deflation via beamstate:{}".format(
vehicle.get_object_position()))
print("vehicle position before deflation via vehicle state:{}".format(
vehicle.state))
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
plt.imshow(image)
plt.pause(0.01)
vehicle.deflate_tires(deflation_pattern)
bng.step(steps_per_sec * 6)
vehicle.update_vehicle()
# print("vehicle position after deflation via beamstate:{}".format(vehicle.get_object_position()))
# print("vehicle position after deflation via vehicle state:{}".format(vehicle.state))
pitch = vehicle.state['pitch'][0]
roll = vehicle.state['roll'][0]
z = vehicle.state['pos'][2]
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
plt.imshow(image)
plt.pause(0.01)
bng.resume()
vehicle.ai_set_mode('chase')
vehicle.ai_set_target('chase_vehicle')
vehicle.ai_drive_in_lane(True)
damage_prev = None
runtime = 0.0
traj = []
kphs = []
for _ in range(650):
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
damage = bng.poll_sensors(vehicle)['damage']
wheelspeed = bng.poll_sensors(vehicle)['electrics']['wheelspeed']
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
runtime = bng.poll_sensors(vehicle)['timer']['time']
vehicle.update_vehicle()
traj.append(vehicle.state['pos'])
kphs.append(ms_to_kph(wheelspeed))
if new_damage > 0.0:
break
bng.step(5)
bng.close()
results = {
'runtime': round(runtime, 3),
'damage': damage,
'kphs': kphs,
'traj': traj,
'pitch': round(pitch, 3),
'roll': round(roll, 3),
"z": round(z, 3),
'final_img': image
}
return results
