def getStrangeScenario():
beamNGPAth = getBeamngDirectory()
beamng = BeamNGpy(
'localhost', 64256,
home=beamNGPAth) # This is the host & port used to communicate over
wallCrashScenario = Scenario('smallgrid', 'road_test')
wall = StaticObject(
name="Crash_Test_Wall",
pos=(10, 0, 0),
rot=(0, 0, 0),
scale=(1, 10, 1),
shape='/levels/smallgrid/art/shapes/misc/gm_alpha_barrier.dae')
testRoad = Road('track_editor_A_center', rid='Test_Road')
roadNode = [(-10, 0, 0, 7), (20, 0, 62)]
testRoad.nodes.extend(roadNode)
testVehicle = Vehicle('Test_Vehicule',
model='etkc',
licence='LIFLAB',
colour='Blue')
wallCrashScenario.add_road(testRoad)
wallCrashScenario.add_object(wall)
wallCrashScenario.add_vehicle(testVehicle,
pos=(0, 0, 0),
rot=(0, 180, -90))
scenarioDict = {
'beamng': beamng,
'scenario': wallCrashScenario,
'vehicule': testVehicle
}
return scenarioDict
def main():
beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
road_a = Road('custom_track_center', looped=False)
nodes = [
(0, 0, 0, 4),
(0, 400, 0, 4),
]
road_a.nodes.extend(nodes)
scenario.add_road(road_a)
vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black')
scenario.add_vehicle(vehicle, pos = ( 0, 0, 0.163609) , rot=(0,0,180)) # 0.163609 ,
scenario.make(beamng)
script = list()
node0 = {
'x': 0,
'y': 2,
'z': 0.163609,
't': 0.01,
}
node1 = {
'x': 0,
'y': 3,
'z': 0.163609,
't': 0.01,
}
node2 = {
'x': 0,
'y': 350,
'z': 0.163609,
't': 15,
}
node3 = {
'x': 0,
'y': 400,
'z': 0.163609,
't': 5,
}
script.append(node0)
script.append(node1)
script.append(node2)
script.append(node3)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_script(script)
input('Press enter when done...')
finally:
bng.close()
def main():
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('GridMap', 'road_test')
road_a = Road('track_editor_C_center', looped=True)
nodes = [
(-25, 300, 0, 5),
(25, 300, 0, 6),
(25, 350, 0, 4),
(-25, 350, 0, 5),
]
road_a.nodes.extend(nodes)
scenario.add_road(road_a)
road_b = Road('track_editor_C_center')
nodes = [
(0, 325, 0, 5),
(50, 375, 0, 5),
]
road_b.nodes.extend(nodes)
scenario.add_road(road_b)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def main():
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.0')
scenario = Scenario('GridMap', 'road_test')
road_a = Road('track_editor_C_center', rid='circle_road', looped=True)
nodes = [
(-25, 300, 0, 5),
(25, 300, 0, 6),
(25, 350, 0, 4),
(-25, 350, 0, 5),
]
road_a.nodes.extend(nodes)
scenario.add_road(road_a)
road_b = Road('track_editor_C_center', rid='center_road')
nodes = [
(0, 325, 0, 5),
(50, 375, 0, 5),
]
road_b.nodes.extend(nodes)
scenario.add_road(road_b)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def getSquareRoadScenario():
beamng = BeamNGpy('localhost', 64256, home=getBeamngDirectory()
) # This is the host & port used to communicate over
squareRoadScenario = Scenario('smallgrid', 'Straight_Foward_Test')
testRoad = Road('track_editor_C_center', rid='Test_Road', looped=True)
roadNode = [(0, 0, 0, 7), (250, 0, 0, 7), (250, 250, 0, 7), (0, 250, 0, 7)]
testRoad.nodes.extend(roadNode)
testVehicle = Vehicle('Test_Vehicule',
model='etkc',
licence='LIFLAB',
colour='Blue')
# Create an Electrics sensor and attach it to the vehicle
electrics = Electrics()
testVehicle.attach_sensor('electrics', electrics)
# Create a Damage sensor and attach it to the vehicle if module is selected
damage = Damage()
testVehicle.attach_sensor('damage', damage)
# Create a Gforce sensor and attach it to the vehicle if module is selected
gForce = GForces()
testVehicle.attach_sensor('GForces', gForce)
squareRoadScenario.add_road(testRoad)
squareRoadScenario.add_vehicle(testVehicle, pos=(0, 0, 0), rot=(0, 0, -90))
scenarioDict = {
'beamng': beamng,
'scenario': squareRoadScenario,
'vehicule': testVehicle
}
return scenarioDict
def main():
bng_home = os.environ['BEAMNG_HOME']
road = TrainingRoad(ASFAULT_PREFAB)
road.calculate_road_line(back=True)
bng = BeamNGpy('localhost', 64256, home=bng_home)
scenario = Scenario('smallgrid', 'train')
scenario.add_road(road.asphalt)
scenario.add_road(road.mid_line)
scenario.add_road(road.left_line)
scenario.add_road(road.right_line)
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
front_camera = Camera(pos=(0, 1.4, 1.8),
direction=(0, 1, -0.23),
fov=FOV,
resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
colour=True,
depth=False,
annotation=False)
vehicle.attach_sensor("front_camera", front_camera)
# Add it to our scenario at this position and rotation
spawn_point = road.spawn_point()
scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
# Place files defining our scenario for the simulator to read
scenario.make(bng)
prefab_path = scenario.get_prefab_path()
update_prefab(prefab_path)
bng.open()
bng.set_nondeterministic()
bng.set_steps_per_second(60)
# Load and start our scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('span')
vehicle.ai_set_speed(5)
vehicle.ai_set_line([{
'pos': node.pos(),
'speed': 10
} for node in road.road_line])
number_of_images = 0
while number_of_images < 9000:
bng.poll_sensors(vehicle)
number_of_images += 1
#print(number_of_images)
bng.step(1)
sensors = bng.poll_sensors(vehicle)
image = sensors['front_camera']['colour'].convert('RGB')
image = np.array(image)
image = image[:, :, ::-1]
dist = road.dist_to_center(vehicle.state['pos'])
cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image)
bng.close()
def createBeamNGLanes():
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
graph_edges = graph.edges
for sample in graph_edges:
road_a = Road('track_editor_C_border', looped=False)
road_b = Road('track_editor_C_border', looped=False)
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
lane_marking_points = getRoadEndLaneMarking(point1, point2, 4)
nodes0 = [
(lane_marking_points[0], lane_marking_points[1], -0.05, 0.05),
(lane_marking_points[4], lane_marking_points[5], -0.05, 0.05)
]
nodes1 = [
(lane_marking_points[2], lane_marking_points[3], -0.05, 0.05),
(lane_marking_points[6], lane_marking_points[7], -0.05, 0.05)
]
road_a.nodes.extend(nodes0)
road_b.nodes.extend(nodes1)
scenario.add_road(road_a)
scenario.add_road(road_b)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def getWallCrashScenario(testName):
beamng = BeamNGpy('localhost', 64256, home=getBeamngDirectory()
) # This is the host & port used to communicate over
wallCrashScenario = Scenario('smallgrid', str(testName))
wall = StaticObject(
name="Crash_Test_Wall",
pos=(420, 0, 0),
rot=(0, 0, 0),
scale=(1, 10, 75),
shape='/levels/smallgrid/art/shapes/misc/gm_alpha_barrier.dae')
testRoad = Road('track_editor_B_center', rid='Test_Road')
roadNode = [(-2, 0, 0, 7), (420, 0, 0, 7)]
testRoad.nodes.extend(roadNode)
testVehicle = Vehicle('Test_Vehicule',
model=SelectCar(),
licence='LIFLAB',
colour='Blue')
# Create an Electrics sensor and attach it to the vehicle
electrics = Electrics()
testVehicle.attach_sensor('electrics', electrics)
# Create a Damage sensor and attach it to the vehicle if module is selected
damage = Damage()
testVehicle.attach_sensor('damage', damage)
# Create a Gforce sensor and attach it to the vehicle if module is selected
gForce = GForces()
testVehicle.attach_sensor('GForces', gForce)
wallCrashScenario.add_road(testRoad)
wallCrashScenario.add_object(wall)
wallCrashScenario.add_vehicle(testVehicle, pos=(0, 0, 0), rot=(0, 0, -90))
scenarioDict = {
'beamng': beamng,
'scenario': wallCrashScenario,
'vehicule': testVehicle
}
return scenarioDict
def createBeamNGLanes():
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
graph_edges = graph.edges
for sample in graph_edges:
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
print(point1)
print(point2)
lane_marking_points = getRoadLaneMarking(point1, point2, 10, 3)
# for loop iterate to put polylines.
for polyline in lane_marking_points:
print(polyline)
road_a = Road('track_editor_C_border', looped=False)
nodes0 = [(polyline[0], polyline[1], -0.05, 0.05),
(polyline[2], polyline[3], -0.05, 0.05)]
road_a.nodes.extend(nodes0)
scenario.add_road(road_a)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def main():
path='C:/Users/marc/Desktop/BeamNG'
beamng = BeamNGpy('localhost', 64256,path)
scenario = Scenario('GridMap', 'vehicle_bbox_example')
road = Road('track_editor_C_center', rid='main_road', texture_length=5)
orig = (-107, 70, 0)
goal = (-300, 70, 0)
road.nodes = [
(*orig, 7),
(*goal, 7),
]
scenario.add_road(road)
script = [{'x': orig[0], 'y': orig[1], 'z': .3, 't': 0}]
h=0
i = 0.2
while script[h]['x'] > goal[0]:
node = {
'x': -10 * i + orig[0],
'y': 8 * np.sin(i) + orig[1],
'z': 0.3,
't': 1.5 * i,
}
script.append(node)
i += 0.2
h+=1
print(script)
vehicle = Vehicle('ego_vehicle', model='etkc', licence='PYTHON')
scenario.add_vehicle(vehicle, pos=orig)
scenario.make(beamng)
bng = beamng.open()
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_script(script)
bng.pause()
bng.step(1)
def createBeamNGRoads():
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
print(graph.edges)
graph_edges = graph.edges
print(len(graph_edges))
sample_list = []
for sample in graph_edges:
road_a = Road('track_editor_C_center', looped=False)
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
nodes0 = [(point1[0], point1[1], -4, 4), (point2[0], point2[1], -4, 4)]
road_a.nodes.extend(nodes0)
scenario.add_road(road_a)
sample_list.append(point1)
sample_list.append(point2)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def main():
beamng = BeamNGpy('localhost', 64256,getBeamngDirectory())
scenario = Scenario('smallgrid', 'road_test')
vehicle = Vehicle('LIF_Mobile', model='etkc', licence='LIFLAB', colour='Blue')
ramp = StaticObject(name='pyramp', pos=(250,0, 0), rot=(0, 0, 90), scale=(0.5, 0.5, 0.5),
shape='/levels/west_coast_usa/art/shapes/objects/ramp_massive.dae')
ring = ProceduralRing(name='pyring', pos=(380, 0, 60), rot=(0, 0, 0), radius=5, thickness=2.5)
wall= StaticObject(name="trumps_wall",pos=(420,0,0),rot=(0,0,0), scale=(1,10,75),
shape='/levels/smallgrid/art/shapes/misc/gm_alpha_barrier.dae')
road_c = Road('track_editor_B_center', rid='jump_road')
roadC_Nodes=[(-2,0,0,10),(420,0,0,7)]
road_c.nodes.extend(roadC_Nodes)
scenario.add_road(road_c)
scenario.add_procedural_mesh(ring)
scenario.add_object(ramp)
scenario.add_object(wall)
scenario.add_vehicle(vehicle,(0,0,0),(0,0,-90))
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def main():
beamng = BeamNGpy('localhost', 64256, home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
road_a = Road('custom_track_center', looped=False)
nodes = [
(0, 0, 0, 8),
(125, 0, 0, 8),
(250, -10, 0, 12)
]
road_a.nodes.extend(nodes)
scenario.add_road(road_a)
road_b = Road('custom_track_center', looped=False)
nodes = [
(125, 0, 0,12 ),
(250, -10, 0, 12)
]
road_b.nodes.extend(nodes)
scenario.add_road(road_b)
road_c = Road('custom_track_center', looped=False)
nodes = [
(125, 125, 0, 8),
(125, 0, 0, 8),
]
road_c.nodes.extend(nodes)
scenario.add_road(road_c)
vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black')
scenario.add_vehicle(vehicle, pos=(0, 0, 0.163609), rot=(0, 0, 180)) # 0.163609 ,
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def add_roads_to_scenario(self, scenario: Scenario) -> None:
from beamngpy import Road
from shapely.geometry import LineString
from scipy.interpolate import splev, splprep
from numpy.ma import arange
from numpy import repeat, linspace
from collections import defaultdict
@static_vars(rounding_precision=3)
def _interpolate_nodes(
old_x_vals: List[float], old_y_vals: List[float],
old_width_vals: List[float], num_nodes: int
) -> Tuple[List[float], List[float], List[float], List[float]]:
assert len(old_x_vals) == len(old_y_vals) == len(old_width_vals), \
"The lists for the interpolation must have the same length."
k = 1 if len(old_x_vals) <= 3 else 3
pos_tck, pos_u = splprep([old_x_vals, old_y_vals],
s=self.add_roads_to_scenario.smoothness,
k=k)
step_size = 1 / num_nodes
unew = arange(0, 1 + step_size, step_size)
new_x_vals, new_y_vals = splev(unew, pos_tck)
z_vals = repeat(0.01, len(unew))
width_tck, width_u = splprep(
[pos_u, old_width_vals],
s=self.add_roads_to_scenario.smoothness,
k=k)
_, new_width_vals = splev(unew, width_tck)
# Reduce floating point rounding errors otherwise these may cause problems with calculating parallel_offset
return [round(v, _interpolate_nodes.rounding_precision) for v in new_x_vals], \
[round(v, _interpolate_nodes.rounding_precision) for v in new_y_vals], \
z_vals, new_width_vals
for road in self.roads:
unique_nodes = []
node_pos_tracker = defaultdict(lambda: list())
for node in road.nodes:
x = node.position[0]
y = node.position[1]
if x not in node_pos_tracker or y not in node_pos_tracker[x]:
unique_nodes.append(node)
node_pos_tracker[x].append(y)
old_x_vals = [node.position[0] for node in unique_nodes]
old_y_vals = [node.position[1] for node in unique_nodes]
old_width_vals = [node.width for node in unique_nodes]
# FIXME Set interpolate=False for all roads?
main_road = Road('road_rubber_sticky', rid=road.rid)
new_x_vals, new_y_vals, z_vals, new_width_vals \
= _interpolate_nodes(old_x_vals, old_y_vals, old_width_vals, self.add_roads_to_scenario.num_nodes)
main_nodes = list(
zip(new_x_vals, new_y_vals, z_vals, new_width_vals))
main_road.nodes.extend(main_nodes)
scenario.add_road(main_road)
# FIXME Recognize changing widths --- Basic drawing works for all the roads I have testes so far,
# however strong width changes cause stair stepping, this can be countered by a smoothing parameter,
# but this itself can introduce low poly lines and inaccuracies. Better post processing or a dynamic
# sampling rate may fix this.
road_width = unique_nodes[0].width
if road.markings:
def _calculate_parallel_coords(offset: float, line_width: float) \
-> Optional[List[Tuple[float, float, float, float]]]:
original_line = LineString(zip(new_x_vals, new_y_vals))
try:
offset_line = original_line.parallel_offset(offset)
coords = offset_line.coords.xy
except (NotImplementedError,
Exception): # FIXME Where is TopologyException
_logger.exception(
"Creating an offset line for lane markings failed")
return None
# NOTE The parallel LineString may have a different number of points than initially given
num_coords = len(coords[0])
z_vals = repeat(0.01, num_coords)
marking_widths = repeat(line_width, num_coords)
return list(
zip(coords[0], coords[1], z_vals, marking_widths))
def _calculate_offset_list(relative_offset: float, absolute_offset: float,
output_number_of_points: int = self.add_roads_to_scenario.num_nodes // 3) \
-> List[float]:
""" calculates a list of relative offsets to the road centre
uses new_width_vals for dynamic offset
change the default value of output_number_of_points for more precision, has to be less \
than num_nodes//2
:param relative_offset: relative to the width of the road, must be between -0.5 and 0.5
:param absolute_offset: absolute, to account for line width
:param output_number_of_points: number of outputs in list
:return: list of width offsets
"""
assert 0 < output_number_of_points < new_width_vals.__len__() // 2, \
"choose a valid number of output vals"
assert -0.5 <= relative_offset <= 0.5, "relative offset must be between -0.5 and 0.5"
assert -max(new_width_vals) / 2 < absolute_offset < max(new_width_vals) / 2, \
"absolute offset must be smaller than half of the road"
cutting_points = linspace(0,
new_width_vals.__len__() - 1,
dtype=int,
num=output_number_of_points)
output_vals = list(
map(
lambda i: new_width_vals[i] * relative_offset +
absolute_offset, cutting_points))
return output_vals
def _calculate_parallel_pieces(offset: List[float], cutting_points: List[int]) \
-> Tuple[List[float], List[float]]:
""" This method will calculate offsets for smaller pieces of road.
uses new_x_vals and new_y_vals as baseline road
:param offset: list of width offsets, must be smaller than num_nodes//2, should be equidistant
:param cutting_points: list of points at which the road is split into pieces
:return: Returns a tuple of float lists for x and y values
"""
assert cutting_points.__len__() < self.add_roads_to_scenario.num_nodes // 2, \
"too many cutting points"
assert new_x_vals.__len__() > 1 and new_y_vals.__len__() > 1 and new_width_vals.__len__() > 1, \
"cannot work with an empty road or a point"
original_line = LineString(zip(new_x_vals, new_y_vals))
i = 0
previous_p = 0
offset_sub_lines_x = []
offset_sub_lines_y = []
for p in cutting_points:
if p > previous_p:
new_x_piece, new_y_piece = _road_piece(
offset[i],
original_line,
first_cutting_point=previous_p,
second_cutting_point=p)
offset_sub_lines_x.extend(new_x_piece)
offset_sub_lines_y.extend(new_y_piece)
previous_p = p
i += 1
return offset_sub_lines_x, offset_sub_lines_y
def _road_piece(offset: float, original_line: LineString, first_cutting_point: int,
second_cutting_point: int) \
-> Tuple[List[float], List[float]]:
""" helper method for _calculate_parallel_pieces, calculates each road piece for a certain offset
:param offset: absolute offset of the piece
:param original_line: LineString of baseline road coordinates
:param first_cutting_point: first point to split
:param second_cutting_point: second point to split
:return: returns a tuple of float lists for x and y values
"""
from shapely.errors import TopologicalError
try:
piece_coords = original_line.coords[
first_cutting_point:second_cutting_point]
road_lnstr = LineString(piece_coords).parallel_offset(
offset)
offset_sub_lines = road_lnstr.coords.xy
# shapely reverses if the offset is positive
if offset > 0:
offset_sub_lines[0].reverse()
offset_sub_lines[1].reverse()
return offset_sub_lines
except ValueError:
_logger.exception(
"Some portions of the LineString are empty")
except TopologicalError:
_logger.exception(
"Shapely cannot create a valid polygon")
def _smoothen_line(offset_sub_lines_x: List[float], offset_sub_lines_y: List[float]) \
-> Tuple[List[float], List[float]]:
""" Smoothens a line by the usage of LineString.simplify() and reduces stair stepping
:param offset_sub_lines: Tuple of float lists for x and y values
:return: Smoothed tuple of float lists for x and y values
"""
assert offset_sub_lines_x.__len__() > 1 and offset_sub_lines_y.__len__() > 1, \
"cannot smooth an empty line or a point"
point_list = list(
map(
lambda i:
(offset_sub_lines_x[i], offset_sub_lines_y[i]),
range(0,
offset_sub_lines_x.__len__() - 1)))
lstr = LineString(point_list)
lstr = lstr.simplify(tolerance=self.add_roads_to_scenario.
markings_smoothing)
return lstr.coords.xy
def _calculate_parallel_coords_list(offset: List[float], line_width: float) \
-> Optional[List[Tuple[float, float, float, float]]]:
""" calculates parallel coordinates of a road
:param offset: list of offsets, must be smaller than num_nodes//2, should be equidistant
:param line_width: specifies the width of the output coordinates
:return: coordinates for the road
"""
assert offset.__len__() < self.add_roads_to_scenario.num_nodes // 2, \
"there have to be less than half offset points of num_node for shapely LineStrings to work"
num_points = self.add_roads_to_scenario.num_nodes
# cutting points for LineString
cutting_points = linspace(
0,
num_points - 1,
dtype=int,
num=min(num_points, offset.__len__())).tolist()
# extend the last point just a bit to get all nodes
cutting_points[-1] = cutting_points[-1] + cutting_points[
-1] - cutting_points[-2]
offset_sub_lines_x, offset_sub_lines_y = _calculate_parallel_pieces(
offset, cutting_points)
coords = _smoothen_line(offset_sub_lines_x,
offset_sub_lines_y)
# NOTE The parallel LineString may have a different number of points than initially given
num_coords = len(coords[0])
z_vals = repeat(0.01, num_coords)
marking_widths = repeat(line_width, num_coords)
return list(
zip(coords[0], coords[1], z_vals, marking_widths))
# Draw side lines
side_line_offset = 1.5 * self.add_roads_to_scenario.line_width
left_side_line = Road('line_white',
rid=road.rid + "_left_line")
offset_list_line_left = _calculate_offset_list(
relative_offset=-0.5,
absolute_offset=side_line_offset * 1.5)
left_side_line_nodes = _calculate_parallel_coords_list(
offset_list_line_left,
self.add_roads_to_scenario.line_width)
if left_side_line_nodes:
left_side_line.nodes.extend(left_side_line_nodes)
scenario.add_road(left_side_line)
else:
_logger.warning("Could not create left side line")
right_side_line = Road('line_white',
rid=road.rid + "_right_line")
offset_list_line_right = _calculate_offset_list(
relative_offset=.5, absolute_offset=-side_line_offset)
right_side_line_nodes = _calculate_parallel_coords_list(
offset_list_line_right,
self.add_roads_to_scenario.line_width)
if right_side_line_nodes:
right_side_line.nodes.extend(right_side_line_nodes)
scenario.add_road(right_side_line)
else:
_logger.warning("Could not create right side line")
# Draw line separating left from right lanes
if road.left_lanes > 0 and road.right_lanes > 0:
divider_rel_off = -0.5 + road.left_lanes / (
road.left_lanes + road.right_lanes)
offset_list_divider = _calculate_offset_list(
relative_offset=divider_rel_off,
absolute_offset=-side_line_offset)
left_right_divider = Road("line_yellow_double",
rid=road.rid +
"_left_right_divider")
left_right_divider_nodes \
= _calculate_parallel_coords_list(offset_list_divider,
2 * self.add_roads_to_scenario.line_width)
if left_right_divider_nodes:
left_right_divider.nodes.extend(
left_right_divider_nodes)
scenario.add_road(left_right_divider)
else:
_logger.warning(
"Could not create line separating lanes having different directions"
)
# Draw lines separating left and right lanes from each other
total_num_lane_markings = road.left_lanes + road.right_lanes
offsets_dashed = linspace(-0.5,
0.5,
num=total_num_lane_markings,
endpoint=False).tolist()
offsets_dashed = offsets_dashed[1:len(offsets_dashed)]
# do not draw dashed line over divider line
if road.left_lanes > 0 and road.right_lanes > 0:
offsets_dashed.remove(offsets_dashed[road.left_lanes - 1])
# add each separating line
for offs in offsets_dashed:
# '.' have to be removed from the name, else there are prefab parsing errors for positive offsets
lane_separation_line = Road('line_dashed_short',
rid=road.rid + "_separator_" +
str(offs).replace('.', ''))
offs_list = _calculate_offset_list(offs, 0)
lane_separation_line_nodes \
= _calculate_parallel_coords_list(offs_list, self.add_roads_to_scenario.line_width)
if lane_separation_line_nodes:
lane_separation_line.nodes.extend(
lane_separation_line_nodes)
scenario.add_road(lane_separation_line)
else:
_logger.warning(
"Could not create line separating lanes having the same direction"
)
road_a = Road('custom_track_center', looped=False)
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
#print(getDistance(point1,point2))
nodes0 = [
(
point1[0], point1[1], 0, 8
), # method to get the road width from elastic search or number of lanes. (forward and backward)
(point2[0], point2[1], 0, 8)
]
road_a.nodes.extend(nodes0)
scenario.add_road(road_a)
vehicleStriker = Vehicle('striker',
model='etk800',
licence='Striker',
colour='Yellow')
damageStriker = Damage()
vehicleStriker.attach_sensor('damagesS', damageStriker)
vehicleVictim = Vehicle('victim',
model='etk800',
licence='Victim',
colour='White')
damageVictim = Damage()
vehicleVictim.attach_sensor('damagesV', damageVictim)
# road_a.nodes.extend(nodes0)
# scenario.add_road(road_a)
# Temporary road.
roads = Road('track_editor_C_center', looped=False)
nodesA = [(road_a[0][0], road_a[0][1], -4, 4),
(road_a[1][0], road_a[1][1], -4, 4)]
nodesB = [(road_b[0][0], road_b[0][1], -4, 4),
(road_b[1][0], road_b[1][1], -4, 4)]
nodesC = [(road_c[0][0], road_c[0][1], -4, 4),
(road_c[1][0], road_c[1][1], -4, 4)]
roads.nodes.extend(nodesA)
roads.nodes.extend(nodesB)
roads.nodes.extend(nodesC)
scenario.add_road(roads)
def AngleBtw2Points(pointA, pointB):
changeInX = pointB[0] - pointA[0]
changeInY = pointB[1] - pointA[1]
return degrees(
atan2(changeInY,
changeInX)) #remove degrees if you want your answer in radians
def getPolyLineCoordinates(node_a, node_b, distance, width):
#print("get polyline coordinate")
# Assumption. width from the center of the road.
real_distance = getDistance(node_a, node_b)
t = distance / real_distance
from matplotlib.pyplot import imshow
from PIL import Image
from shapely.geometry import Polygon
from beamngpy import BeamNGpy, Vehicle, Scenario, Road
from beamngpy.sensors import Camera
from getAIScript import getAIScript
beamng = BeamNGpy('localhost', 64256, getBeamngDirectory())
scenario = Scenario('smallgrid', 'vehicle_bbox_example')
road = Road('track_editor_A_center', rid='main_road')
orig = (0, -2, 0)
goal = (1150, -22, 0)
nodes = [(*orig, 7), (*goal, 7)]
road.nodes.extend(nodes)
scenario.add_road(road)
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
overhead = Camera((0, -10, 5), (0, 1, -0.75), 60, (1024, 1024))
vehicle.attach_sensor('overhead', overhead)
scenario.add_vehicle(vehicle, pos=orig, rot=(0, 0, -90))
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
script = getAIScript()
vehicle.ai_set_script(script)
while (True):
vehicle.update_vehicle()
def createBeamNGRoads():
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
print(graph.edges)
graph_edges = graph.edges
print(len(graph_edges))
sample_list = []
for sample in graph_edges:
# road creation
road_a = Road('track_editor_C_center', looped=False)
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
nodes0 = [(point1[0], point1[1], -4, 4), (point2[0], point2[1], -4, 4)]
road_a.nodes.extend(nodes0)
scenario.add_road(road_a)
sample_list.append(point1)
sample_list.append(point2)
# road edge lane marking
road_b = Road('track_editor_C_border',
render_priority=10,
looped=False)
road_c = Road('track_editor_C_border',
render_priority=10,
looped=False)
point1 = list(beamng_dict[sample[0]])
point2 = list(beamng_dict[sample[1]])
lane_marking_points = getRoadEndLaneMarking(point1, point2, 4)
nodes0 = [
(lane_marking_points[0], lane_marking_points[1], -0.05, 0.05),
(lane_marking_points[4], lane_marking_points[5], -0.05, 0.05)
]
nodes1 = [
(lane_marking_points[2], lane_marking_points[3], -0.05, 0.05),
(lane_marking_points[6], lane_marking_points[7], -0.05, 0.05)
]
road_b.nodes.extend(nodes0)
road_c.nodes.extend(nodes1)
scenario.add_road(road_b)
scenario.add_road(road_c)
# road lane marking
lane_marking_points = getRoadLaneMarking(point1, point2, 10, 3)
# for loop iterate to put polylines.
for polyline in lane_marking_points:
#print(polyline)
road_d = Road('track_editor_C_border',
render_priority=10,
looped=False)
nodes0 = [(polyline[0], polyline[1], -0.05, 0.05),
(polyline[2], polyline[3], -0.05, 0.05)]
road_d.nodes.extend(nodes0)
scenario.add_road(road_d)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
input('Press enter when done...')
finally:
bng.close()
def getDonutScenario():
beamng = BeamNGpy('localhost', 64256, home=getBeamngDirectory()
) # This is the host & port used to communicate over
donutScenario = Scenario('smallgrid', 'road_test')
concreteWallSide1 = StaticObject(
name="Crash_Test_Wall",
pos=(20, 10, 0),
rot=(0, 0, 0),
scale=(10, 1, 1),
shape=
'/levels/driver_training/art/shapes/race/concrete_road_barrier_a.dae')
concreteWallSide2 = StaticObject(
name="Crash_Test_Wall2",
pos=(35, -5, 0),
rot=(0, 0, 90),
scale=(10, 1, 1),
shape=
'/levels/driver_training/art/shapes/race/concrete_road_barrier_a.dae')
concreteWallSide3 = StaticObject(
name="Crash_Test_Wall3",
pos=(20, -20, 0),
rot=(0, 0, 0),
scale=(10, 1, 1),
shape=
'/levels/driver_training/art/shapes/race/concrete_road_barrier_a.dae')
concreteWallSide4 = StaticObject(
name="Crash_Test_Wall4",
pos=(5, -5, 0),
rot=(0, 0, 90),
scale=(10, 1, 1),
shape=
'/levels/driver_training/art/shapes/race/concrete_road_barrier_a.dae')
testRoad = Road('track_editor_C_center', rid='Test_Road')
roadNode = [(*(-25, 25, 0), 45), (*(15, 25, 0), 45)]
testRoad.nodes.extend(roadNode)
testVehicle = Vehicle('Test_Vehicule',
model='etkc',
licence='LIFLAB',
colour='Blue')
# Create an Electrics sensor and attach it to the vehicle
electrics = Electrics()
testVehicle.attach_sensor('electrics', electrics)
# Create a Damage sensor and attach it to the vehicle if module is selected
damage = Damage()
testVehicle.attach_sensor('damage', damage)
# Create a Gforce sensor and attach it to the vehicle if module is selected
gForce = GForces()
testVehicle.attach_sensor('GForces', gForce)
donutScenario.add_vehicle(testVehicle, pos=(20, 0, 0), rot=(0, 0, 0))
donutScenario.add_object(concreteWallSide1)
donutScenario.add_object(concreteWallSide2)
donutScenario.add_object(concreteWallSide3)
donutScenario.add_object(concreteWallSide4)
donutScenario.add_road(testRoad)
scenarioDict = {
'beamng': beamng,
'scenario': donutScenario,
'vehicule': testVehicle
}
return scenarioDict
def main():
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_test')
road_a = Road('custom_track_center', looped=False)
nodes = [
(0, 0, 0, 4),
(0, 400, 0, 4),
]
road_a.nodes.extend(nodes)
scenario.add_road(road_a)
vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Black')
scenario.add_vehicle(vehicle, pos=(0, 0, 0.163609),
rot=(0, 0, 180)) # 0.163609 ,
scenario.make(beamng)
script = list()
node0 = {
'pos': (0, 0, 0.163609),
'speed': 20,
}
node1 = {
'pos': (0, 100, 0.163609),
'speed': 30,
}
script.append(node0)
script.append(node1)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_line(script)
# update the state of vehicle at impact.
for _ in range(100):
time.sleep(0.1)
vehicle.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle
) # Polls the data of all sensors attached to the vehicle
print(vehicle.state['pos'])
if vehicle.state['pos'][1] > 99:
print('free state')
vehicle.control(throttle=0,
steering=0,
brake=0,
parkingbrake=0)
vehicle.update_vehicle()
bng.stop_scenario()
for _ in range(20):
time.sleep(0.1)
bng.load_scenario(scenario)
bng.start_scenario()
node0 = {
'pos': (0, 50, 0.163609),
'speed': 20,
}
node1 = {
'pos': (0, 100, 0.163609),
'speed': 30,
}
script.append(node0)
script.append(node1)
vehicle.ai_set_line(script)
input('Press enter when done...')
finally:
bng.close()
def main():
"""
Generate a bunch of images by driving along a predefined sequence of points, while capturing camera images
to JPG files.
:return: (None)
"""
bng_home = os.environ['BEAMNG_HOME']
road = TrainingRoad(ASFAULT_PREFAB)
road.calculate_road_line(back=True)
bng = BeamNGpy('localhost', 64256, home=bng_home)
scenario = Scenario('smallgrid', 'train')
# Asphalt and lines are actually considered as differently colored roads by beamngpy.
scenario.add_road(road.asphalt)
scenario.add_road(road.mid_line)
scenario.add_road(road.left_line)
scenario.add_road(road.right_line)
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
# Create a dash cam that is somewhat down-sloped.
front_camera = Camera(pos=(0, 1.4, 1.8),
direction=(0, 1, -0.23),
fov=FOV,
resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
colour=True,
depth=False,
annotation=False)
vehicle.attach_sensor("front_camera", front_camera)
# Get a spawn point and initial rotation of the vehicle.
spawn_point = road.spawn_point()
scenario.add_vehicle(vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
# Place files defining our scenario for the simulator to read.
scenario.make(bng)
prefab_path = scenario.get_prefab_path()
update_prefab(prefab_path)
bng.open()
bng.set_nondeterministic()
bng.set_steps_per_second(60)
# Load and start our scenario
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_mode('span')
vehicle.ai_set_speed(5)
vehicle.ai_set_line([{
'pos': node.pos(),
'speed': 10
} for node in road.road_line])
number_of_images = 0
while number_of_images < NUMBER_OF_IMAGES:
bng.poll_sensors(vehicle)
number_of_images += 1
bng.step(1)
sensors = bng.poll_sensors(vehicle)
image = sensors['front_camera']['colour'].convert('RGB')
image = np.array(image)
image = image[:, :, ::-1]
dist = road.dist_to_center(vehicle.state['pos'])
cv2.imwrite('datasets\\{}.jpg'.format(number_of_images), image)
bng.close()
def GenerateTrack(trackLength, sampleRate, show, startBeamNG):
populationSize = 100
maxAcc = 1
times = 20
relNewSize = 0.6
duplicatesThreshold = 0.05
intersectionDelta = 0.01
mutationProb = 0.1
mutationDeviation = 0.01
print("Generating Tracks")
pop = initPop(populationSize, trackLength, maxAcc, 20)
pop = evolve(pop, times, relNewSize, duplicatesThreshold,
intersectionDelta, mutationProb, mutationDeviation, 10)
print("eliminating intersecting tracks")
pop = elminateIntersecting(pop, intersectionDelta)
if show:
plotTrack(pop, 100)
tracks = []
nr = 0
first = True
for track in pop:
track = np.vstack((track, completeAcc(track)))
poly = polysFromAccs(track)
bez = bezFromPolys(poly)
smpl = sampleBezSeries(bez, sampleRate).transpose()
smpl = scaleTrack(smpl, 100, 100)
smpl = np.array(smpl)
smpl = np.vstack((smpl, [smpl[0]]))
if(first):
writeCriteria(smpl)
first = False
tracks.append(smpl)
writeTrack(smpl, nr)
nr += 1
if startBeamNG:
nodes = []
for p in tracks[0]: nodes.append((p[0], p[1], 0, 7))
beamng = BeamNGpy('localhost', 64256)
vehicle = Vehicle('ego', model='etk800', licence='PYTHON', colour='Green')
scenario = Scenario('GridMap_v0422', 'track test')
scenario.add_vehicle(vehicle, pos=(0, 0, -16.64), rot=(0, 0, 180))
road = Road(material='track_editor_C_center', rid='main_road', texture_length=5, looped=True)
road.nodes.extend(nodes)
scenario.add_road(road)
scenario.make(beamng)
beamng.open()
beamng.load_scenario(scenario)
beamng.start_scenario()
vehicle.ai_set_mode('span')
while 1:
vehicle.update_vehicle()
print(vehicle.state['pos'])
sleep(1)
class Simulation(object):
def __init__(self) -> None:
super().__init__()
thread = Thread(target=self._intervene)
thread.start()
self.step = 0
self.dist_driven = 0
self.done = False
self.last_action = (0.0, 0.0)
self.bng = BeamNGpy('localhost', 64257, home=BEAMNG_HOME)
self.scenario = Scenario('train', 'train', authors='Vsevolod Tymofyeyev',
description='Reinforcement learning')
self.road = TrainingRoad(ASFAULT_PREFAB)
self.road.calculate_road_line()
spawn_point = self.road.spawn_point()
self.last_pos = spawn_point.pos()
self.scenario.add_road(self.road.asphalt)
self.scenario.add_road(self.road.mid_line)
self.scenario.add_road(self.road.left_line)
self.scenario.add_road(self.road.right_line)
self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='RL FTW', color='Red')
front_camera = Camera(pos=(0, 1.4, 1.8), direction=(0, 1, -0.23), fov=FOV,
resolution=(CAMERA_WIDTH, CAMERA_HEIGHT),
colour=True, depth=False, annotation=False)
self.vehicle.attach_sensor("front_camera", front_camera)
self.scenario.add_vehicle(self.vehicle, pos=spawn_point.pos(), rot=spawn_point.rot())
self.scenario.make(self.bng)
prefab_path = self.scenario.get_prefab_path()
update_prefab(prefab_path)
self.bng.open()
self.bng.set_deterministic()
self.bng.set_steps_per_second(SPS)
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
# self.bng.hide_hud()
self.bng.pause()
def _intervene(self):
while True:
a = input()
self.done = not self.done
def take_action(self, action):
steering, throttle = action
steering = steering.item()
throttle = throttle.item()
self.last_action = action
self.step += 1
self.vehicle.control(steering=steering, throttle=throttle, brake=0.0)
self.bng.step(STEPS_INTERVAL)
def _reward(self, done, dist):
steering = self.last_action[0]
throttle = self.last_action[1]
velocity = self.velocity() # km/h
if not done:
reward = REWARD_STEP + THROTTLE_REWARD_WEIGHT * throttle #- MID_DIST_PENALTY_WEIGHT * dist
else:
reward = REWARD_CRASH - CRASH_SPEED_WEIGHT * throttle
return reward
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
image = sensors['front_camera']['colour'].convert("RGB")
image = np.array(image)
r = ROI
# Cut to the relevant region
image = image[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])]
# Convert to BGR
state = image[:, :, ::-1]
#done = self.done
pos = self.vehicle.state['pos']
dir = self.vehicle.state['dir']
self.refresh_dist(pos)
self.last_pos = pos
dist = self.road.dist_to_center(self.last_pos)
#velocity = self.velocity()
done = dist > MAX_DIST #or velocity > MAX_VELOCITY
reward = self._reward(done, dist)
return state, reward, done, {}
def velocity(self):
state = self.vehicle.state
velocity = np.linalg.norm(state["vel"])
return velocity * 3.6
def position(self):
return self.vehicle.state["pos"]
def refresh_dist(self, pos):
pos = np.array(pos)
last_pos = np.array(self.last_pos)
dist = np.linalg.norm(pos - last_pos)
self.dist_driven += dist
def close_connection(self):
self.bng.close()
def reset(self):
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.poll_sensors(self.vehicle)
self.dist_driven = 0
self.step = 0
self.done = False
current_pos = self.vehicle.state['pos']
closest_point = self.road.closest_waypoint(current_pos)
#closest_point = self.road.random_waypoint()
self.bng.teleport_vehicle(vehicle=self.vehicle, pos=closest_point.pos(), rot=closest_point.rot())
self.bng.pause()
# TODO delete
def wait(self):
from client.aiExchangeMessages_pb2 import SimStateResponse
return SimStateResponse.SimState.RUNNING
def createCrashSimulation():
print("Crash Simulation")
beamng = BeamNGpy('localhost',
64256,
home='F:\Softwares\BeamNG_Research_SVN')
scenario = Scenario('GridMap', 'road_crash_simulation')
road_a = Road('track_editor_C_center', looped=False)
road_b = Road('track_editor_C_center', looped=False)
nodesa = [(30, 0, -4, 4), (0, 0, -4, 4)]
nodesb = [(0, 30, -4, 4), (0, 0, -4, 4)]
road_a.nodes.extend(nodesa)
road_b.nodes.extend(nodesb)
# Create an ETK800 with the licence plate 'PYTHON'
vehicleA = Vehicle('ego_vehicleA', model='etk800', licence='PYTHON')
# Add it to our scenario at this position and rotation
damageS = Damage()
vehicleA.attach_sensor('damagesS', damageS)
scenario.add_vehicle(vehicleA, pos=(30, 0, 0), rot=(0, 0, 90))
# Create an ETK800 with the licence plate 'PYTHON'
vehicleB = Vehicle('ego_vehicleB', model='etk800', licence='PYTHON')
# Add it to our scenario at this position and rotation
damageV = Damage()
vehicleB.attach_sensor('damagesV', damageV)
scenario.add_vehicle(vehicleB, pos=(0, 30, 0), rot=(0, 0, 0))
scenario.add_road(road_a)
scenario.add_road(road_b)
scenario.make(beamng)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
node0 = {
'pos': (30, 0, 0),
'speed': 0,
}
node1 = {
'pos': (0, 0, 0),
'speed': 20,
}
script = list()
script.append(node0)
script.append(node1)
vehicleA.ai_set_line(script)
node3 = {
'pos': (0, 30, 0),
'speed': 0,
}
node4 = {
'pos': (0, 0, 0),
'speed': 20,
}
script = list()
script.append(node3)
script.append(node4)
vehicleB.ai_set_line(script)
time.sleep(12)
# input('Press enter when done...')
vehicleA.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleA)
damage_striker = sensors['damagesS']
print(sensors['damagesS'])
print(vehicleA.state['pos'])
vehicleB.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleB)
damage_victim = sensors['damagesV']
print(sensors['damagesV'])
print(vehicleB.state['pos'])
multiObjectiveFitnessFunction(123456789, damage_striker,
vehicleA.state['pos'], (0, 0),
damage_victim, vehicleB.state['pos'],
(0, 0))
# multiobjective fitness function.
bng.stop_scenario()
# bng.load_scenario(scenario)
# bng.start_scenario()
#
# print("sleep")
# time.sleep(10)
# print("wake")
#
# node0 = {
# 'pos': (30, 0, 0),
# 'speed': 0,
# }
#
# node1 = {
# 'pos': (0, 0, 0),
# 'speed': 30,
# }
#
# script = list()
# script.append(node0)
# script.append(node1)
#
# vehicleA.ai_set_line(script)
#
# node0 = {
# 'pos': (0, 30, 0),
# 'speed': 0,
# }
#
# node1 = {
# 'pos': (0, 0, 0),
# 'speed': 30,
# }
#
# script = list()
# script.append(node0)
# script.append(node1)
#
# vehicleB.ai_set_line(script)
#
# input('Press enter when done...')
finally:
bng.close()
def main() -> None:
# Read CommonRoad scenario
cr_scenario, cr_planning_problem_set = CommonRoadFileReader(cr_scenario_path) \
.open()
if cr_planning_problem_set:
for _, pp in cr_planning_problem_set.planning_problem_dict.items():
cr_planning_problem = pp
break # Read only the first one
else:
raise Exception(
"The given CommonRoad scenario does not define a planning problem."
)
# Setup BeamNG
bng = BeamNGpy('localhost', 64256, home=home_path, user=user_path)
bng_scenario = Scenario(
bng_scenario_environment,
bng_scenario_name,
authors='Stefan Huber',
description='Simple visualization of the CommonRoad scenario ' +
cr_scenario_name)
# Add lane network
lanes = cr_scenario.lanelet_network.lanelets
for lane in lanes:
lane_nodes = []
for center in lane.center_vertices:
lane_nodes.append((center[0], center[1], 0,
4)) # FIXME Determine appropriate width
road = Road(cr_road_material)
road.nodes.extend(lane_nodes)
bng_scenario.add_road(road)
# Add ego vehicle
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Cornflowerblue')
ego_init_state = cr_planning_problem.initial_state
ego_init_state.position[0] = 82.8235
ego_init_state.position[1] = 31.5786
add_vehicle_to_bng_scenario(bng_scenario, ego_vehicle, ego_init_state,
etk800_z_offset)
obstacles_to_move = dict()
# Add truck
semi = Vehicle('truck', model='semi', color='Red')
semi_init_state = cr_scenario.obstacle_by_id(206).initial_state
add_vehicle_to_bng_scenario(bng_scenario, semi, semi_init_state,
semi_z_offset)
obstacles_to_move[206] = semi
# Add truck trailer
tanker_init_state = copy(semi_init_state)
tanker_init_state.position += [6, 3]
add_vehicle_to_bng_scenario(
bng_scenario, Vehicle('truck_trailer', model='tanker', color='Red'),
tanker_init_state, tanker_z_offset)
# Add other traffic participant
opponent = Vehicle('opponent',
model='etk800',
licence='VS',
color='Cornflowerblue')
add_vehicle_to_bng_scenario(bng_scenario, opponent,
cr_scenario.obstacle_by_id(207).initial_state,
etk800_z_offset)
obstacles_to_move[207] = opponent
# Add static obstacle
obstacle_shape: Polygon = cr_scenario.obstacle_by_id(399).obstacle_shape
obstacle_pos = obstacle_shape.center
obstacle_rot_deg = rad2deg(semi_init_state.orientation) + rot_offset
obstacle_rot_rad = semi_init_state.orientation + deg2rad(rot_offset)
for w in range(3):
for h in range(3):
for d in range(2):
obstacle = Vehicle('obstacle' + str(w) + str(h) + str(d),
model='haybale',
color='Red')
haybale_pos_diff = obstacle_pos \
+ pol2cart(1.3 * d, obstacle_rot_rad + pi / 4) \
+ pol2cart(2.2 * w, pi / 2 - obstacle_rot_rad)
bng_scenario.add_vehicle(obstacle,
pos=(haybale_pos_diff[0],
haybale_pos_diff[1], h * 1),
rot=(0, 0, obstacle_rot_deg))
bng_scenario.make(bng)
# Manually set the overObjects attribute of all roads to True (Setting this option is currently not supported)
prefab_path = os.path.join(user_path, 'levels', bng_scenario_environment,
'scenarios', bng_scenario_name + '.prefab')
lines = open(prefab_path, 'r').readlines()
for i in range(len(lines)):
if 'overObjects' in lines[i]:
lines[i] = lines[i].replace('0', '1')
open(prefab_path, 'w').writelines(lines)
# Start simulation
bng.open(launch=True)
try:
bng.load_scenario(bng_scenario)
bng.start_scenario()
bng.pause()
bng.display_gui_message(
"The scenario is fully prepared and paused. You may like to position the camera first."
)
delay_to_resume = 15
input("Press enter to continue the simulation... You have " +
str(delay_to_resume) +
" seconds to switch back to the BeamNG window.")
sleep(delay_to_resume)
bng.resume()
for id, obstacle in obstacles_to_move.items():
obstacle.ai_drive_in_lane(False)
obstacle.ai_set_line(
generate_node_list(cr_scenario.obstacle_by_id(id)))
ego_vehicle.ai_drive_in_lane(False)
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='limit')
speed = 65 / 3.6
ego_vehicle.ai_set_line([{
'pos': ego_vehicle.state['pos']
}, {
'pos': (129.739, 56.907, etk800_z_offset),
'speed': speed
}, {
'pos': (139.4, 62.3211, etk800_z_offset),
'speed': speed
}, {
'pos': (149.442, 64.3655, etk800_z_offset),
'speed': speed
}, {
'pos': (150.168, 63.3065, etk800_z_offset),
'speed': speed
}, {
'pos': (188.495, 78.8517, etk800_z_offset),
'speed': speed
}])
# FIXME Setting the speed does not work as expected
# ego_vehicle.ai_set_speed(cr_planning_problem.initial_state.velocity * 3.6, mode='set')
input("Press enter to end simulation...")
finally:
bng.close()
