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():
setup_logging()
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('west_coast_usa', 'ai_sine')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='AI')
orig = (-769.1, 400.8, 142.8)
scenario.add_vehicle(vehicle, pos=orig, rot=None, rot_quat=(0, 0, 1, 0))
scenario.make(beamng)
script = list()
for i in range(3600):
node = {
# Calculate the position as a sinus curve that makes the vehicle
# drive from left to right. The z-coordinate is not calculated in
# any way because `ai_set_script` by default makes the polyline to
# follow cling to the ground, meaning the z-coordinate will be
# filled in automatically.
'x': 4 * np.sin(np.radians(i)) + orig[0],
'y': i * 0.2 + orig[1],
'z': orig[2],
# Calculate timestamps for each node such that the speed between
# points has a sinusoidal variance to it.
't': (2 * i + (np.abs(np.sin(np.radians(i)))) * 64) / 64,
}
script.append(node)
bng = beamng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_set_script(script)
while True:
bng.step(60)
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)
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()
print(vehicle.state['pos'])
input()
finally:
bng.close()
import mmap
def test_vehicle_ai(beamng):
with beamng as bng:
bng.set_deterministic()
scenario = Scenario('west_coast_usa', 'ai_test')
vehicle = Vehicle('test_car', model='etk800')
other = Vehicle('other', model='etk800')
pos = [-717.121, 101, 118.675]
scenario.add_vehicle(vehicle, pos=pos, rot=(0, 0, 45))
scenario.add_vehicle(other, pos=(-453, 700, 75), rot=(0, 0, 45))
scenario.make(bng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.pause()
vehicle.ai_set_mode('span')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_waypoint('Bridge4_B')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_target('other', mode='chase')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
vehicle.ai_set_target('other', mode='flee')
assert_continued_movement(bng, vehicle, pos)
bng.restart_scenario()
bng.pause()
script = [
{
'x': -735,
'y': 86.7,
'z': 119,
't': 0
},
{
'x': -752,
'y': 70,
'z': 119,
't': 5
},
{
'x': -762,
'y': 60,
'z': 119,
't': 8
},
]
vehicle.ai_set_script(script)
bng.step(600, wait=True)
vehicle.update_vehicle()
ref = [script[1]['x'], script[1]['y'], script[1]['z']]
pos = vehicle.state['pos']
ref, pos = np.array(ref), np.array(pos)
assert np.linalg.norm(ref - pos) < 2.5
scenario.delete(beamng)
scenario.make(beamng)
bng = beamng.open(launch=True)
bng.set_deterministic()
try:
bng.load_scenario(scenario)
bng.start_scenario()
bng.add_debug_line(striker_path['points'], striker_path['point_colors'],
spheres=striker_path['spheres'], sphere_colors=striker_path['sphere_colors'],
cling=True, offset=0.1)
bng.add_debug_line(victim_path['points'], victim_path['point_colors'],
spheres=victim_path['spheres'], sphere_colors=victim_path['sphere_colors'],
cling=True, offset=0.1)
vehicleStriker.ai_set_script(striker_path['script'])
vehicleVictim.ai_set_script(victim_path['script'])
for number in range(60):
time.sleep(0.20)
vehicleStriker.update_vehicle() # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleStriker) # Polls the data of all sensors attached to the vehicle
# print(vehicleStriker.state['pos'])
if vehicleStriker.state['pos'][0] > 236 and vehicleStriker.state['pos'][1] > 141:
# print('free state')
vehicleStriker.control(throttle=0, steering=0, brake=0, parkingbrake=0)
vehicleStriker.update_vehicle()
vehicleVictim.update_vehicle() # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicleVictim) # Polls the data of all sensors attached to the vehicle
def run_scenario_ai_version(vehicle_model='etk800',
deflation_pattern=[0, 0, 0, 0],
parts_config='vehicles/hopper/custom.pc'):
global base_filename, default_color, default_scenario, default_spawnpoint, setpoint, steps_per_sec, prev_error
random.seed(1703)
setup_logging()
home = 'H:/BeamNG.research.v1.7.0.1clean' #'H:/BeamNG.research.v1.7.0.1untouched/BeamNG.research.v1.7.0.1' #'H:/BeamNG.tech.v0.21.3.0' #
beamng = BeamNGpy('localhost', 64256,
home=home) #, user='******')
scenario = Scenario(default_scenario, 'research_test')
vehicle = Vehicle('ego_vehicle',
model=vehicle_model,
licence='AI',
color=default_color)
vehicle = setup_sensors(vehicle)
spawn = spawn_point(default_scenario, default_spawnpoint)
scenario.add_vehicle(vehicle,
pos=spawn['pos'],
rot=None,
rot_quat=spawn['rot_quat'])
# Compile the scenario and place it in BeamNG's map folder
scenario.make(beamng)
# Start BeamNG and enter the main loop
bng = beamng.open(launch=True)
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(steps_per_sec) # With 100hz temporal resolution
# Load and start the scenario
bng.load_scenario(scenario)
bng.start_scenario()
# create vehicle to be chased
# chase_vehicle = Vehicle('chase_vehicle', model='miramar', licence='CHASEE', color='Red')
# bng.spawn_vehicle(chase_vehicle, pos=(469.784, 346.391, 144.982), rot=None,
# rot_quat=(-0.0037852677050978, -0.0031219546217471, -0.78478640317917, 0.61974692344666))
# bng.spawn_vehicle(vehicle, pos=spawn['pos'], rot=None, rot_quat=spawn['rot_quat'], partConfig=parts_config)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
bng.resume()
ai_line, bng = create_ai_line_from_road(spawn, bng)
# ai_line, bng = create_ai_line_from_centerline(bng)
# ai_line, bng = create_ai_line(bng)
vehicle.ai_set_script(ai_line, cling=True)
pitch = vehicle.state['pitch'][0]
roll = vehicle.state['roll'][0]
z = vehicle.state['pos'][2]
image = bng.poll_sensors(vehicle)['front_cam']['colour'].convert('RGB')
# bng.resume()
# vehicle.ai_set_mode('chase')
# vehicle.ai_set_target('chase_vehicle')
# vehicle.ai_set_mode("traffic")
# vehicle.ai_set_speed(12, mode='set')
# vehicle.ai_drive_in_lane(True)
damage_prev = None
runtime = 0.0
traj = []
kphs = []
# with open("ai_lap_data.txt", 'w') as f:
for _ in range(1024):
sensors = bng.poll_sensors(vehicle)
image = sensors['front_cam']['colour'].convert('RGB')
damage = sensors['damage']
wheelspeed = sensors['electrics']['wheelspeed']
new_damage = diff_damage(damage, damage_prev)
damage_prev = damage
runtime = sensors['timer']['time']
vehicle.update_vehicle()
traj.append(vehicle.state['pos'])
# f.write("{}\n".format(vehicle.state['pos']))
kphs.append(ms_to_kph(wheelspeed))
if new_damage > 0.0:
break
# if distance(spawn['pos'], vehicle.state['pos']) < 3 and sensors['timer']['time'] > 90:
# reached_start = True
# plt.imshow(image)
# plt.show()
# break
bng.step(1)
bng.close()
plot_trajectory(traj, "AI Lap")
results = {
'runtime': round(runtime, 3),
'damage': damage,
'kphs': kphs,
'traj': traj,
'pitch': round(pitch, 3),
'roll': round(roll, 3),
"z": round(z, 3),
'final_img': image
}
return results
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()
