import matplotlib
import numpy as np
import matplotlib.pyplot as plt
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
beamng = BeamNGpy('localhost',
64256,
home='C:/Users/merie/Documents/BeamNG.research.v1.7.0.0')
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)
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)
scenario.make(beamng)
from beamngpy import BeamNGpy, Scenario, Vehicle, StaticObject
from beamngpy.sensors import Electrics, Damage
import numpy as np
from time import sleep, time
beamng = BeamNGpy('localhost', 64256, home=r'C:\BeamNG_unlimited\trunk')
scenario = Scenario('west_coast_usa', 't_intersection_w_obj_ai')
vut = Vehicle('vut', model='coupe', licence='VUT', colour='Red')
electrics = Electrics()
damage = Damage()
vut.attach_sensor('electrics', electrics)
vut.attach_sensor('damage', damage)
scenario.add_vehicle(vut, pos=(-198.5, -164.189, 119.7), rot=(0, 0, -126.25))
obj_1 = StaticObject(
'obj_1',
pos=(-140, -121.233, 119.586),
rot=(0, 0, 55),
scale=(1, 1, 1),
shape='/levels/west_coast_usa/art/shapes/objects/barrierfence_folk.dae')
scenario.add_object(obj_1)
car_1 = Vehicle('car_1', model='etk800', licence='CAR 1', colour='Blue')
scenario.add_vehicle(car_1, pos=(-140, -121.233, 119.586), rot=(0, 0, 55))
car_2 = Vehicle('car_2', model='etk800', licence='CAR 2', colour='Blue')
scenario.add_vehicle(car_2, pos=(-140, -121.233, 119.586), rot=(0, 0, 55))
scenario.make(beamng)
bng = beamng.open(launch=True)
def launch(beamNGPath, car1, car2, speed_car2):
crash = False
dist_2car = 20
speed_car2 = int(speed_car2)
bng = BeamNGpy('localhost', 64256, beamNG_path)
#bng = BeamNGpy('localhost', 64256, home='D:/BeamNGReasearch/Unlimited_Version/trunk')
scenario = Scenario('GridMap', 'example')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle1 = Vehicle('ego_vehicle',
model='etk800',
licence='PYTHON',
color='Red')
vehicle2 = Vehicle('vehicle',
model='etk800',
licence='CRASH',
color='Blue')
electrics1 = Electrics()
electrics2 = Electrics()
vehicle1.attach_sensor('electrics1', electrics1)
vehicle2.attach_sensor('electrics2', electrics2)
#position to try drive then teleport
#-365.2436828613281, -214.7460479736328, 1.2118444442749023], [0.9762436747550964, 0.20668958127498627, 0.0650215595960617]]
#[[-362.4477844238281, -214.16226196289062, 1.32931387424469], [0.9824153780937195, 0.1852567195892334, 0.023236412554979324]]
pos2 = (25.75335693359375, -127.78406524658203, 0.2072899490594864)
pos1 = (-88.8136978149414, -261.0204162597656, 0.20253072679042816)
#pos_tel1 = (53.35311508178711, -139.84017944335938, 0.20729705691337585) #change this
#pos_tel2 = (75.94310760498047, -232.62135314941406, 0.20568031072616577) #change this
pos_tel1 = car1[0]
pos_tel2 = car2[0]
rot2 = (0.9298766851425171, -0.36776003241539, 0.009040255099534988)
rot1 = (-0.9998571872711182, 0.016821512952446938, -0.0016229493776336312)
# rot_tel1= (0.8766672611236572, -0.4810631573200226, 0.005705998744815588) #change this
# rot_tel2 = (-0.896364688873291, -0.4433068335056305, -0.0030648468527942896) #change this
rot_tel1 = car1[1]
rot_tel2 = car2[1]
#initial postion of two car.
# run 2cars on particular road until they reach particular speed which satisfies the condisiton of the given testcase
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
scenario.add_vehicle(vehicle2, pos=pos2, rot=rot2)
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle1.ai_set_speed(10, mode='set')
vehicle1.ai_set_mode('span')
vehicle2.ai_set_speed(speed_car2,
mode='set') ##change this param of speed
vehicle2.ai_set_mode('chase')
sys_output.print_sub_tit("Initial Position and rotation of car")
print("\nInitial Position and rotation of car1 %s %s " %
(pos1, rot1))
print("\nInitial Position and rotation of car2 %s %s " %
(pos2, rot2))
sys_output.print_sub_tit(
"\n when speed of car 1 rearch 10 and speed car 2 reach %s. Two cars are teleport to new locations."
% (speed_car2))
for _ in range(450):
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
vehicle2.update_vehicle()
sensors1 = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
sensors2 = bng.poll_sensors(vehicle2)
speed1 = sensors1['electrics1']['values']['wheelspeed']
speed2 = sensors2['electrics2']['values']['wheelspeed']
print("speed of car 1", speed1)
print("speed of car 2", speed2)
#print([vehicle1.state['pos'],vehicle1.state['dir']])
if speed1 > 9 and speed2 > speed_car2 - 1: #change speed here
bng.teleport_vehicle(vehicle1, pos_tel1, rot_tel1)
bng.teleport_vehicle(vehicle2, pos_tel2, rot_tel2)
sys_output.print_sub_tit("Teleport 2 car to new location")
print("\n Car1 : " + str(pos_tel1) + ", " + str(rot_tel1))
print("\n Car2 : " + str(pos_tel2) + ", " + str(rot_tel2))
print("\n Distance between two cars: " +
str(distance.euclidean(pos_tel1, pos_tel2)))
break
# if speed > 19:
# bng.teleport_vehicle(vehicle1,pos_tel,rot_tel )
# break
for _ in range(100):
#pos1 = []
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
vehicle2.update_vehicle()
sensors1 = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
sensors2 = bng.poll_sensors(vehicle2)
speed1 = sensors1['electrics1']['values']['wheelspeed']
speed2 = sensors2['electrics2']['values']['wheelspeed']
#pos1.append(vehicle1.state['pos'])
#pos2.append(vehicle2.state['pos'])
dist_2car = distance.euclidean(vehicle1.state['pos'],
vehicle2.state['pos'])
if dist_2car < 5: #or int(speed2)== 0 :
crash = True
print(
"\n Failed because distance of two cars are less than 5")
break
print("\n speed1 %s, speed2: %s, distance: %s" %
(speed1, speed2, dist_2car))
if int(speed2) == 0:
print("\n Pass because car2 stoped")
break
bng.stop_scenario()
bng.close()
finally:
bng.close()
return crash
def main(MAX_SPEED):
setup_logging()
# Gains to port TORCS actuators to BeamNG
# steering_gain = translate_steering()
acc_gain = 0.5 # 0.4
brake_gain = 1.0
# BeamNG images are too bright for DeepDrive
brightness = 0.4
# Set up first vehicle
# ! A vehicle with the name you specify here has to exist in the scenario
vehicle = Vehicle('egovehicle')
# Set up sensors
# Set up sensors
resolution = (280, 210)
# Original Settings
#pos = (-0.5, 1.8, 0.8) # Left/Right, Front/Back, Above/Below
# 0.4 is inside
pos = (0, 2.0, 0.5) # Left/Right, Front/Back, Above/Below
direction = (0, 1, 0)
# direction = (180, 0, 180)
# FOV 60, MAX_SPEED 100, 20 (3) Hz fails
# FOV 60, MAX_SPEED 80, 20 (3) Hz Ok
# FOV 60, MAX_SPEED 80, 12 Hz Ok-ish Oscillations
# FOV 60, MAX_SPEED 80, 10 Hz Ok-ish Oscillations
# FOV 40, MAX_SPEED 50, 12 Hz Seems to be fine but drives slower
# FOV 40, MAX_SPEED 80, 10 Hz Seems to be fine but drives slower
fov = 60
# MAX_SPEED = 70
MAX_FPS = 60
SIMULATION_STEP = 6
# Running the controller at 20 hz makes experiments 3 to 4 times slower ! 5 minutes of simulations end up sucking 20 minutes !
#
# WORKING SETTINGS: 20 Freq, 90 FOV.
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
electrics = Electrics()
vehicle.attach_sensor('front_cam', front_camera)
vehicle.attach_sensor('electrics', electrics)
# Setup the SHM with DeepDrive
# Create shared memory object
Memory = sd.CSharedMemory(TargetResolution=[280, 210])
# Enable Pause-Mode
Memory.setSyncMode(True)
Memory.Data.Game.UniqueRaceID = int(time.time())
print("Setting Race ID at ", Memory.Data.Game.UniqueRaceID)
# Setting Max_Speed for the Vehicle.
# TODO What's this? Maybe some hacky way to pass a parameter which is not supposed to be there...
Memory.Data.Game.UniqueTrackID = int(MAX_SPEED)
# Speed is KM/H
print("Setting speed at ", Memory.Data.Game.UniqueTrackID)
# Default for AsFault
Memory.Data.Game.Lanes = 1
# By default the AI is in charge
Memory.Data.Control.IsControlling = 1
deep_drive_engaged = True
STATE = "NORMAL"
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset ! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
# Not sure this will have any effect
Memory.indicateWrite()
Memory.waitOnRead()
if Memory.Data.Control.Breaking == 3.0 or Memory.Data.Control.Breaking == 2.0:
print("\n\n\nState not reset Again! ", Memory.Data.Control.Breaking)
Memory.Data.Control.Breaking = 0.0
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Connect to running beamng
beamng = BeamNGpy(
'localhost',
64256,
home='C://Users//Alessio//BeamNG.research_unlimited//trunk')
bng = beamng.open(launch=False)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(MAX_FPS) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle)
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
# Road interface is not available in BeamNG.research yet
# Get the road map from the level
# roads = bng.get_roads()
# # find the actual road. Dividers lane markings are all represented as roads
# theRoad = None
# for road in enumerate(roads):
# # ((left, centre, right), (left, centre, right), ...)
# # Compute the width of the road
# left = Point(road[0][0])
# right = Point(road[0][1])
# distance = left.distance( right )
# if distance < 2.0:
# continue
# else:
# theRoad = road;
# break
#
# if theRoad is None:
# print("WARNING Cannot find the main road of the map")
while True:
# Resume the execution
# 6 steps correspond to 10 FPS with a resolution of 60FPS
# 5 steps 12 FPS
# 3 steps correspond to 20 FPS
bng.step(SIMULATION_STEP)
# Retrieve sensor data and show the camera data.
sensors = bng.poll_sensors(vehicle)
# print("vehicle.state", vehicle.state)
# # TODO: Is there a way to query for the speed directly ?
speed = math.sqrt(
vehicle.state['vel'][0] * vehicle.state['vel'][0] +
vehicle.state['vel'][1] * vehicle.state['vel'][1])
# Speed is M/S ?
# print("Speed from BeamNG is: ", speed, speed*3.6)
imageData = preprocess(sensors['front_cam']['colour'], brightness)
Height, Width = imageData.shape[:2]
# print("Image size ", Width, Height)
# TODO Size of image should be right since the beginning
Memory.write(Width, Height, imageData, speed)
# Pass the computation to DeepDrive
Memory.indicateWrite()
# Wait for the control commands to send to the vehicle
# This includes a sleep and will be unlocked by writing data to it
Memory.waitOnRead()
# TODO Assumption. As long as the car is out of the road for too long this value stays up
if Memory.Data.Control.Breaking == 3.0:
if STATE != "DISABLED":
print(
"Abnormal situation detected. Disengage DeepDrive and enable BeamNG AI"
)
vehicle.ai_set_mode("manual")
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_speed(MAX_SPEED)
vehicle.ai_set_waypoint("waypoint_goal")
deep_drive_engaged = False
STATE = "DISABLED"
elif Memory.Data.Control.Breaking == 2.0:
if STATE != "GRACE":
print("Grace period. Deep Driving still disengaged")
vehicle.ai_set_mode("manual")
vehicle.ai_set_waypoint("waypoint_goal")
# vehicle.ai_drive_in_lane(True)
STATE = "GRACE"
else:
if STATE != "NORMAL":
print("DeepDrive re-enabled")
# Disable BeamNG AI driver
vehicle.ai_set_mode("disabled")
deep_drive_engaged = True
STATE = "NORMAL"
# print("State ", STATE, "Memory ",Memory.Data.Control.Breaking )
if STATE == "NORMAL":
vehicle.ai_set_mode("disabled")
# Get commands from SHM
# Apply Control - not sure cutting at 3 digit makes a difference
steering = round(
translate_steering(Memory.Data.Control.Steering), 3)
throttle = round(Memory.Data.Control.Accelerating * acc_gain,
3)
brake = round(Memory.Data.Control.Breaking * brake_gain, 3)
# Apply commands
vehicle.control(throttle=throttle,
steering=steering,
brake=brake)
#
# print("Suggested Driving Actions: ")
# print(" Steer: ", steering)
# print(" Accel: ", throttle)
# print(" Brake: ", brake)
finally:
bng.close()
def 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()
