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)
scenario.delete(beamng)
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)
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
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 getOob(oobList, test, port):
# 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')
scenario = Scenario('asfault', 'asfault', True)
# 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)
with open(pathToScenarioBeamng + '/asfault.json', 'r') as f:
jsonScenario = json.load(f)
with open(pathToScenarioBeamng + '/asfault.lua', 'r') as f:
luaScenario = f.readlines()
with open(pathToScenarioBeamng + '/asfault.prefab', 'r') as f:
prefabScenario = f.readlines()
with open(pathToScenarioDocuments + '/asfault.json', 'w') as outfile:
json.dump(jsonScenario, outfile)
with open(pathToScenarioDocuments + '/asfault.lua', 'w') as outfile:
outfile.writelines(luaScenario)
with open(pathToScenarioDocuments + '/asfault.prefab', 'w') as outfile:
outfile.writelines(prefabScenario)
deep_drive_engaged = True
STATE = "NORMAL"
notFinished = True
portCount = 1
multiplier = 2
turtleMode = False
turtleSpeed = 10
offRoad = False
oobSegKey = None
oobSearch = False
print(oobList)
speedList = []
currentSpeed = oobList[0][0][0]
for speed in oobList[0][0]:
speedList.append(speed)
if speed > currentSpeed:
currentSpeed = speed
currentSpeed = currentSpeed * 3.6
waypointList = []
resultList = []
oobWaypoint = []
newOobWaypointList = []
for key in oobList[0][1]:
waypointList.append(key)
print(waypointList)
for waypoint in waypointList:
for key in waypointList:
print(key)
for k in key:
oobWaypoint.append(k)
resultList.append(k)
while notFinished:
# Connect to running beamng
setup_logging()
beamng = BeamNGpy('localhost', port)
bng = beamng.open(launch=True)
scenario.make(bng)
bng.load_scenario(scenario)
try:
bng.set_deterministic() # Set simulator to be deterministic
bng.set_steps_per_second(60) # With 60hz temporal resolution
# Connect to the existing vehicle (identified by the ID set in the vehicle instance)
bng.connect_vehicle(vehicle, port + portCount)
bng.start_scenario()
# Put simulator in pause awaiting further inputs
bng.pause()
assert vehicle.skt
testDic = RoadTest.to_dict(test)
parentage = testDic['network']['parentage']
for parent in parentage:
roadKeys = []
for keys in parentage:
for key in keys:
roadKeys.append(key)
nodesDict = testDic['network']['nodes']
nodes = []
for node in nodesDict:
nodes.append(node)
vehicle.update_vehicle()
oobCount = 0
inOob = False
vehicle.ai_set_speed(70)
if oobSearch:
if turtleMode:
currentSpeed = turtleSpeed
else:
currentSpeed = currentSpeed - 10
print(currentSpeed)
running = True
while running:
# 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)
# # 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)
#vehicle.ai_drive_in_lane(True)
vehicle.update_vehicle()
tup = get_segment(test, vehicle.state)
currentSegmentDict = None
currentSegKey = None
if tup is not None:
currentSegmentDict = NetworkNode.to_dict(tup)
currentSegKey = currentSegmentDict['key']
if offRoad:
if oobSegKey in resultList:
index = resultList.index(oobSegKey)
lostSeg = resultList.pop(index)
print('resList without seg', resultList)
print(lostSeg)
if oobSegKey not in newOobWaypointList:
newOobWaypointList.append(oobSegKey)
print('new OOB list', newOobWaypointList)
offRoad = False
if inOob and off_track(test, vehicle.state):
print('off track')
offRoad = True
oobCount = oobCount + 1
if currentSegKey in oobWaypoint:
vehicle.ai_drive_in_lane(True)
inOob = True
oobSegKey = currentSegKey
vehicle.ai_set_speed(currentSpeed)
else:
inOob = False
vehicle.ai_set_speed(70)
vehicle.ai_set_waypoint('waypoint_goal')
vehicle.ai_drive_in_lane(True)
if goal_reached(test, vehicle.state):
print(turtleMode)
for res in resultList:
segOobDict = testDic['execution']['seg_oob_count']
segOobDict[res] = currentSpeed / 3.6
if oobCount == 0:
testDic['execution']['oobs'] = 0
print('waypointlist', newOobWaypointList)
print('resultlist', resultList)
test = RoadTest.from_dict(testDic)
return test
else:
running = False
portCount = portCount + 1
multiplier = multiplier + 1
oobSearch = True
oobWaypoint = newOobWaypointList
newOobWaypointList = []
if currentSpeed < 20:
print(currentSpeed)
print('NewoobWay: ', newOobWaypointList)
print('resultlist: ', resultList)
if turtleMode:
for res in oobWaypoint:
print(res)
segOobDict = testDic['execution'][
'seg_oob_count']
segOobDict[res] = 12
testDic['execution']['oobs'] = 0
test = RoadTest.from_dict(testDic)
print(segOobDict)
print(resultList)
running = False
oobCount = 0
return test
else:
turtleMode = True
print('mode on')
bng.close()
finally:
if oobCount == 0:
bng.close()
else:
print('restart')
def run_sim(nodes, ai_aggression, street_1: DecalRoad, street_2: DecalRoad):
waypoint_goal = BeamNGWaypoint('waypoint_goal',
get_node_coords(street_1.nodes[-1]))
maps.beamng_map.generated().write_items(street_1.to_json() + '\n' +
waypoint_goal.to_json() + '\n' +
street_2.to_json())
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('tig', 'tigscenario')
vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
sim_data_collector = TrainingDataCollectorAndWriter(
vehicle, beamng, street_1)
scenario.add_vehicle(vehicle,
pos=get_node_coords(street_1.nodes[0]),
rot=get_rotation(street_1))
scenario.make(beamng)
beamng.open()
beamng.set_deterministic()
beamng.load_scenario(scenario)
beamng.pause()
beamng.start_scenario()
vehicle.ai_set_aggression(ai_aggression)
vehicle.ai_drive_in_lane(True)
# vehicle.ai_set_speed(25.0 / 4)
vehicle.ai_set_waypoint(waypoint_goal.name)
# vehicle.ai_set_mode("manual")
# sleep(5)
steps = 5
print(nodes)
print(beamng.get_road_edges("street_1"))
def start():
for idx in range(1000):
if (idx * 0.05 * steps) > 3.:
sim_data_collector.collect_and_write_current_data()
dist = distance(sim_data_collector.last_state.pos,
waypoint_goal.position)
if dist < 15.0:
beamng.resume()
break
# one step is 0.05 seconds (5/100)
beamng.step(steps)
try:
start()
finally:
beamng.close()
pos=(104.647, -1.92827, -3.54338),
rot=(0, 0, 90))
#add vehicle as Obstacle
vehicle1 = Vehicle('test_vehicle', model='etk800', licence='RED', color='Blue')
scenario.add_vehicle(vehicle1,
pos=(-9.66595, -90.7465, -3.45737),
rot=(0, 0, 180))
scenario.make(beamng)
bng = beamng.open()
bng.load_scenario(scenario)
#set the ego vehicle to AI mode
vehicle.ai_set_mode('Manual')
vehicle.ai_set_waypoint('DecalRoad5248_4')
vehicle1.control(throttle=0.44)
bng.start_scenario()
# After loading, the simulator waits for further input to actually start
positions = list()
directions = list()
wheel_speeds = list()
speed = list()
brakes = list()
damage1 = list()
vehicle.update_vehicle()
sensors = bng.poll_sensors(vehicle)
print('The vehicle position is:')
print(vehicle.state['pos'])
