def run_scenario_ai_version(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0],
parts_config='vehicles/hopper/custom.pc'):
global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec, prev_error
random.seed(1703)
setup_logging()
home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.research.v1.7.0.1untouched/BeamNG.research.v1.7.0.1' #'H:/BeamNG.tech.v0.21.3.0' #
beamng = BeamNGpy('localhost', 64256,
home=home) #, user='******')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='AI',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario, default_spawnpoint)
scenario.add_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# create vehicle to be chased
# chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red')
# bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None,
# rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666))
# bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
ai_line, bng = create_ai_line_from_road(spawn, bng)
# ai_line, bng = create_ai_line_from_centerline(bng)
# ai_line, bng = create_ai_line(bng)
vehicle.ai_set_script(ai_line, cling=True)
pitch = vehicle.state['pitch'][0]
roll = vehicle.state['roll'][0]
z = vehicle.state['pos'][2]
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
# bng.resume()
# vehicle.ai_set_mode('chase')
# vehicle.ai_set_target('chase_vehicle')
# vehicle.ai_set_mode("traffic")
# vehicle.ai_set_speed(12, mode='set')
# vehicle.ai_drive_in_lane(True)
damage_prev = None
runtime = 0.0
traj = []
kphs = []
# with open("ai_lap_data.txt", 'w') as f:
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
damage = sensors['damage']
wheelspeed = sensors['electrics']['wheelspeed']
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
runtime = sensors['timer']['time']
vehicle.update_vehicle()
traj.append(vehicle.state['pos'])
# f.write("{}\n".format(vehicle.state['pos']))
kphs.append(ms_to_kph(wheelspeed))
if new_damage > 0.0:
break
# if distance(spawn['pos'], vehicle.state['pos']) < 3 and sensors['timer']['time'] > 90:
# reached_start = True
# plt.imshow(image)
# plt.show()
# break
bng.step(1)
bng.close()
plot_trajectory(traj, "AI Lap")
results = {
'runtime': round(runtime, 3),
'damage': damage,
'kphs': kphs,
'traj': traj,
'pitch': round(pitch, 3),
'roll': round(roll, 3),
"z": round(z, 3),
'final_img': image
}
return results
def run_scenario(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0],
parts_config=None):
global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec
global integral, prev_error
integral = 0.0
prev_error = 0.0
# setup DNN model + weights
# m = Model()
# model = m.define_model_BeamNG("BeamNGmodel-racetrack.h5")
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='H:/BeamNG.research.v1.7.0.1clean')
scenario = Scenario(default_scenario, 'research_test')
# unperturbed_vehicle = Vehicle('unperturbed_vehicle', model=vehicle_model,
# licence='SAFE', color='Red')
# unperturbed_vehicle = setup_sensors(unperturbed_vehicle)
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='EGO',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = get_spawn_point(default_scenario, default_spawnpoint)
scenario.add_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'])
temp = copy.deepcopy(spawn['pos'])
temp = [temp[0] + lanewidth, temp[1] + lanewidth, temp[2]]
# scenario.add_vehicle(unperturbed_vehicle, pos=temp, rot=None, rot_quat=spawn['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
bng = beamng.open(launch=True)
#bng.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_sec)
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
#bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
# perturb vehicle
vehicle.deflate_tires(deflation_pattern)
bng.step(steps_per_sec * 6)
vehicle.update_vehicle()
damage = 0
runtime = 0
while damage <= 0:
sensors = bng.poll_sensors(vehicle)
headlight_img = sensors['headlight_cam']['colour'].convert('RGB')
# sensors['']
# prediction = model.predict()
# steering = float(prediction[0][0]) #random.uniform(-1.0, 1.0)
# vehicle.control(throttle=throttle, steering=steering, brake=brake)
return_str = '\nPERTURBED headlight_cam INFO:'
print('\nPERTURBED headlight_cam INFO:')
temp = bng.poll_sensors(vehicle)['headlight_cam']
for key in temp:
if key == 'rotation':
degs = euler_from_quaternion(temp[key][0], temp[key][1],
temp[key][2], temp[key][3])
return_str = "{}\nquaternions {}".format(return_str, temp[key])
return_str = "{}\n{} {}".format(return_str, key,
[round(i, 3) for i in degs])
print(key, degs)
elif key != "colour" and key != "annotation" and key != "depth":
return_str = "{}\n{} {}".format(return_str, key, temp[key])
print(key, temp[key])
print('\nPERTURBED rearview_cam INFO:')
return_str = "{}\nPERTURBED rearview_cam INFO:".format(return_str)
# temp = bng.poll_sensors(vehicle)['rearview_cam']
for key in temp:
if key == 'rotation':
degs = euler_from_quaternion(temp[key][0], temp[key][1],
temp[key][2], temp[key][3])
return_str = "{}\nquaternions {}".format(return_str, temp[key])
return_str = "{}\n{} {}".format(return_str, key,
[round(i, 3) for i in degs])
print(key, degs)
elif key != "colour" and key != "annotation" and key != "depth":
return_str = "{}\n{} {}".format(return_str, key, temp[key])
print(key, temp[key])
# rearview_img = bng.poll_sensors(vehicle)['rearview_cam']['colour'].convert('RGB')
headlight_img = sensors['headlight_cam']['colour'].convert('RGB')
bng.step(1)
damage = sensors['damage']['damage']
runtime = sensors['timer']['time']
# print("runtime:{}".format(round(runtime, 2)))
print("time to crash:{} damage:{}".format(round(runtime, 2), damage))
bng.close()
# avg_kph = float(sum(kphs)) / len(kphs)
# plt.imshow(rearview_img)
# plt.pause(0.01)
plt.imshow(headlight_img)
plt.pause(0.01)
# results = {'pitch': round(pitch,3), 'roll':round(roll,3), "z":round(z,3), 'rearview_img':rearview_img, 'headlight_img':headlight_img}
return return_str
]
csv_file = "pos_crash_analysis.csv"
try:
with open(csv_file, 'a', encoding='utf-8') as csvfile:
writer = csv.DictWriter(csvfile,
fieldnames=csv_columns,
delimiter=',',
lineterminator='\n')
writer.writerow(pos_crash_dict)
except IOError:
print("I/O error")
# -------------------------------------------------------------------------
beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'crash_simulation_3')
road_a = Road('custom_track_center', looped=False)
collision_point = []
three_way = []
# 3 Way intersection
for tup in graph_degree:
three_way_coordinate = []
if tup[1] == 3:
three_way_coordinate.append(node_dict[tup[0]])
three_way_points = graph.neighbors(tup[0])
way_geo = (tup[0], node_dict[tup[0]], lane_dict[tup[0]],
width_dict[tup[0]])
bng.teleport_vehicle(pickup.vid, pos, rot=(0, 45, 0))
pickup.poll_sensors()
pos_after = pickup.state['pos']
assert (pos_before != pos_after)
pickup.poll_sensors()
pos_before = pickup.state['pos']
rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)
bng.teleport_vehicle(pickup.vid, pos, rot_quat=rot_quat)
pickup.poll_sensors()
pos_after = pickup.state['pos']
assert (pos_before != pos_after)
try:
bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0))
assert True
except socket.timeout:
assert False
try:
rot_quat = (-0.003337, -0.0021882, -0.6891695, 0.7245892)
bng.teleport_scenario_object(rb, (-10, 5, 0), rot_quat=rot_quat)
assert True
except socket.timeout:
assert False
if __name__ == '__main__':
bng = BeamNGpy('localhost', 64256)
test_quats(bng)
def run_sim(street_1: DecalRoad):
waypoints = []
for node in street_1.nodes:
waypoint = BeamNGWaypoint("waypoint_" + str(node),
get_node_coords(node))
waypoints.append(waypoint)
print(len(waypoints))
maps.beamng_map.generated().write_items(
street_1.to_json() + '\n' +
"\n".join([waypoint.to_json() for waypoint in waypoints]))
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_drive_in_lane(True)
vehicle.ai_set_mode("disabled")
vehicle.ai_set_speed(10 / 4)
steps = 5
def start():
for waypoint in waypoints[10:-1:20]:
vehicle.ai_set_waypoint(waypoint.name)
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.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 main():
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
state_cfg = Config()
parts_cfg = Config()
# Create a scenario in west_coast_usa
scenario = Scenario('west_coast_usa',
'research_test',
description='Random driving for research')
# Set up first vehicle, with two cameras, gforces sensor, lidar, electrical
# sensors, and damage sensors
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='RED',
color='Red')
vehicle = setup_sensors(vehicle)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=None,
rot_quat=(0, 0, 0.3826834, 0.9238795))
# Compile the scenario and place it in BeamNG's map folder
#scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
try:
#bng.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# Put simulator in pause awaiting further inputs
bng.pause()
pcfg = vehicle.get_part_config()
assert vehicle.skt
# Send random inputs to vehicle and advance the simulation 20 steps
#for _ in range(1024):
for _ in range(30):
throttle = random.uniform(0.0, 1.0)
steering = random.uniform(-1.0, 1.0)
brake = random.choice([0, 0, 0, 1])
vehicle.control(throttle=throttle, steering=steering, brake=brake)
# bng.step(20)
bng.step(20)
# Retrieve sensor data and show the camera data.
sensors = bng.poll_sensors(vehicle)
print('\n{} seconds passed.'.format(sensors['timer']['time']))
print("step in loop {}".format(_))
finally:
sensors = bng.poll_sensors(vehicle)
for s in sensors.keys():
print("{} : {}".format(s, sensors[s]))
damage_dict = sensors['damage']
state_cfg.update(sensors['damage'])
state_cfg.update(vehicle.state)
state_cfg.save('{}/state_cfg.json'.format(beamng.home))
vehicle.annotate_parts()
vehicle.update_vehicle()
#part_options = vehicle.get_part_options()
parts_cfg_dict = vehicle.get_part_config()
parts_cfg.load_values(vehicle.get_part_config())
#parts_cfg.update(vehicle.get_part_config())
parts_cfg.save('{}/parts_cfg.json'.format(beamng.home))
vehicle.save()
bng.close()
# reload scenario with saved config
random.seed(1703)
setup_logging()
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
loaded_config = state_cfg.load("{}/state_cfg.json".format(beamng.home))
scenario = Scenario('west_coast_usa',
'research_test',
description='Random driving for research')
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='RED',
color='Red')
vehicle = setup_sensors(vehicle)
vehicle.set_part_config(parts_cfg)
scenario.add_vehicle(vehicle,
pos=tuple(state_cfg.pos),
rot=None,
rot_quat=beamngpy.angle_to_quat(state_cfg.dir))
scenario.make(beamng)
bng = beamng.open(launch=True)
bng.spawn_vehicle(vehicle,
pos=tuple(state_cfg.pos),
rot=None,
rot_quat=beamngpy.angle_to_quat(state_cfg.dir))
# TODO: debug scenario restart
# scenario.restart()
try:
bng.hide_hud()
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
vehicle.load()
# Send random inputs to vehicle and advance the simulation 20 steps
#for _ in range(1024):
for _ in range(30):
throttle = random.uniform(0.0, 1.0)
steering = random.uniform(-1.0, 1.0)
brake = random.choice([0, 0, 0, 1])
vehicle.control(throttle=throttle, steering=steering, brake=brake)
# bng.step(20)
bng.step(20)
# Retrieve sensor data and show the camera data.
sensors = bng.poll_sensors(vehicle)
print('\n{} seconds passed.'.format(sensors['timer']['time']))
print("step in loop {}".format(_))
finally:
sensors = bng.poll_sensors(vehicle)
for s in sensors.keys():
print("{} : {}".format(s, sensors[s]))
state_cfg.update(sensors['damage'])
state_cfg.update(vehicle.state)
state_cfg.save('{}/state_cfg.json'.format(beamng.home))
parts_cfg.update(vehicle.get_part_config())
parts_cfg.save('{}/parts_cfg.json'.format(beamng.home))
bng.close()
def run_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 collect_data(beamNG_path):
bng = BeamNGpy('localhost', 64256, beamNG_path)
#bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')
scenario = Scenario('GridMap', 'example')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle1 = Vehicle('ego_vehicle',
model='etk800',
licence='PYTHON',
color='Red')
#vehicle2 = Vehicle('vehicle', model='etk800', licence='CRASH', color='Blue')
electrics = Electrics()
vehicle1.attach_sensor('electrics', electrics)
pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
# Place files defining our scenario for the simulator to read
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
#SIZE = 50
try:
bng.load_scenario(scenario)
bng.start_scenario()
#vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
vehicle1.ai_set_mode('span')
#collect data
print("\n Position and rotation of car \n ")
# for _ in range(200):
for _ in range(200):
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle1.state['pos'])
directions.append(vehicle1.state['dir'])
print([vehicle1.state['pos'], vehicle1.state['dir']])
#write data into file
# print ("position :",positions)
# print ("position :",directions)
sys_output.print_title(
"\n The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \""
)
write_data(FILE_POS, positions)
write_data(FILE_ROT, directions)
bng.stop_scenario()
bng.close()
time.sleep(2)
finally:
bng.close()
return (positions, directions)
#%matplotlib inline
import matplotlib
import matplotlib.pyplot as plt # for later visualization
import numpy as np # for easier vector computations
from beamngpy import BeamNGpy, Scenario, Vehicle
from beamngpy.sensors import Camera
import os
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.1')
# Create a vehicle with a camera sensor attached to it
vehicle = Vehicle('ego_vehicle', model='etki', licence='PYTHON', color='Green')
cam_pos = np.array([0, 0,
0]) # placeholder values that will be recomputed later
cam_dir = np.array([0, 0,
0]) # placeholder values that will be recomputed later
cam_fov = 70
cam_res = (1024, 1024) #(512, 512)
camera = Camera(cam_pos, cam_dir, cam_fov, cam_res, colour=True)
vehicle.attach_sensor('camera', camera)
# Simple scenario with the vehicle we just created standing at the origin
car_pos = np.array([0, 0, 0])
scenario = Scenario(
'smallgrid',
'multishot',
description='Demo of the camera sensor used like a multishot camera')
class Driver:
vehicle = None
bng = None
labeler = None
left_path = None
right_path = None
driving_path = None
road_profiler = None
car_model = None
road_model = None
def __init__(self, car_model: dict, road_model: dict, control_model: dict):
self.car_model = car_model
self.road_model = road_model
self.control_model = control_model
# Note: Speed limit must be m/s
self.road_profiler = RoadProfiler(
road_model['mu'], road_model['speed_limit'] / 3.6,
control_model['discretization_factor'])
def _compute_driving_path(self, car_state, road_name):
road_geometry = self.bng.get_road_edges(road_name)
left_edge_x = np.array([e['left'][0] for e in road_geometry])
left_edge_y = np.array([e['left'][1] for e in road_geometry])
right_edge_x = np.array([e['right'][0] for e in road_geometry])
right_edge_y = np.array([e['right'][1] for e in road_geometry])
road_edges = dict()
road_edges['left_edge_x'] = left_edge_x
road_edges['left_edge_y'] = left_edge_y
road_edges['right_edge_x'] = right_edge_x
road_edges['right_edge_y'] = right_edge_y
self.right_edge = LineString(
zip(road_edges['right_edge_x'][::-1],
road_edges['right_edge_y'][::-1]))
self.left_edge = LineString(
zip(road_edges['left_edge_x'], road_edges['left_edge_y']))
current_position = Point(car_state['pos'][0], car_state['pos'][1])
from shapely.ops import nearest_points
from shapely.affinity import rotate
projection_point_on_right = nearest_points(self.right_edge,
current_position)[0]
projection_point_on_left = nearest_points(self.left_edge,
current_position)[0]
# If the car is closest to the left, then we need to switch the direction of the road...
if current_position.distance(
projection_point_on_right) > current_position.distance(
projection_point_on_left):
# Swap the axis and recompute the projection points
l.debug("Reverse traffic direction")
temp = self.right_edge
self.right_edge = self.left_edge
self.left_edge = temp
del temp
projection_point_on_right = nearest_points(self.right_edge,
current_position)[0]
projection_point_on_left = nearest_points(self.left_edge,
current_position)[0]
# Traffic direction is always 90-deg counter clockwise from right
# Now rotate right point 90-deg counter clockwise from left and we obtain the traffic direction
rotated_right = rotate(projection_point_on_right,
90.0,
origin=projection_point_on_left)
# Vector defining the direction of the road
traffic_direction = np.array([
rotated_right.x - projection_point_on_left.x,
rotated_right.y - projection_point_on_left.y
])
# Find the segment containing the projection of current location
# Starting point on right edge
start_point = None
for pair in pairs(list(self.right_edge.coords[:])):
segment = LineString([pair[0], pair[1]])
# xs, ys = segment.coords.xy
# plt.plot(xs, ys, color='green')
if segment.distance(projection_point_on_right) < 1.8e-5:
road_direction = np.array(
[pair[1][0] - pair[0][0], pair[1][1] - pair[0][1]])
if dot(traffic_direction, road_direction) < 0:
l.debug("Reverse order !")
self.right_edge = LineString([
Point(p[0], p[1]) for p in self.right_edge.coords[::-1]
])
start_point = Point(pair[0][0], pair[0][1])
break
else:
l.debug("Original order !")
start_point = Point(pair[1][0], pair[1][1])
break
assert start_point is not None
# At this point compute the driving path of the car (x, y, t)
self.driving_path = [current_position]
# plt.plot(current_position.x, current_position.y, color='black', marker="x")
# # This might not be robust we need to get somethign close by
# plt.plot([pair[0][0], pair[1][0]], [pair[0][1], pair[1][1]], marker="o")
# plt.plot(projection_point_on_right.x, projection_point_on_right.y, color='b', marker="*")
#
started = False
for right_position in [
Point(p[0], p[1]) for p in list(self.right_edge.coords)
]:
if right_position.distance(start_point) < 1.8e-5:
# print("Start to log positions")
# plt.plot(right_position.x, right_position.y, color='blue', marker="o")
started = True
if not started:
# print("Skip point")
# plt.plot(right_position.x, right_position.y, color='red', marker="*")
continue
else:
# print("Consider point")
# plt.plot(right_position.x, right_position.y, color='green', marker="o")
pass
# Project right_position to left_edge
projected_point = self.left_edge.interpolate(
self.left_edge.project(right_position))
# Translate the right_position 2m toward the center
line = LineString([(right_position.x, right_position.y),
(projected_point.x, projected_point.y)])
self.driving_path.append(line.interpolate(2.0))
def plot_all(self, car_state):
current_position = Point(car_state['pos'][0], car_state['pos'][1])
plt.figure(1, figsize=(5, 5))
plt.clf()
ax = plt.gca()
x, y = self.left_edge.coords.xy
ax.plot(x, y, 'r-')
x, y = self.right_edge.coords.xy
ax.plot(x, y, 'b-')
driving_lane = LineString([p for p in self.driving_path])
x, y = driving_lane.coords.xy
ax.plot(x, y, 'g-')
# node = {
# 'x': target_position.x,
# 'y': target_position.y,
# 'z': 0.3,
# 't': target_time,
# }
xs = [node['x'] for node in self.script]
ys = [node['y'] for node in self.script]
# print("{:.2f}".format(3.1415926));
vs = ['{:.2f}'.format(node['avg_speed'] * 3.6) for node in self.script]
# plt.plot(xs, ys, marker='.')
ax = plt.gca()
for i, txt in enumerate(vs):
ax.annotate(txt, (xs[i], ys[i]))
plt.plot(current_position.x,
current_position.y,
marker="o",
color="green")
# Center around current_positions
ax.set_xlim([current_position.x - 50, current_position.x + 50])
ax.set_ylim([current_position.y - 50, current_position.y + 50])
plt.draw()
plt.pause(0.01)
def run(self, debug=False):
try:
self.vehicle = Vehicle(car_model['id'])
electrics = Electrics()
self.vehicle.attach_sensor('electrics', electrics)
# Connect to running beamng
self.bng = BeamNGpy(
'localhost', 64256
) #, home='C://Users//Alessio//BeamNG.research_unlimited//trunk')
self.bng = self.bng.open(launch=False)
# Put simulator in pause awaiting while planning the driving
self.bng.pause()
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
self.bng.set_deterministic() # Set simulator to be deterministic
self.bng.connect_vehicle(self.vehicle)
assert self.vehicle.skt
# Get Initial state of the car. This assumes that the script is invoked after the scenario is started
self.bng.poll_sensors(self.vehicle)
# Compute the "optimal" driving path and program the ai_script
self._compute_driving_path(self.vehicle.state,
self.road_model['street'])
self.script = self.road_profiler.compute_ai_script(
LineString(self.driving_path), self.car_model)
# Enforce initial car direction nad up
start_dir = (self.vehicle.state['dir'][0],
self.vehicle.state['dir'][1],
self.vehicle.state['dir'][2])
up_dir = (0, 0, 1)
# Configure the ego car
self.vehicle.ai_set_mode('disabled')
# Note that set script teleports the car by default
self.vehicle.ai_set_script(self.script,
start_dir=start_dir,
up_dir=up_dir)
# Resume the simulation
self.bng.resume()
# At this point the controller can stop ? or wait till it is killed
while True:
if debug:
self.bng.pause()
self.bng.poll_sensors(self.vehicle)
self.plot_all(self.vehicle.state)
self.bng.resume()
# Progress the simulation for some time...
# self.bng.step(50)
sleep(2)
except Exception:
# When we brutally kill this process there's no need to log an exception
l.error("Fatal Error", exc_info=True)
finally:
self.bng.close()
activatedSensors[x] = True
valid = True
elif (choice == 'n'):
activatedSensors[x] = False
valid = True
elif (choice != 'n' and choice != 'y'):
print("Please only enter 'y' for yes or 'n' for no")
#endregion
sns.set() # Make seaborn set matplotlib styling
beamNGPAth = "C:/Users/julie/Desktop/beamng"
# Instantiate a BeamNGpy instance the other classes use for reference & communication
beamng = BeamNGpy(
'localhost', 64256,
beamNGPAth) # This is the host & port used to communicate over
# Create a vehicle instance that will be called 'ego' in the simulation
# using the etk800 model the simulator ships with
vehicle = Vehicle('ego', model='etkc', licence='LIFLAB', colour='Blue')
# Create an Electrics sensor and attach it to the vehicle
electrics = Electrics()
vehicle.attach_sensor('electrics', electrics)
#Create a Damage sensor and attach it to the vehicle
damage = Damage()
vehicle.attach_sensor('damage', damage)
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
# 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 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
class Simulation(object):
def __init__(self):
super().__init__()
# Break an episode upon user input.
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())
# At this point, BeamNG creates a prefab file which contains the specified roads.
self.scenario.make(self.bng)
prefab_path = self.scenario.get_prefab_path()
# Update the road definitions in the prefab.
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):
"""
Break an episode in case of user input.
:return: (None)
"""
while True:
a = input()
self.done = not self.done
def take_action(self, action):
"""
Execute an action.
:param action: ([float])
:return: (None)
"""
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):
"""
Compute the reward based on the observed state.
:param done: (bool)
:param dist: (float)
:return: (float)
"""
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):
"""
Observe the current state.
:return: (np.ndarray, float, bool, dict)
"""
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]
pos = self.vehicle.state['pos']
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):
"""
Get the current velocity of the vehicle.
:return: (float)
"""
state = self.vehicle.state
velocity = np.linalg.norm(state["vel"])
return velocity * 3.6
def position(self):
"""
Get the current position coordinates of the vehicle.
:return: ([float])
"""
return self.vehicle.state["pos"]
def refresh_dist(self, pos):
"""
Update the driven distance since the last observation.
:param pos: ([float])
:return: (None)
"""
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):
"""
Close connection to the BeamNG instance.
:return: (None)
"""
self.bng.close()
def reset(self):
"""
Resets the state of the simulation and starts a new episode.
:return: (None)
"""
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']
# Respawn the vehicle close to its crash point.
closest_point = self.road.closest_roadpoint(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 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
map_beamng_serialize = path + filename + '.nodes.serialize.beamng'
map_degree_serialize = path + filename + '.ways.degree.serialize'
node_dict = pickle.load(open(map_nodes_serialize, "rb"))
print("Nodes Loaded")
graph = pickle.load(open(map_ways_serialize, "rb"))
print("Graph Loaded")
beamng_dict = pickle.load(open(map_beamng_serialize, "rb"))
print("BeamNG Nodes Loaded")
graph_degree = pickle.load(open(map_degree_serialize, "rb"))
print("Graph Degree")
beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
def getRoadPolyLineLaneMarking(point1,point2, width):
print("Road Lanes")
#https://stackoverflow.com/questions/47040213/find-perpendicular-line-using-points-on-that-line
print(point1,point2, width)
dx = float(point2[0] - point1[0])
dy = float(point2[1] - point1[1])
L = float(math.sqrt(float(float(dx * dx) + float(dy * dy))))
U = (float(-dy / L), float(dx / L))
F = float(float(width) - 0.05)
import matplotlib
import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
import seaborn as sns
from beamngpy import BeamNGpy, Vehicle, Scenario
from beamngpy.sensors import Camera, GForces, Lidar, Electrics, Damage
sns.set()
# Instantiate a BeamNGpy instance the other classes use for reference & communication
# This is the host & port used to communicate over
beamng = BeamNGpy('localhost', 64294)
#loads Custom Map
scenario = Scenario('Mytest', 'demo')
#adding ego Vehicle to the scenario
vehicle = Vehicle('ego_vehicle', model='etk800', licence='RED', color='Green')
electrics = Electrics()
vehicle.attach_sensor('electrics', electrics)
damage1 = Damage()
vehicle.attach_sensor('damage1', damage1)
scenario.add_vehicle(vehicle,
pos=(104.647, -1.92827, -3.54338),
rot=(0, 0, 90))
#add vehicle as Obstacle
class ImpactGenerator:
parts = [
'etk800_bumper_F',
'etk800_bumperbar_F',
'etk800_bumper_R',
'etk800_fender_L',
'etk800_fender_R',
'etk800_hood',
'etk800_towhitch',
'etk800_steer',
'etk800_radiator',
'etk800_roof_wagon',
'wheel_F_5',
]
emptyable = {
'etk800_bumperbar_F',
'etk800_towhitch',
}
wca = {
'level': 'west_coast_usa',
'a_spawn': (-270.75, 678, 74.9),
'b_spawn': (-260.25, 678, 74.9),
'pole_pos': (-677.15, 848, 75.1),
'linear_pos_a': (-630, 65, 103.4),
'linear_pos_b': (-619, 77, 102.65),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 75.1),
't_pos_b': (-453, 700, 75.1),
't_rot_b': (0, 0, 315),
'ref_pos': (-18, 610, 75),
}
smallgrid = {
'level': 'smallgrid',
'a_spawn': (-270.75, 678, 0.1),
'b_spawn': (-260.25, 678, 0.1),
'pole_pos': (-677.15, 848, 0.1),
'pole': (-682, 842, 0),
'linear_pos_a': (-630, 65, 0.1),
'linear_pos_b': (-619, 77, 0.1),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 0.1),
't_pos_b': (-453, 700, 0.1),
't_rot_b': (0, 0, 315),
'ref_pos': (321, 321, 0.1),
}
def __init__(self,
bng_home,
output,
config,
poolsize=2,
smallgrid=False,
sim_mtx=None,
similarity=0.5,
random_select=False,
single=False):
self.bng_home = bng_home
self.output = output
self.config = config
self.smallgrid = smallgrid
self.single = single
self.impactgen_mats = None
if smallgrid:
mats = materialmngr.get_impactgen_materials(bng_home)
self.impactgen_mats = sorted(list(mats))
self.poolsize = poolsize
self.similarity = similarity
self.sim_mtx = sim_mtx
self.random_select = random_select
self.pole_space = None
self.t_bone_space = None
self.linear_space = None
self.nocrash_space = None
self.post_space = None
self.total_possibilities = 0
self.bng = BeamNGpy('localhost', 64256, home=bng_home)
self.scenario = None
scenario_props = ImpactGenerator.wca
if smallgrid:
scenario_props = ImpactGenerator.smallgrid
self.vehicle_a = Vehicle('vehicle_a', model='etk800')
self.vehicle_b = Vehicle('vehicle_b', model='etk800')
self.scenario = Scenario(scenario_props['level'], 'impactgen')
self.scenario.add_vehicle(self.vehicle_a,
pos=scenario_props['a_spawn'],
rot=(0, 0, 0))
self.scenario.add_vehicle(self.vehicle_b,
pos=scenario_props['b_spawn'],
rot=(0, 0, 0))
self.vehicle_a_parts = defaultdict(set)
self.vehicle_a_config = None
self.vehicle_b_config = None
def generate_colors(self):
return copy.deepcopy(self.config['colors'])
def generate_nocrash_space(self, props):
nocrash_options = []
for part in ImpactGenerator.parts: # Vary each configurable part
nocrash_options.append(self.vehicle_a_parts[part])
self.nocrash_space = OptionSpace(nocrash_options)
def generate_pole_space(self, props):
pole_options = [(False, True)] # Vehicle facing forward/backward
pole_options.append(np.linspace(-0.75, 0.75, 5)) # Position offset
pole_options.append(np.linspace(0.15, 0.5, 4)) # Throttle intensity
for part in ImpactGenerator.parts: # Vary each configurable part
pole_options.append(self.vehicle_a_parts[part])
self.pole_space = OptionSpace(pole_options)
def generate_t_bone_space(self, props):
t_options = [(False, True)] # Vehicle hit left/right
t_options.append(np.linspace(-30, 30, 11)) # A rotation offset
t_options.append(np.linspace(-1.5, 1.5, 5)) # B pos. offset
t_options.append(np.linspace(0.2, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
t_options.append(self.vehicle_a_parts[part])
self.t_bone_space = OptionSpace(t_options)
def generate_linear_space(self, props):
linear_options = [(False, True)] # Vehicle hit front/back
linear_options.append(np.linspace(-15, 15, 5)) # A rot. offset
linear_options.append(np.linspace(-1.33, 1.33, 5)) # B pos. offset
linear_options.append(np.linspace(0.25, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
linear_options.append(self.vehicle_a_parts[part])
self.linear_space = OptionSpace(linear_options)
def get_material_options(self):
if not self.random_select:
selected = materialmngr.pick_materials(self.impactgen_mats,
self.sim_mtx,
poolsize=self.poolsize,
similarity=self.similarity)
if selected is None:
log.info('Could not find material pool through similarity. '
'Falling back to random select.')
else:
selected = random.sample(self.impactgen_mats, self.poolsize)
return selected
def generate_post_space(self):
colors = self.generate_colors()
post_options = []
post_options.append(self.config['times'])
if self.smallgrid:
post_options.append([0])
post_options.append([0])
else:
post_options.append(self.config['clouds'])
post_options.append(self.config['fogs'])
post_options.append(colors)
if self.smallgrid:
mats = self.get_material_options()
if mats is not None:
post_options.append(list(mats))
post_options.append(list(mats))
return OptionSpace(post_options)
def generate_spaces(self):
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
self.generate_nocrash_space(props)
self.generate_t_bone_space(props)
self.generate_linear_space(props)
self.generate_pole_space(props)
def scan_parts(self, parts, known=set()):
with open('out.json', 'w') as outfile:
outfile.write(json.dumps(parts, indent=4, sort_keys=True))
for part_type in ImpactGenerator.parts:
options = parts[part_type]
self.vehicle_a_parts[part_type].update(options)
def init_parts(self):
self.vehicle_a_config = self.vehicle_a.get_part_config()
self.vehicle_b_config = self.vehicle_b.get_part_config()
b_parts = self.vehicle_b_config['parts']
b_parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
b_parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
b_parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
options = self.vehicle_a.get_part_options()
self.scan_parts(options)
for k in self.vehicle_a_parts.keys():
self.vehicle_a_parts[k] = list(self.vehicle_a_parts[k])
if k in ImpactGenerator.emptyable:
self.vehicle_a_parts[k].append('')
def init_settings(self):
self.bng.set_particles_enabled(False)
self.generate_spaces()
log.info('%s pole crash possibilities.', self.pole_space.count)
log.info('%s T-Bone crash possibilities.', self.t_bone_space.count)
log.info('%s parallel crash possibilities.', self.linear_space.count)
log.info('%s no crash possibilities.', self.nocrash_space.count)
self.total_possibilities = \
self.pole_space.count + \
self.t_bone_space.count + \
self.linear_space.count + \
self.nocrash_space.count
log.info('%s total incidents possible.', self.total_possibilities)
def get_vehicle_config(self, setting):
parts = dict()
for idx, part in enumerate(ImpactGenerator.parts):
parts[part] = setting[idx]
refwheel = parts['wheel_F_5']
parts['wheel_R_5'] = refwheel.replace('_F', '_R')
# Force licence plate to always be German
parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
config = copy.deepcopy(self.vehicle_a_config)
config['parts'] = parts
return config
def set_annotation_paths(self):
part_path = os.path.join(self.bng_home, PART_ANNOTATIONS)
part_path = os.path.abspath(part_path)
obj_path = os.path.join(self.bng_home, OBJ_ANNOTATIONS)
obj_path = os.path.abspath(obj_path)
req = dict(type='ImpactGenSetAnnotationPaths')
req['partPath'] = part_path
req['objPath'] = obj_path
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenAnnotationPathsSet'
def set_image_properties(self):
req = dict(type='ImpactGenSetImageProperties')
req['imageWidth'] = self.config['imageWidth']
req['imageHeight'] = self.config['imageHeight']
req['colorFmt'] = self.config['colorFormat']
req['annotFmt'] = self.config['annotFormat']
req['radius'] = self.config['cameraRadius']
req['height'] = self.config['cameraHeight']
req['fov'] = self.config['fov']
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenImagePropertiesSet'
def setup(self):
self.scenario.make(self.bng)
log.debug('Loading scenario...')
self.bng.load_scenario(self.scenario)
log.debug('Setting steps per second...')
self.bng.set_steps_per_second(50)
log.debug('Enabling deterministic mode...')
self.bng.set_deterministic()
log.debug('Starting scenario...')
self.bng.start_scenario()
log.debug('Scenario started. Sleeping 20s.')
time.sleep(20)
self.init_parts()
self.init_settings()
log.debug('Setting annotation properties.')
self.set_annotation_paths()
self.set_image_properties()
def settings_exhausted(self):
return self.t_bone_space.exhausted() and \
self.linear_space.exhausted() and \
self.pole_space.exhausted() and \
self.nocrash_space.exhausted()
def set_post_settings(self, vid, settings):
req = dict(type='ImpactGenPostSettings')
req['ego'] = vid
req['time'] = settings[0]
req['clouds'] = settings[1]
req['fog'] = settings[2]
req['color'] = settings[3]
if len(settings) > 4:
req['skybox'] = settings[4]
if len(settings) > 5:
req['ground'] = settings[5]
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPostSet'
def finished_producing(self):
req = dict(type='ImpactGenOutputGenerated')
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipGenerated'
return resp['state']
def produce_output(self, color_name, annot_name):
while not self.finished_producing():
time.sleep(0.2)
req = dict(type='ImpactGenGenerateOutput')
req['colorName'] = color_name
req['annotName'] = annot_name
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipStarted'
'ImpactGenZipStarted'
def capture_post(self, crash_setting):
log.info('Enumerating post-crash settings and capturing output.')
self.bng.switch_vehicle(self.vehicle_a)
ref_pos = ImpactGenerator.wca['ref_pos']
if self.smallgrid:
ref_pos = ImpactGenerator.smallgrid['ref_pos']
self.bng.teleport_vehicle(self.vehicle_a, ref_pos)
self.bng.teleport_vehicle(self.vehicle_b, (10000, 10000, 10000))
self.bng.step(50, wait=True)
self.bng.pause()
self.post_space = self.generate_post_space()
while not self.post_space.exhausted():
post_setting = self.post_space.sample_new()
scenario = [[str(s) for s in crash_setting]]
scenario.append([str(s) for s in post_setting])
key = str(scenario).encode('ascii')
key = hashlib.sha512(key).hexdigest()[:30]
t = int(time.time())
color_name = '{}_{}_0_image.zip'.format(t, key)
annot_name = '{}_{}_0_annotation.zip'.format(t, key)
color_name = os.path.join(self.output, color_name)
annot_name = os.path.join(self.output, annot_name)
log.info('Setting post settings.')
self.set_post_settings(self.vehicle_a.vid, post_setting)
log.info('Producing output.')
self.produce_output(color_name, annot_name)
if self.single:
break
self.bng.resume()
def run_t_bone_crash(self):
log.info('Running t-bone crash setting.')
if self.t_bone_space.exhausted():
log.debug('T-Bone crash setting exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.t_bone_space.sample_new()
side, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if side:
angle += 225
else:
angle += 45
pos_a = props['t_pos_a']
rot_b = props['t_rot_b']
pos_b = list(props['t_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunTBone')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending t-bone crash config.')
self.bng.send(req)
log.debug('T-Bone crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenTBoneRan'
return setting
def run_linear_crash(self):
log.info('Running linear crash setting.')
if self.linear_space.exhausted():
log.debug('Linear crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.linear_space.sample_new()
back, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if back:
angle += 225
else:
offset += 1.3
angle += 45
pos_a = props['linear_pos_a']
rot_b = props['linear_rot_b']
pos_b = list(props['linear_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunLinear')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending linear crash config.')
self.bng.send(req)
log.debug('Linear crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenLinearRan'
return setting
def run_pole_crash(self):
log.info('Running pole crash setting.')
if self.pole_space.exhausted():
log.debug('Pole crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.pole_space.sample_new()
back, offset, throttle = setting[:3]
config = setting[3:]
config = self.get_vehicle_config(config)
angle = 45
if back:
angle = 225
offset += 0.85
throttle = -throttle
pos = list(props['pole_pos'])
pos[0] += offset
req = dict(type='ImpactGenRunPole')
req['ego'] = self.vehicle_a.vid
req['config'] = config
req['position'] = pos
req['angle'] = angle
req['throttle'] = throttle
if self.smallgrid:
req['polePosition'] = props['pole']
log.debug('Sending pole crash config.')
self.bng.send(req)
log.debug('Got pole crash response.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPoleRan'
return setting
def run_no_crash(self):
log.info('Running non-crash scenario.')
if self.nocrash_space.exhausted():
return None
setting = self.nocrash_space.sample_new()
log.info('Got new setting: %s', setting)
vehicle_config = self.get_vehicle_config(setting)
log.info('Got new vehicle config: %s', vehicle_config)
req = dict(type='ImpactGenRunNonCrash')
req['ego'] = self.vehicle_a.vid
req['config'] = vehicle_config
log.info('Sending Non-Crash request: %s', req)
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenNonCrashRan'
log.info('Non-crash finished.')
return setting
def run_incident(self, incident):
log.info('Setting up next incident.')
self.bng.display_gui_message('Setting up next incident...')
setting = incident()
self.capture_post(setting)
return setting
def run_incidents(self):
log.info('Enumerating possible incidents.')
count = 1
incidents = [
self.run_t_bone_crash,
self.run_linear_crash,
self.run_pole_crash,
self.run_no_crash,
]
while not self.settings_exhausted():
log.info('Running incident %s of %s...', count,
self.total_possibilities)
self.bng.restart_scenario()
log.info('Scenario restarted.')
time.sleep(5.0)
self.vehicle_b.set_part_config(self.vehicle_b_config)
log.info('Vehicle B config set.')
incident = incidents[count % len(incidents)]
if self.run_incident(incident) is None:
log.info('Ran out of options for: %s', incident)
incidents.remove(incident) # Possibility space exhausted
count += 1
def run(self):
log.info('Starting up BeamNG instance.')
self.bng.open(['impactgen/crashOutput'])
self.bng.skt.settimeout(1000)
try:
log.info('Setting up BeamNG instance.')
self.setup()
self.run_incidents()
finally:
log.info('Closing BeamNG instance.')
self.bng.close()
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)
# 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()
# Attach them
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('back_cam', back_camera)
vehicle.attach_sensor('gforces', gforces)
vehicle.attach_sensor('lidar', lidar)
vehicle.attach_sensor('electrics', electrics)
vehicle.attach_sensor('damage', damage)
scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=(0, 0, 45))
# 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.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()
bng.hide_hud()
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)
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()
from beamngpy import BeamNGpy, Scenario, Vehicle, StaticObject
from beamngpy.sensors import Electrics, Damage
import numpy as np
from time import sleep, time
beamng = BeamNGpy('localhost', 64256, home=r'C:\BeamNG_unlimited\trunk')
scenario = Scenario('west_coast_usa', 't_intersection_wo_obj')
vut = Vehicle('vut', model='coupe', licence='VUT', colour='Red')
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))
car_2 = Vehicle('car_2', model='etk800', licence='CAR 2', colour='Blue')
scenario.add_vehicle(car_2, 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}])
car_2.ai_set_line([{'pos': (-198.5, -164.189, 119.7), 'speed': 2000}])
def main():
beamng = BeamNGpy('localhost', 64256)
bng = beamng.open(launch=True)
scenario = Scenario('smallgrid', 'mesh_test')
cylinder = ProceduralCylinder(name='cylinder',
radius=3.5,
height=5,
pos=(10, -10, 0),
rot=None,
rot_quat=(0, 0, 0, 1))
scenario.add_procedural_mesh(cylinder)
bump = ProceduralBump(name='bump',
pos=(-10, -10, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
width=5,
length=7,
height=2,
upper_length=2,
upper_width=2
)
scenario.add_procedural_mesh(bump)
cone = ProceduralCone(name='cone',
pos=(-10, -20, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
radius=3.5,
height=5)
scenario.add_procedural_mesh(cone)
cube = ProceduralCube(name='cube',
pos=(0, -20, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
size=(5, 2, 3))
scenario.add_procedural_mesh(cube)
ring = ProceduralRing(name='ring',
pos=(10, -20, 0),
rot=None,
rot_quat=(0, 0.7071068, 0, 0.7071068),
radius=2,
thickness=1
)
scenario.add_procedural_mesh(ring)
vehicle = Vehicle('ego_vehicle', model='etk800')
scenario.add_vehicle(vehicle, pos=(0, 0, 0),
rot=None, rot_quat=(0, 0, 0, 1))
scenario.make(bng)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.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
def beamng():
beamng = BeamNGpy('localhost', 64256)
return beamng
def set_time_of_day_to(self, instance: BeamNGpy):
if self.time_of_day:
instance.set_tod(self.time_of_day)
