class BeamNGBrewer:
def __init__(self, beamng_home=None, road_nodes: List4DTuple = None):
self.beamng = BeamNGpy('localhost', 64256, home=beamng_home)
self.vehicle: Vehicle = None
self.camera: BeamNGCamera = None
if road_nodes:
self.setup_road_nodes(road_nodes)
steps = 5
self.params = SimulationParams(beamng_steps=steps,
delay_msec=int(steps * 0.05 * 1000))
self.vehicle_start_pose = BeamNGPose()
def setup_road_nodes(self, road_nodes):
self.road_nodes = road_nodes
self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points(
road_nodes)
self.road_points = RoadPoints().add_middle_nodes(road_nodes)
def setup_vehicle(self) -> Vehicle:
assert self.vehicle is None
self.vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
return self.vehicle
def setup_scenario_camera(self,
resolution=(1280, 1280),
fov=120) -> BeamNGCamera:
assert self.camera is None
self.camera = BeamNGCamera(self.beamng, 'brewer_camera')
return self.camera
def bring_up(self):
self.scenario = Scenario('tig', 'tigscenario')
if self.vehicle:
self.scenario.add_vehicle(self.vehicle,
pos=self.vehicle_start_pose.pos,
rot=self.vehicle_start_pose.rot)
if self.camera:
self.scenario.add_camera(self.camera.camera, self.camera.name)
self.scenario.make(self.beamng)
if not self.beamng.server:
self.beamng.open()
self.beamng.pause()
self.beamng.set_deterministic()
self.beamng.load_scenario(self.scenario)
self.beamng.start_scenario()
def __del__(self):
if self.beamng:
try:
self.beamng.close()
except:
pass
def run_sim(street_1: DecalRoad, ai_aggression):
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())
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(False)
vehicle.ai_set_speed(25)
vehicle.ai_set_waypoint(waypoint_goal.name)
#vehicle.ai_set_mode("manual")
steps = 5
def start():
for idx in range(1000):
if (idx * 0.05 * steps) > 3.:
sim_data_collector.collect_and_write_current_data()
if sim_data_collector.oob_monitor.is_oob(wrt="center"):
raise ValueError("OOB detected during training")
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()
class BeamNGBrewer:
def __init__(self,
beamng_home=None,
beamng_user=None,
reuse_beamng=True,
road_nodes: List4DTuple = None):
self.reuse_beamng = reuse_beamng
if self.reuse_beamng:
# This represents the running BeamNG simulator. Since we use launch=True this should automatically
# shut down when the main python process exits or when we call self.beamng_process.stop()
self.beamng_process = BeamNGpy('localhost',
64256,
home=beamng_home,
user=beamng_user)
self.beamng_process = self.beamng_process.open(launch=True)
# This is used to bring up each simulation without restarting the simulator
self.beamng = BeamNGpy('localhost',
64256,
home=beamng_home,
user=beamng_user)
# We need to wait until this point otherwise the BeamNG loggers's level will be (re)configured by BeamNGpy
log.info("Disabling BEAMNG logs")
for id in [
"beamngpy", "beamngpy.beamngpycommon", "beamngpy.BeamNGpy",
"beamngpy.beamng", "beamngpy.Scenario", "beamngpy.Vehicle"
]:
logger = log.getLogger(id)
logger.setLevel(log.CRITICAL)
logger.disabled = True
self.vehicle: Vehicle = None
self.camera: BeamNGCamera = None
if road_nodes:
self.setup_road_nodes(road_nodes)
steps = 5
self.params = SimulationParams(beamng_steps=steps,
delay_msec=int(steps * 0.05 * 1000))
self.vehicle_start_pose = BeamNGPose()
def setup_road_nodes(self, road_nodes):
self.road_nodes = road_nodes
self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points(
road_nodes)
self.road_points = RoadPoints().add_middle_nodes(road_nodes)
def setup_vehicle(self) -> Vehicle:
assert self.vehicle is None
self.vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
return self.vehicle
def setup_scenario_camera(self,
resolution=(1280, 1280),
fov=120) -> BeamNGCamera:
assert self.camera is None
self.camera = BeamNGCamera(self.beamng, 'brewer_camera')
return self.camera
# TODO COnsider to transform brewer into a ContextManager or get rid of it...
def bring_up(self):
if self.reuse_beamng:
# This assumes BeamNG is already running
self.beamng.open(launch=False)
else:
self.beamng.open(launch=True)
# After 1.18 to make a scenario one needs a running instance of BeamNG
self.scenario = Scenario('tig', 'tigscenario')
if self.vehicle:
self.scenario.add_vehicle(self.vehicle,
pos=self.vehicle_start_pose.pos,
rot=self.vehicle_start_pose.rot)
if self.camera:
self.scenario.add_camera(self.camera.camera, self.camera.name)
self.scenario.make(self.beamng)
self.beamng.set_deterministic()
self.beamng.load_scenario(self.scenario)
self.beamng.start_scenario()
# Pause the simulator only after loading and starting the scenario
self.beamng.pause()
def __del__(self):
if self.beamng:
try:
self.beamng.close()
except:
pass
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()
setup_logging()
bng = BeamNGpy('localhost', 64256)
scenario = Scenario('smallgrid', 'small_test')
vehicle = Vehicle('egovehicle', model='etk800', licence='313')
scenario.add_vehicle(vehicle, pos=(-1.5, -1, 0), rot=(0, 0, 0))
nodes = [(0, 0, 0, 6), (0, -20, 0, 6), (0, -100, 0, 6), (-25, -200, 0, 6),
(25, -300, 0, 6)]
road = Road(material='a_asphalt_01_a',
rid='main_road',
looped=False,
texture_length=10)
road.nodes.extend(nodes)
scenario.add_road(road)
scenario.make(bng)
for i, line in enumerate(fileinput.input(scenario.get_prefab_path(),
inplace=1)):
sys.stdout.write(line.replace('overObjects = "0";', 'overObjects = "1";'))
bng.open()
bng.load_scenario(scenario)
bng.start_scenario()
bng.set_deterministic()
bng.pause()
class Simulation(object):
def __init__(self) -> None:
super().__init__()
thread = Thread(target=self._intervene)
thread.start()
self.step = 0
self.dist_driven = 0
self.done = False
self.last_action = (0.0, 0.0)
self.bng = BeamNGpy('localhost', 64257, home=BEAMNG_HOME)
self.scenario = Scenario('train', 'train', authors='Vsevolod Tymofyeyev',
description='Reinforcement learning')
self.road = TrainingRoad(ASFAULT_PREFAB)
self.road.calculate_road_line()
spawn_point = self.road.spawn_point()
self.last_pos = spawn_point.pos()
self.scenario.add_road(self.road.asphalt)
self.scenario.add_road(self.road.mid_line)
self.scenario.add_road(self.road.left_line)
self.scenario.add_road(self.road.right_line)
self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='RL FTW', color='Red')
front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV,
resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
colour=True, depth=False, annotation=False)
self.vehicle.attach_sensor("front_camera", front_camera)
self.scenario.add_vehicle(self.vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
self.scenario.make(self.bng)
prefab_path = self.scenario.get_prefab_path()
update_prefab(prefab_path)
self.bng.open()
self.bng.set_deterministic()
self.bng.set_steps_per_second(SPS)
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
# self.bng.hide_hud()
self.bng.pause()
def _intervene(self):
while True:
a = input()
self.done = not self.done
def take_action(self, action):
steering, throttle = action
steering = steering.item()
throttle = throttle.item()
self.last_action = action
self.step += 1
self.vehicle.control(steering=steering, throttle=throttle, brake=0.0)
self.bng.step(STEPS_INTERVAL)
def _reward(self, done, dist):
steering = self.last_action[0]
throttle = self.last_action[1]
velocity = self.velocity() # km/h
if not done:
reward = REWARD_STEP + THROTTLE_REWARD_WEIGHT * throttle #- MID_DIST_PENALTY_WEIGHT * dist
else:
reward = REWARD_CRASH - CRASH_SPEED_WEIGHT * throttle
return reward
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
image = sensors['front_camera']['colour'].convert("RGB")
image = np.array(image)
r = ROI
# Cut to the relevant region
image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])]
# Convert to BGR
state = image[:, :, ::-1]
#done = self.done
pos = self.vehicle.state['pos']
dir = self.vehicle.state['dir']
self.refresh_dist(pos)
self.last_pos = pos
dist = self.road.dist_to_center(self.last_pos)
#velocity = self.velocity()
done = dist > MAX_DIST #or velocity > MAX_VELOCITY
reward = self._reward(done, dist)
return state, reward, done, {}
def velocity(self):
state = self.vehicle.state
velocity = np.linalg.norm(state["vel"])
return velocity * 3.6
def position(self):
return self.vehicle.state["pos"]
def refresh_dist(self, pos):
pos = np.array(pos)
last_pos = np.array(self.last_pos)
dist = np.linalg.norm(pos - last_pos)
self.dist_driven += dist
def close_connection(self):
self.bng.close()
def reset(self):
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.poll_sensors(self.vehicle)
self.dist_driven = 0
self.step = 0
self.done = False
current_pos = self.vehicle.state['pos']
closest_point = self.road.closest_waypoint(current_pos)
#closest_point = self.road.random_waypoint()
self.bng.teleport_vehicle(vehicle=self.vehicle, pos=closest_point.pos(), rot=closest_point.rot())
self.bng.pause()
# TODO delete
def wait(self):
from client.aiExchangeMessages_pb2 import SimStateResponse
return SimStateResponse.SimState.RUNNING
class WCARaceGeometry(gym.Env):
sps = 50
rate = 5
front_dist = 800
front_step = 100
trail_dist = 104
trail_step = 13
starting_proj = 1710
max_damage = 100
def __init__(self, host='localhost', port=64256):
self.steps = WCARaceGeometry.sps // WCARaceGeometry.rate
self.host = host
self.port = port
self.action_space = self._action_space()
self.observation_space = self._observation_space()
self.episode_steps = 0
self.spine = None
self.l_edge = None
self.r_edge = None
self.polygon = None
front_factor = WCARaceGeometry.front_dist / WCARaceGeometry.front_step
trail_factor = WCARaceGeometry.trail_dist / WCARaceGeometry.trail_step
self.front = lambda step: +front_factor * step
self.trail = lambda step: -trail_factor * step
self.bng = BeamNGpy(self.host, self.port)
self.vehicle = Vehicle('racecar',
model='sunburst',
licence='BEAMNG',
colour='red',
partConfig='vehicles/sunburst/hillclimb.pc')
electrics = Electrics()
damage = Damage()
self.vehicle.attach_sensor('electrics', electrics)
self.vehicle.attach_sensor('damage', damage)
scenario = Scenario('west_coast_usa', 'wca_race_geometry_v0')
scenario.add_vehicle(self.vehicle,
pos=(394.5, -247.925, 145.25),
rot=(0, 0, 90))
scenario.make(self.bng)
self.bng.open(launch=True)
self.bng.set_deterministic()
self.bng.set_steps_per_second(WCARaceGeometry.sps)
self.bng.load_scenario(scenario)
self._build_racetrack()
self.observation = None
self.last_observation = None
self.last_spine_proj = None
self.bng.start_scenario()
self.bng.pause()
def __del__(self):
self.bng.close()
def _build_racetrack(self):
roads = self.bng.get_roads()
track = roads['race_ref']
l_vtx = []
s_vtx = []
r_vtx = []
for right, middle, left in track:
r_vtx.append(right)
s_vtx.append(middle)
l_vtx.append(left)
self.spine = LinearRing(s_vtx)
self.r_edge = LinearRing(r_vtx)
self.l_edge = LinearRing(l_vtx)
r_vtx = [v[0:2] for v in r_vtx]
l_vtx = [v[0:2] for v in l_vtx]
self.polygon = Polygon(l_vtx, holes=[r_vtx])
def _action_space(self):
action_lo = [-1., -1.]
action_hi = [+1., +1.]
return gym.spaces.Box(np.array(action_lo),
np.array(action_hi),
dtype=float)
def _observation_space(self):
# n vertices of left and right polylines ahead and behind, 3 floats per
# vtx
scope = WCARaceGeometry.trail_step + WCARaceGeometry.front_step
obs_lo = [
-np.inf,
] * scope * 3
obs_hi = [
np.inf,
] * scope * 3
obs_lo.extend([
-np.inf, # Distance to left edge
-np.inf, # Distance to right edge
-2 * np.pi, # Inclination
-2 * np.pi, # Angle
-2 * np.pi, # Vertical angle
-np.inf, # Spine speed
0, # RPM
-1, # Gear
0, # Throttle
0, # Brake
-1.0, # Steering
0, # Wheel speed
-np.inf, # Altitude
])
obs_hi.extend([
np.inf, # Distance to left edge
np.inf, # Distance to right edge
2 * np.pi, # Inclincation
2 * np.pi, # Angle
2 * np.pi, # Vertical angle
np.inf, # Spine speed
np.inf, # RPM
8, # Gear
1.0, # Throttle
1.0, # Brake
1.0, # Steering
np.inf, # Wheel speed
np.inf, # Altitude
])
return gym.spaces.Box(np.array(obs_lo), np.array(obs_hi), dtype=float)
def _make_commands(self, action):
brake = 0
throttle = action[1]
steering = action[0]
if throttle < 0:
brake = -throttle
throttle = 0
self.vehicle.control(steering=steering, throttle=throttle, brake=brake)
def _project_vehicle(self, pos):
r_proj = self.r_edge.project(pos)
r_proj = self.r_edge.interpolate(r_proj)
l_proj = self.l_edge.project(r_proj)
l_proj = self.l_edge.interpolate(l_proj)
s_proj = self.spine.project(r_proj)
s_proj = self.spine.interpolate(s_proj)
return l_proj, s_proj, r_proj
def _get_vehicle_angles(self, vehicle_pos, spine_seg):
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
spine_angle = np.arctan2(spine_end[1] - spine_beg[1],
spine_end[0] - spine_beg[0])
vehicle_angle = self.vehicle.state['dir'][0:2]
vehicle_angle = np.arctan2(vehicle_angle[1], vehicle_angle[0])
vehicle_angle = normalise_angle(vehicle_angle - spine_angle)
vehicle_angle -= np.pi
elevation = np.arctan2(spine_beg[2] - spine_end[2], spine_seg.length)
vehicle_elev = self.vehicle.state['dir']
vehicle_elev = np.arctan2(vehicle_elev[2],
np.linalg.norm(vehicle_elev))
return vehicle_angle, vehicle_elev, elevation
def _wrap_length(self, target):
length = self.spine.length
while target < 0:
target += length
while target > length:
target -= length
return target
def _gen_track_scope_loop(self, it, fn, base, s_scope, s_width):
for step in it:
distance = base + fn(step)
distance = self._wrap_length(distance)
s_proj = self.spine.interpolate(distance)
s_scope.append(s_proj)
l_proj = self.l_edge.project(s_proj)
l_proj = self.l_edge.interpolate(l_proj)
r_proj = self.r_edge.project(s_proj)
r_proj = self.r_edge.interpolate(r_proj)
width = l_proj.distance(r_proj)
s_width.append(width)
def _gen_track_scope(self, pos, spine_seg):
s_scope = []
s_width = []
base = self.spine.project(pos)
it = range(WCARaceGeometry.trail_step, 0, -1)
self._gen_track_scope_loop(it, self.trail, base, s_scope, s_width)
it = range(1)
self._gen_track_scope_loop(it, lambda x: x, base, s_scope, s_width)
it = range(WCARaceGeometry.front_step + 1)
self._gen_track_scope_loop(it, self.front, base, s_scope, s_width)
s_proj = self.spine.interpolate(base)
offset = (-s_proj.x, -s_proj.y, -s_proj.z)
s_line = LineString(s_scope)
s_line = affinity.translate(s_line, *offset)
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
direction = [spine_end[i] - spine_beg[i] for i in range(3)]
angle = np.arctan2(direction[1], direction[0]) + np.pi / 2
s_line = affinity.rotate(s_line,
-angle,
origin=(0, 0),
use_radians=True)
ret = list()
s_scope = s_line.coords
for idx in range(1, len(s_scope) - 1):
curvature = calculate_curvature(s_scope, idx)
inclination = calculate_inclination(s_scope, idx)
width = s_width[idx]
ret.append(curvature)
ret.append(inclination)
ret.append(width)
return ret
def _spine_project_vehicle(self, vehicle_pos):
proj = self.spine.project(vehicle_pos) - WCARaceGeometry.starting_proj
if proj < 0:
proj += self.spine.length
proj = self.spine.length - proj
return proj
def _get_spine_speed(self, vehicle_pos, vehicle_dir, spine_seg):
spine_beg = spine_seg.coords[0]
future_pos = Point(vehicle_pos.x + vehicle_dir[0],
vehicle_pos.y + vehicle_dir[1],
vehicle_pos.z + vehicle_dir[2])
spine_end = self.spine.project(future_pos)
spine_end = self.spine.interpolate(spine_end)
return spine_end.distance(Point(*spine_beg))
def _make_observation(self, sensors):
electrics = sensors['electrics']['values']
vehicle_dir = self.vehicle.state['dir']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
spine_beg = self.spine.project(vehicle_pos)
spine_end = spine_beg
spine_end += WCARaceGeometry.front_dist / WCARaceGeometry.front_step
spine_beg = self.spine.interpolate(spine_beg)
spine_end = self.spine.interpolate(spine_end)
spine_seg = LineString([spine_beg, spine_end])
spine_speed = self._get_spine_speed(vehicle_pos, vehicle_dir,
spine_seg)
l_dist = self.l_edge.distance(vehicle_pos)
r_dist = self.r_edge.distance(vehicle_pos)
angle, vangle, elevation = self._get_vehicle_angles(
vehicle_pos, spine_seg)
l_proj, s_proj, r_proj = self._project_vehicle(vehicle_pos)
s_scope = self._gen_track_scope(vehicle_pos, spine_seg)
obs = list()
obs.extend(s_scope)
obs.append(l_dist)
obs.append(r_dist)
obs.append(elevation)
obs.append(angle)
obs.append(vangle)
obs.append(spine_speed)
obs.append(electrics['rpm'])
obs.append(electrics['gearIndex'])
obs.append(electrics['throttle'])
obs.append(electrics['brake'])
obs.append(electrics['steering'])
obs.append(electrics['wheelspeed'])
obs.append(electrics['altitude'])
return np.array(obs)
def _compute_reward(self, sensors):
damage = sensors['damage']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
if damage['damage'] > WCARaceGeometry.max_damage:
return -1, True
if not self.polygon.contains(Point(vehicle_pos.x, vehicle_pos.y)):
return -1, True
score, done = -1, False
spine_proj = self._spine_project_vehicle(vehicle_pos)
if self.last_spine_proj is not None:
diff = spine_proj - self.last_spine_proj
if diff >= -0.10:
if diff < 0.5:
return -1, False
else:
score, done = diff / self.steps, False
elif np.abs(diff) > self.spine.length * 0.95:
score, done = 1, True
else:
score, done = -1, True
self.last_spine_proj = spine_proj
return score, done
def reset(self):
self.episode_steps = 0
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.restart_scenario()
self.bng.step(30)
self.bng.pause()
self.vehicle.set_shift_mode(3)
self.vehicle.control(gear=2)
sensors = self.bng.poll_sensors(self.vehicle)
self.observation = self._make_observation(sensors)
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
self.last_spine_proj = self._spine_project_vehicle(vehicle_pos)
return self.observation
def advance(self):
self.bng.step(self.steps, wait=False)
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
new_observation = self._make_observation(sensors)
return new_observation, sensors
def step(self, action):
action = [*np.clip(action, -1, 1), action[0], action[1]]
action = [float(v) for v in action]
self.episode_steps += 1
self._make_commands(action)
self.advance()
new_observation, sensors = self.observe()
if self.observation is not None:
self.last_observation = self.observation
self.observation = new_observation
score, done = self._compute_reward(sensors)
print((' A: {:5.2f} B: {:5.2f} '
' S: {:5.2f} T: {:5.2f} R: {:5.2f}').format(
action[2], action[3], action[0], action[1], score))
# if done:
# self.bng.step(WCARaceGeometry.sps * 1)
return self.observation, score, done, {}
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()
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()
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()
