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()
class BeamNGBrewer:
def __init__(self, beamng_home=None, road_nodes: List4DTuple = None):
self.beamng = BeamNGpy('localhost', 64256, home=beamng_home)
self.vehicle: Vehicle = None
self.camera: BeamNGCamera = None
if road_nodes:
self.setup_road_nodes(road_nodes)
steps = 5
self.params = SimulationParams(beamng_steps=steps,
delay_msec=int(steps * 0.05 * 1000))
self.vehicle_start_pose = BeamNGPose()
def setup_road_nodes(self, road_nodes):
self.road_nodes = road_nodes
self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points(
road_nodes)
self.road_points = RoadPoints().add_middle_nodes(road_nodes)
def setup_vehicle(self) -> Vehicle:
assert self.vehicle is None
self.vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
return self.vehicle
def setup_scenario_camera(self,
resolution=(1280, 1280),
fov=120) -> BeamNGCamera:
assert self.camera is None
self.camera = BeamNGCamera(self.beamng, 'brewer_camera')
return self.camera
def bring_up(self):
self.scenario = Scenario('tig', 'tigscenario')
if self.vehicle:
self.scenario.add_vehicle(self.vehicle,
pos=self.vehicle_start_pose.pos,
rot=self.vehicle_start_pose.rot)
if self.camera:
self.scenario.add_camera(self.camera.camera, self.camera.name)
self.scenario.make(self.beamng)
if not self.beamng.server:
self.beamng.open()
self.beamng.pause()
self.beamng.set_deterministic()
self.beamng.load_scenario(self.scenario)
self.beamng.start_scenario()
def __del__(self):
if self.beamng:
try:
self.beamng.close()
except:
pass
def run_sim(street_1: DecalRoad, ai_aggression):
waypoint_goal = BeamNGWaypoint('waypoint_goal', get_node_coords(street_1.nodes[-1]))
maps.beamng_map.generated().write_items(street_1.to_json() + '\n' + waypoint_goal.to_json())
beamng = BeamNGpy('localhost', 64256)
scenario = Scenario('tig', 'tigscenario')
vehicle = Vehicle('ego_vehicle', model='etk800', licence='TIG', color='Red')
sim_data_collector = TrainingDataCollectorAndWriter(vehicle, beamng, street_1)
scenario.add_vehicle(vehicle, pos=get_node_coords(street_1.nodes[0]), rot=get_rotation(street_1))
scenario.make(beamng)
beamng.open()
beamng.set_deterministic()
beamng.load_scenario(scenario)
beamng.pause()
beamng.start_scenario()
vehicle.ai_set_aggression(ai_aggression)
vehicle.ai_drive_in_lane(False)
vehicle.ai_set_speed(25)
vehicle.ai_set_waypoint(waypoint_goal.name)
#vehicle.ai_set_mode("manual")
steps = 5
def start():
for idx in range(1000):
if (idx * 0.05 * steps) > 3.:
sim_data_collector.collect_and_write_current_data()
if sim_data_collector.oob_monitor.is_oob(wrt="center"):
raise ValueError("OOB detected during training")
dist = distance(sim_data_collector.last_state.pos, waypoint_goal.position)
if dist < 15.0:
beamng.resume()
break
# one step is 0.05 seconds (5/100)
beamng.step(steps)
try:
start()
finally:
beamng.close()
def runScenario(scenario):
setup_logging()
bng = BeamNGpy('localhost',
64256,
home='C:/Users/apsara.murali-simha/beamng_research/trunk')
# Create an ETK800 with the licence plate 'PYTHON'
vehicle = Vehicle('ego_vehicle', model='etk800', licence='PYTHON')
# Add it to our scenario at this position and rotation
scenario.add_vehicle(vehicle, pos=(-717, 101, 118), rot=(0, 0, 45))
# Place files defining our scenario for the simulator to read
scenario.make(bng)
# Launch BeamNG.research
bng.open()
# Load and start our scenario
bng.load_scenario(scenario)
bng.start_scenario()
# Make the vehicle's AI span the map
vehicle.ai_set_mode('span')
class BeamNG_ACC_Test():
def __init__(self, **kwargs):
keys = kwargs.keys()
if "test_controller" in keys:
self.controller = kwargs.get("test_controller")
else:
self.controller = ACC_Test()
config = BeamNG_Cruise_Control.Config(KP=CC_P,
KI=CC_I,
KD=CC_D,
target=ACC_CAR_SPEED)
config2 = BeamNG_Cruise_Control.Config(KP=CC_P,
KI=CC_I,
KD=CC_D,
target=FRONT_CAR_SPEED)
self.PID = BeamNG_Cruise_Control.PID_Controller(config=config)
self.PID2 = BeamNG_Cruise_Control.PID_Controller(config=config2)
def setup_bngServer(self):
# Instantiate BeamNGpy instance running the simulator from the given path,
if ('BNG_HOME' in os.environ):
self.bng = BeamNGpy('localhost',
64256,
home=os.environ['BNG_HOME'])
else:
print(
"WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)"
)
self.bng = BeamNGpy(
'localhost',
64256,
home='C:\\Users\\Ingars\\Beamng-unlimited\\trunk')
def setup_BeamNG(self):
# Create a scenario in test map
self.scenario = Scenario('cruise-control', 'example')
# Create an ETK800 with the licence plate 'PID'
self.vehicle = Vehicle('ACC', model='etk800', licence='ACC')
electrics = Electrics()
self.vehicle.attach_sensor('electrics',
electrics) # Cars electric system
# Add it to our scenario at this position and rotation
self.scenario.add_vehicle(self.vehicle, pos=ACC_CAR_POS, rot=(0, 0, 0))
self.vehicle2 = Vehicle('FRONT', model='etk800', licence='FRONT')
self.vehicle2.attach_sensor('electrics',
electrics) # Cars electric system
self.scenario.add_vehicle(self.vehicle2,
pos=FRONT_CAR_POS,
rot=(0, 0, 180))
# Place files defining our scenario for the simulator to read
self.scenario.make(self.bng)
# Launch BeamNG.research
self.bng.open()
# Load and start our scenario
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
def runTest(self):
self.bng.restart_scenario()
self.bng.teleport_vehicle(self.vehicle,
pos=ACC_CAR_POS,
rot=(0, 0, 180))
self.run()
def run(self):
self.setupTest()
wanted_t = ACC_CAR_SPEED
wanted_d = WANTED_DISTANCE
soft_brake_d = wanted_d - (wanted_d / 2)
wanted_d_buffer = wanted_d - (wanted_d / 5)
# Wanted distance has a buffer of 25%, to compensate holding same constant speed at bigger distance
deacc_d = wanted_d_buffer + 10
# +10 meters is buffer for starting to decelerate towards front car speed and wanted distance.
# the buffer should be calculated dynamically instead to compensate for different speeds
while self.controller.ended == False:
start_time = datetime.datetime.now()
sensors = self.bng.poll_sensors(self.vehicle)
position = self.vehicle.state['pos']
position2 = self.vehicle2.state['pos']
sensors2 = self.bng.poll_sensors(
self.vehicle2
) # replace with self.vehicle.update_vehicle when not using pre-generated coordinates of vehicle to follow
wheel_speed = sensors['electrics']['values']['wheelspeed']
distance = self.controller.setDistance(position, position2)
front_car_speed = self.controller.getFrontCarSpeed(wheel_speed)
curr_target = self.PID.get_target()
print("distance: " + str(distance))
#FORMULA USED for deacceleration = front_car_speed^2 = wheel_speed^2 + (distance-20)*2*a
#d = (front_car_speed - wheel_speed) / (-0.3 * 2) # 0.3 deacceleration? distance for reducing speed to front cars speed with -0.3 acceleration
#print("d: " + str(d))
if distance < 5: # 5 is manually set as last allowed distance between both cars, will not work good if ACC car is driving too fast. Would be better to calculate it as braking distance.
print("brake1")
self.vehicle.control(brake=1)
self.vehicle.control(throttle=0)
elif distance < soft_brake_d:
print("brake1")
self.vehicle.control(
brake=0.1
) #Softness of brake could also be calculated dynamically to compensate different speeds
self.vehicle.control(throttle=0)
else:
if distance <= wanted_d_buffer:
print("wanted")
if front_car_speed > curr_target + 3.5 or front_car_speed < curr_target - 2: #or front_car_speed < curr_target - 1 // calibrate 3.5 and 2
self.PID.change_target(max(front_car_speed, 0))
elif distance <= deacc_d:
a = (front_car_speed - wheel_speed) / (
(distance - wanted_d_buffer) * 2)
print("a:" + str(a))
self.PID.change_target(a + wheel_speed)
elif curr_target != wanted_t:
self.PID.change_target(wanted_t)
print("throttle1")
value = self.PID.calculate_throttle(wheel_speed)
value = min(1, value)
self.vehicle.control(brake=0)
self.vehicle.control(throttle=value)
#PID for front car
wheel_speed2 = sensors2['electrics']['values']['wheelspeed']
# print("real front:" + str(wheel_speed2))
value2 = self.PID2.calculate_throttle(wheel_speed2)
value2 = min(1, value2)
self.vehicle2.control(brake=0)
self.vehicle2.control(throttle=value2)
elapsed_time = datetime.datetime.now() - start_time
while (elapsed_time.total_seconds() * 1000) < 100:
elapsed_time = datetime.datetime.now() - start_time
elapsed_total = self.controller.calculateElapsed()
# Change front car speed after 10 seconds and after 20 seconds
if elapsed_total.total_seconds() > float(10):
self.PID2.change_target(20)
if elapsed_total.total_seconds() > float(20):
self.PID2.change_target(10)
print("Ending Test")
def close(self):
self.bng.close()
def setupTest(self):
self.vehicle.update_vehicle()
self.controller.last_position = self.vehicle.state['pos']
self.controller.start()
class Server:
def __init__(self, host: str = None, port: str = None) -> None:
print("INFO::Init")
self.HOST = '' if host is None else host
self.PORT = 6555 if port is None else port
self.scenario_map = 'west_coast_usa'
self.scenario_name = 'LIDAR Testing'
self.scenario_desc = 'No description'
self.vehicle_name = 'ego_vehicle'
self.vehicle_model = 'etk800'
self.vehicle_license = 'LIDAR'
self.vehicle_pos = (-717.121, 101, 118.675)
self.vehicle_rot = None
self.vehicle_rot_quat = (0, 0, 0.3826834, 0.9238795)
print("INFO::Starting & Initializing BeamNG")
self.beamng = BeamNGpy('localhost', 64256) # Using BNG_HOME env var
self.beamng.open(launch=True)
print("INFO::Connection successful")
self._init_beamNG()
print("DONE::Starting & Initializing BeamNG")
def _init_beamNG(self) -> None:
self.scenario = Scenario(self.scenario_map,
self.scenario_name,
description=self.scenario_desc)
self.vehicle = Vehicle(self.vehicle_name,
model=self.vehicle_model,
license=self.vehicle_license)
self.lidar_sensor = Lidar()
self.vehicle.attach_sensor('lidar', self.lidar_sensor)
self.scenario.add_vehicle(self.vehicle, self.vehicle_pos,
self.vehicle_rot, self.vehicle_rot_quat)
self.scenario.make(self.beamng)
self.beamng.load_scenario(self.scenario)
def start_socket(self) -> None:
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.bind((self.HOST, self.PORT))
s.listen()
print(f"INFO::Socket ready")
while True:
try:
conn, addr = s.accept()
with conn:
print("INFO::New connection ", addr)
while conn:
try:
self.vehicle.poll_sensors()
except AssertionError:
print("WARN::AssertionError @ poll_sensors()")
self._points = self.lidar_sensor.data['points']
self._packet = [self._points, self.vehicle.state]
conn.sendall(pickle.dumps(self._packet))
print("INFO::Sent data! @ ", datetime.now())
time.sleep(0.5)
except ConnectionResetError:
print("WARN::Lost connection")
for i in range(FIRST_TEST, LAST_TEST+1):
# Load different scenarios.
scenario_name = SCENARIO_BASE_NAME + str(i)
scenario = Scenario('drivebuild', scenario_name)
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
# Has to be the correct folder.
destination_path = 'C:\\Users\\fraun\\Documents\\BeamNG\\levels\\drivebuild\\scenarios'
scenario.path = Path(destination_path)
prefab_path = Path(destination_path).joinpath(Path(scenario_name + ".prefab"))
cov_collector = CoverageCollector(vehicle, bng, prefab_path)
bng.open()
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.ai_drive_in_lane(True)
vehicle.ai_set_speed(20, "limit")
vehicle.ai_set_mode("span")
# Data collecting loop. Collects every three steps data.
counter = 0
last_counter = 0
steps = 2
while counter < 300:
if counter > last_counter + steps:
cov_collector.collect()
counter += 1
if counter % 10 == 0:
def scenario(beamNG_path):
if (beamNG_path == ""):
beamNG_path = 'D:/BeamNGReasearch/Unlimited_Version/trunk'
bng = BeamNGpy('localhost', 64256, beamNG_path)
#scenario = Scenario('west_coast_usa', 'example')
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')
electrics = Electrics()
vehicle1.attach_sensor('electrics', electrics)
pos2 = (-13.04469108581543, -107.0409164428711, 0.202297180891037)
pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
rot2 = (0.0009761620895005763, -0.9999936819076538, -0.0034209610894322395)
rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
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)
SIZE = 500
try:
bng.load_scenario(scenario)
bng.start_scenario()
vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
vehicle1.ai_set_mode('span')
vehicle2.ai_set_mode('chase')
#collect data
positions = list()
directions = list()
wheel_speeds = list()
for _ in range(100):
time.sleep(0.1)
vehicle1.update_vehicle() # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(vehicle1) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle1.state['pos'])
directions.append(vehicle1.state['dir'])
wheel_speeds.append(sensors['electrics']['values']['wheelspeed'])
print('The Final result - position :')
print(positions)
print('The Final result - direction :')
print(directions)
print('The Final result - speed :')
print(wheel_speeds)
bng.stop_scenario()
bng.close()
finally:
bng.close()
class BeamNG_Cruise_Controller_Test():
def __init__(self, **kwargs):
keys = kwargs.keys()
if "test_controller" in keys:
self.controller = kwargs.get("test_controller")
else:
self.controller = PID_Controller_Test()
if "testing_times" in keys:
self.testing_times = kwargs.get("testing_times")
else:
self.testing_times = 1
if "test_name" in keys:
self.test_name = kwargs.get("test_name")
else:
self.test_name = "test"
if "targets" in keys:
self.targets = kwargs.get("targets")
else:
self.targets = [0]
def setup_bngServer(self):
# Instantiate BeamNGpy instance running the simulator from the given path,
if ('BNG_HOME' in os.environ):
self.bng = BeamNGpy('localhost',
64256,
home=os.environ['BNG_HOME'])
else:
print(
"WARNING no BNG_HOME is set! Make sure to set BeamNG.research path to research\trunk\ as environment variable (or write the path in the script by yourself)"
)
self.bng = BeamNGpy('localhost', 64256, home='')
def setup_BeamNG(self):
# Create a scenario in test map
scenario = Scenario('cruise-control', 'example')
# Create an ETK800 with the licence plate 'PID'
self.vehicle = Vehicle('ego_vehicle', model='etk800', licence='PID')
electrics = Electrics()
self.vehicle.attach_sensor('electrics',
electrics) # Cars electric system
# Add it to our scenario at this position and rotation
scenario.add_vehicle(self.vehicle,
pos=(406.787, 1115.518, 0),
rot=(0, 0, 0))
# Place files defining our scenario for the simulator to read
scenario.make(self.bng)
# Launch BeamNG.research
self.bng.open()
# Load and start our scenario
self.bng.load_scenario(scenario)
self.bng.start_scenario()
def runTest(self, test_K_values, test_name):
for test_K_value in test_K_values:
KP_in, KI_in, KD_in = map(float, test_K_value)
if KP_in == 0 and KI_in == 0 and KD_in == 0:
config = Config()
controller = On_Off_Controller(config=config)
else:
config = Config(KP=KP_in, KI=KI_in, KD=KD_in)
controller = PID_Controller(config=config)
self.controller.controller = controller
self.test_name = str(test_name) + "_" + str(KP_in) + "_" + str(
KI_in) + "_" + str(KD_in)
self.runTestForPID()
def runTestForPID(self):
for speed in self.targets:
self.controller.setTarget(speed)
self.controller.setTime(float(30))
#self.runTestOfType("up_{0}_".format(speed), (406.787, 700.517, 0.214829), (0, 0, 0))
self.runTestOfType("straight_{0}_".format(speed),
(406.787, 715.517, 0.214829), (0, 0, 180))
#self.runTestOfType("down_{0}_".format(speed), (406.787, 304.896, 100.211), (0, 0, 180))
def runTestOfType(self, type, pos, rot):
i = 1
while i <= self.testing_times:
self.controller.newLogFile("../logs/" + self.test_name + "_" +
str(type) + "_" + str(i) + ".txt")
self.bng.restart_scenario()
self.bng.teleport_vehicle(self.vehicle, pos=pos, rot=rot)
self.run()
i += 1
def run(self):
self.controller.start()
while self.controller.ended == False:
start_time = datetime.datetime.now()
sensors = self.bng.poll_sensors(self.vehicle)
wheel_speed = sensors['electrics']['values']['wheelspeed']
value = self.controller.calculate_speed_with_logs(wheel_speed)
if value <= 0:
value = max(-1, value - 0.1) * -1
self.vehicle.control(brake=value)
self.vehicle.control(throttle=0)
else:
value = min(1, value)
self.vehicle.control(brake=0)
self.vehicle.control(throttle=value)
elapsed_time = datetime.datetime.now() - start_time
while (elapsed_time.total_seconds() * 1000) < 100:
elapsed_time = datetime.datetime.now() - start_time
print("Ending Test")
def close(self):
self.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()
class BeamNGBrewer:
def __init__(self,
beamng_home=None,
beamng_user=None,
reuse_beamng=True,
road_nodes: List4DTuple = None):
self.reuse_beamng = reuse_beamng
if self.reuse_beamng:
# This represents the running BeamNG simulator. Since we use launch=True this should automatically
# shut down when the main python process exits or when we call self.beamng_process.stop()
self.beamng_process = BeamNGpy('localhost',
64256,
home=beamng_home,
user=beamng_user)
self.beamng_process = self.beamng_process.open(launch=True)
# This is used to bring up each simulation without restarting the simulator
self.beamng = BeamNGpy('localhost',
64256,
home=beamng_home,
user=beamng_user)
# We need to wait until this point otherwise the BeamNG loggers's level will be (re)configured by BeamNGpy
log.info("Disabling BEAMNG logs")
for id in [
"beamngpy", "beamngpy.beamngpycommon", "beamngpy.BeamNGpy",
"beamngpy.beamng", "beamngpy.Scenario", "beamngpy.Vehicle"
]:
logger = log.getLogger(id)
logger.setLevel(log.CRITICAL)
logger.disabled = True
self.vehicle: Vehicle = None
self.camera: BeamNGCamera = None
if road_nodes:
self.setup_road_nodes(road_nodes)
steps = 5
self.params = SimulationParams(beamng_steps=steps,
delay_msec=int(steps * 0.05 * 1000))
self.vehicle_start_pose = BeamNGPose()
def setup_road_nodes(self, road_nodes):
self.road_nodes = road_nodes
self.decal_road: DecalRoad = DecalRoad('street_1').add_4d_points(
road_nodes)
self.road_points = RoadPoints().add_middle_nodes(road_nodes)
def setup_vehicle(self) -> Vehicle:
assert self.vehicle is None
self.vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='TIG',
color='Red')
return self.vehicle
def setup_scenario_camera(self,
resolution=(1280, 1280),
fov=120) -> BeamNGCamera:
assert self.camera is None
self.camera = BeamNGCamera(self.beamng, 'brewer_camera')
return self.camera
# TODO COnsider to transform brewer into a ContextManager or get rid of it...
def bring_up(self):
if self.reuse_beamng:
# This assumes BeamNG is already running
self.beamng.open(launch=False)
else:
self.beamng.open(launch=True)
# After 1.18 to make a scenario one needs a running instance of BeamNG
self.scenario = Scenario('tig', 'tigscenario')
if self.vehicle:
self.scenario.add_vehicle(self.vehicle,
pos=self.vehicle_start_pose.pos,
rot=self.vehicle_start_pose.rot)
if self.camera:
self.scenario.add_camera(self.camera.camera, self.camera.name)
self.scenario.make(self.beamng)
self.beamng.set_deterministic()
self.beamng.load_scenario(self.scenario)
self.beamng.start_scenario()
# Pause the simulator only after loading and starting the scenario
self.beamng.pause()
def __del__(self):
if self.beamng:
try:
self.beamng.close()
except:
pass
class DataCollector:
def __init__(self):
self.collected_data = {"image_name": [], "steering": []}
self.scenario = Scenario(config.get("data_collecting.scenario_level"),
config.get("data_collecting.scenario_name"))
self.vehicle = Vehicle(
'ego_vehicle',
model=config.get("data_collecting.vehicle_model"),
licence='PYTHON')
self.bng = BeamNGpy(config.get("beamNG.host"),
int(config.get("beamNG.port")),
home=config.get("beamNG.home"))
def launch_beam_ng(self, mode="manual_mode"):
# Create an ETK800 with the licence plate 'PYTHON'
electrics = Electrics()
# attach to get steering angles
self.vehicle.attach_sensor('electrics', electrics)
# attach to get images from camera
self.vehicle.attach_sensor('front_cam', self.create_camera_sensor())
# Add it to our scenario at this position and rotation
# self.scenario.add_vehicle(self.vehicle)
self.scenario.add_vehicle(
self.vehicle,
pos=tuple(map(float,
config.get("data_collecting.pos").split(","))),
rot_quat=tuple(
map(float,
config.get("data_collecting.rot_quat").split(","))))
# Place files defining our scenario for the simulator to read
self.scenario.make(self.bng)
# Launch BeamNG.research
self.bng.open()
# Load and start our scenario
self.bng.load_scenario(self.scenario)
self.bng.start_scenario()
if mode == "ai_mode":
# Make the vehicle's AI span the map
self.vehicle.ai_drive_in_lane(True)
self.vehicle.ai_set_mode('span')
def save_image_manually(self, cam_name='front_cam'):
img = self.bng.poll_sensors(self.vehicle)[cam_name]['colour']
steering = self.bng.poll_sensors(self.vehicle)['electrics']['steering']
self.save_data(img, steering)
@staticmethod
def create_camera_sensor(
pos=(-0.3, 2, 1.0), direction=(0, 1, 0), fov=100, res=None):
# Set up camera sensor
resolution = res
if res is None:
resolution = (int(config.get("data_collecting.image_width")),
int(config.get("data_collecting.image_height")))
pos = pos
direction = direction
fov = fov
front_camera = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
return front_camera
def save_data(self, img, steering, i: str = "0"):
file_name = str(int(time.time())) + i + ".jpg"
try:
image_path = definitions.ROOT_DIR + config.get(
'data_collecting.data_path') + file_name
imageio.imwrite(image_path, np.asarray(img.convert('RGB')), "jpg")
self.collected_data["image_name"].append(file_name)
self.collected_data["steering"].append(steering)
except Exception as ex:
logger.info(f"Error while saving data -- {ex}")
raise Exception
def collect_data(self, number_of_images: int, mode="manual_mode"):
self.launch_beam_ng(mode)
logger.info("Start after 3 seconds...")
time.sleep(5)
logger.info(
f"Start collecting {config.get('data_collecting.number_of_images')} training images"
)
i = 0
exit_normally = True
try:
while i < number_of_images:
# image is training image and steering is label
img = self.bng.poll_sensors(
self.vehicle)['front_cam']['colour']
steering = self.bng.poll_sensors(
self.vehicle)['electrics']['steering']
logger.info(f"Saving data {i + 1} ...")
self.save_data(img, steering, str(i))
logger.info("Saved data successfully")
i = i + 1
time.sleep(int(config.get("data_collecting.sleep")))
except Exception as ex:
exit_normally = False
logger.info(f"Error while collecting data {ex}")
finally:
self.bng.close()
return exit_normally
def save_csv_data(self):
logger.info("Start saving csv file......")
csv_path = definitions.ROOT_DIR + config.get(
'data_collecting.csv_path')
df = pd.DataFrame(self.collected_data,
columns=['image_name', 'steering'])
if not os.path.isfile(csv_path):
df.to_csv(csv_path, index=False, header=True)
else: # else it exists so append without writing the header
df.to_csv(csv_path, index=False, mode='a', header=False)
class Server:
def __init__(self,
options: Dict[str, str],
host: str = '',
port: int = 6555) -> None:
"""
Initialize the Server
Args:
options (Dict[str, str]): Options / Characteristics used to construct
the vehicle, scenario, and different sensors
host (str, optional): IP/Hostname that the server listens for, defaults
to '' - loopback / all.
port (int, optional): Port that the server listens for, defaults to 6555.
"""
Log.info("Init")
self.HOST = host
self.PORT = port
self.OPTIONS = options
Log.info("Starting & Initializing BeamNG")
self.beamng = BeamNGpy('localhost', 64256) # Using BNG_HOME env var
self.beamng.open(launch=True)
Log.info("Connection successful")
self._init_beamNG()
Log.done("Starting & Initializing BeamNG")
def _init_beamNG(self) -> None:
"""
Initialize beamNG:
Create the scenario, vehicle, sensors, and load everything
"""
self.scenario = Scenario(self.OPTIONS['scenario_map'],
self.OPTIONS['scenario_name'],
description=self.OPTIONS['scenario_desc'])
self.vehicle = Vehicle(self.OPTIONS['vehicle_name'],
model=self.OPTIONS['vehicle_model'],
license=self.OPTIONS['vehicle_license'])
self.lidar_sensor = Lidar(max_dist=180, vres=24, vangle=25)
self.vehicle.attach_sensor('lidar', self.lidar_sensor)
self.front_camera = Camera(self.OPTIONS['f_cam_pos'],
self.OPTIONS['f_cam_dir'],
self.OPTIONS['f_cam_fov'],
self.OPTIONS['f_cam_res'],
colour=True,
annotation=True)
self.vehicle.attach_sensor('front_camera', self.front_camera)
self.scenario.add_vehicle(self.vehicle, self.OPTIONS['vehicle_pos'],
self.OPTIONS['vehicle_rot'],
self.OPTIONS['vehicle_rot_quat'])
self.scenario.make(self.beamng)
self.beamng.load_scenario(self.scenario)
def start_socket(self, send_delay: float = 0.369) -> None:
"""
Initialize the socket and await (blocking) connections
Args:
send_delay (float, optional): How long to wait before sending a new
packet. Defaults to 0.369.
Packet data - List:
[0]: vehicle_state
[1]: lidar_data
[2]: front_camera_data
"""
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.bind((self.HOST, self.PORT))
s.listen()
Log.info("Socket ready")
while True:
try:
conn, addr = s.accept()
with conn:
Log.done(f"New connection {addr}")
while conn:
self.vehicle.poll_sensors()
self._points = self.lidar_sensor.data['points']
self._camera = self.front_camera.data['colour']
self._packet = [
self.vehicle.state, self._points, self._camera
]
conn.send(pickle.dumps(self._packet))
Log.info(f"Sent data! @ {datetime.now()}")
time.sleep(send_delay)
except ConnectionResetError:
Log.warn("Lost connection")
if input('quit? (y/n)').find('y'):
break
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()
class WCARaceGeometry(gym.Env):
sps = 50
rate = 5
front_dist = 800
front_step = 100
trail_dist = 104
trail_step = 13
starting_proj = 1710
max_damage = 100
def __init__(self, host='localhost', port=64256):
self.steps = WCARaceGeometry.sps // WCARaceGeometry.rate
self.host = host
self.port = port
self.action_space = self._action_space()
self.observation_space = self._observation_space()
self.episode_steps = 0
self.spine = None
self.l_edge = None
self.r_edge = None
self.polygon = None
front_factor = WCARaceGeometry.front_dist / WCARaceGeometry.front_step
trail_factor = WCARaceGeometry.trail_dist / WCARaceGeometry.trail_step
self.front = lambda step: +front_factor * step
self.trail = lambda step: -trail_factor * step
self.bng = BeamNGpy(self.host, self.port)
self.vehicle = Vehicle('racecar',
model='sunburst',
licence='BEAMNG',
colour='red',
partConfig='vehicles/sunburst/hillclimb.pc')
electrics = Electrics()
damage = Damage()
self.vehicle.attach_sensor('electrics', electrics)
self.vehicle.attach_sensor('damage', damage)
scenario = Scenario('west_coast_usa', 'wca_race_geometry_v0')
scenario.add_vehicle(self.vehicle,
pos=(394.5, -247.925, 145.25),
rot=(0, 0, 90))
scenario.make(self.bng)
self.bng.open(launch=True)
self.bng.set_deterministic()
self.bng.set_steps_per_second(WCARaceGeometry.sps)
self.bng.load_scenario(scenario)
self._build_racetrack()
self.observation = None
self.last_observation = None
self.last_spine_proj = None
self.bng.start_scenario()
self.bng.pause()
def __del__(self):
self.bng.close()
def _build_racetrack(self):
roads = self.bng.get_roads()
track = roads['race_ref']
l_vtx = []
s_vtx = []
r_vtx = []
for right, middle, left in track:
r_vtx.append(right)
s_vtx.append(middle)
l_vtx.append(left)
self.spine = LinearRing(s_vtx)
self.r_edge = LinearRing(r_vtx)
self.l_edge = LinearRing(l_vtx)
r_vtx = [v[0:2] for v in r_vtx]
l_vtx = [v[0:2] for v in l_vtx]
self.polygon = Polygon(l_vtx, holes=[r_vtx])
def _action_space(self):
action_lo = [-1., -1.]
action_hi = [+1., +1.]
return gym.spaces.Box(np.array(action_lo),
np.array(action_hi),
dtype=float)
def _observation_space(self):
# n vertices of left and right polylines ahead and behind, 3 floats per
# vtx
scope = WCARaceGeometry.trail_step + WCARaceGeometry.front_step
obs_lo = [
-np.inf,
] * scope * 3
obs_hi = [
np.inf,
] * scope * 3
obs_lo.extend([
-np.inf, # Distance to left edge
-np.inf, # Distance to right edge
-2 * np.pi, # Inclination
-2 * np.pi, # Angle
-2 * np.pi, # Vertical angle
-np.inf, # Spine speed
0, # RPM
-1, # Gear
0, # Throttle
0, # Brake
-1.0, # Steering
0, # Wheel speed
-np.inf, # Altitude
])
obs_hi.extend([
np.inf, # Distance to left edge
np.inf, # Distance to right edge
2 * np.pi, # Inclincation
2 * np.pi, # Angle
2 * np.pi, # Vertical angle
np.inf, # Spine speed
np.inf, # RPM
8, # Gear
1.0, # Throttle
1.0, # Brake
1.0, # Steering
np.inf, # Wheel speed
np.inf, # Altitude
])
return gym.spaces.Box(np.array(obs_lo), np.array(obs_hi), dtype=float)
def _make_commands(self, action):
brake = 0
throttle = action[1]
steering = action[0]
if throttle < 0:
brake = -throttle
throttle = 0
self.vehicle.control(steering=steering, throttle=throttle, brake=brake)
def _project_vehicle(self, pos):
r_proj = self.r_edge.project(pos)
r_proj = self.r_edge.interpolate(r_proj)
l_proj = self.l_edge.project(r_proj)
l_proj = self.l_edge.interpolate(l_proj)
s_proj = self.spine.project(r_proj)
s_proj = self.spine.interpolate(s_proj)
return l_proj, s_proj, r_proj
def _get_vehicle_angles(self, vehicle_pos, spine_seg):
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
spine_angle = np.arctan2(spine_end[1] - spine_beg[1],
spine_end[0] - spine_beg[0])
vehicle_angle = self.vehicle.state['dir'][0:2]
vehicle_angle = np.arctan2(vehicle_angle[1], vehicle_angle[0])
vehicle_angle = normalise_angle(vehicle_angle - spine_angle)
vehicle_angle -= np.pi
elevation = np.arctan2(spine_beg[2] - spine_end[2], spine_seg.length)
vehicle_elev = self.vehicle.state['dir']
vehicle_elev = np.arctan2(vehicle_elev[2],
np.linalg.norm(vehicle_elev))
return vehicle_angle, vehicle_elev, elevation
def _wrap_length(self, target):
length = self.spine.length
while target < 0:
target += length
while target > length:
target -= length
return target
def _gen_track_scope_loop(self, it, fn, base, s_scope, s_width):
for step in it:
distance = base + fn(step)
distance = self._wrap_length(distance)
s_proj = self.spine.interpolate(distance)
s_scope.append(s_proj)
l_proj = self.l_edge.project(s_proj)
l_proj = self.l_edge.interpolate(l_proj)
r_proj = self.r_edge.project(s_proj)
r_proj = self.r_edge.interpolate(r_proj)
width = l_proj.distance(r_proj)
s_width.append(width)
def _gen_track_scope(self, pos, spine_seg):
s_scope = []
s_width = []
base = self.spine.project(pos)
it = range(WCARaceGeometry.trail_step, 0, -1)
self._gen_track_scope_loop(it, self.trail, base, s_scope, s_width)
it = range(1)
self._gen_track_scope_loop(it, lambda x: x, base, s_scope, s_width)
it = range(WCARaceGeometry.front_step + 1)
self._gen_track_scope_loop(it, self.front, base, s_scope, s_width)
s_proj = self.spine.interpolate(base)
offset = (-s_proj.x, -s_proj.y, -s_proj.z)
s_line = LineString(s_scope)
s_line = affinity.translate(s_line, *offset)
spine_beg = spine_seg.coords[+0]
spine_end = spine_seg.coords[-1]
direction = [spine_end[i] - spine_beg[i] for i in range(3)]
angle = np.arctan2(direction[1], direction[0]) + np.pi / 2
s_line = affinity.rotate(s_line,
-angle,
origin=(0, 0),
use_radians=True)
ret = list()
s_scope = s_line.coords
for idx in range(1, len(s_scope) - 1):
curvature = calculate_curvature(s_scope, idx)
inclination = calculate_inclination(s_scope, idx)
width = s_width[idx]
ret.append(curvature)
ret.append(inclination)
ret.append(width)
return ret
def _spine_project_vehicle(self, vehicle_pos):
proj = self.spine.project(vehicle_pos) - WCARaceGeometry.starting_proj
if proj < 0:
proj += self.spine.length
proj = self.spine.length - proj
return proj
def _get_spine_speed(self, vehicle_pos, vehicle_dir, spine_seg):
spine_beg = spine_seg.coords[0]
future_pos = Point(vehicle_pos.x + vehicle_dir[0],
vehicle_pos.y + vehicle_dir[1],
vehicle_pos.z + vehicle_dir[2])
spine_end = self.spine.project(future_pos)
spine_end = self.spine.interpolate(spine_end)
return spine_end.distance(Point(*spine_beg))
def _make_observation(self, sensors):
electrics = sensors['electrics']['values']
vehicle_dir = self.vehicle.state['dir']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
spine_beg = self.spine.project(vehicle_pos)
spine_end = spine_beg
spine_end += WCARaceGeometry.front_dist / WCARaceGeometry.front_step
spine_beg = self.spine.interpolate(spine_beg)
spine_end = self.spine.interpolate(spine_end)
spine_seg = LineString([spine_beg, spine_end])
spine_speed = self._get_spine_speed(vehicle_pos, vehicle_dir,
spine_seg)
l_dist = self.l_edge.distance(vehicle_pos)
r_dist = self.r_edge.distance(vehicle_pos)
angle, vangle, elevation = self._get_vehicle_angles(
vehicle_pos, spine_seg)
l_proj, s_proj, r_proj = self._project_vehicle(vehicle_pos)
s_scope = self._gen_track_scope(vehicle_pos, spine_seg)
obs = list()
obs.extend(s_scope)
obs.append(l_dist)
obs.append(r_dist)
obs.append(elevation)
obs.append(angle)
obs.append(vangle)
obs.append(spine_speed)
obs.append(electrics['rpm'])
obs.append(electrics['gearIndex'])
obs.append(electrics['throttle'])
obs.append(electrics['brake'])
obs.append(electrics['steering'])
obs.append(electrics['wheelspeed'])
obs.append(electrics['altitude'])
return np.array(obs)
def _compute_reward(self, sensors):
damage = sensors['damage']
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
if damage['damage'] > WCARaceGeometry.max_damage:
return -1, True
if not self.polygon.contains(Point(vehicle_pos.x, vehicle_pos.y)):
return -1, True
score, done = -1, False
spine_proj = self._spine_project_vehicle(vehicle_pos)
if self.last_spine_proj is not None:
diff = spine_proj - self.last_spine_proj
if diff >= -0.10:
if diff < 0.5:
return -1, False
else:
score, done = diff / self.steps, False
elif np.abs(diff) > self.spine.length * 0.95:
score, done = 1, True
else:
score, done = -1, True
self.last_spine_proj = spine_proj
return score, done
def reset(self):
self.episode_steps = 0
self.vehicle.control(throttle=0, brake=0, steering=0)
self.bng.restart_scenario()
self.bng.step(30)
self.bng.pause()
self.vehicle.set_shift_mode(3)
self.vehicle.control(gear=2)
sensors = self.bng.poll_sensors(self.vehicle)
self.observation = self._make_observation(sensors)
vehicle_pos = self.vehicle.state['pos']
vehicle_pos = Point(*vehicle_pos)
self.last_spine_proj = self._spine_project_vehicle(vehicle_pos)
return self.observation
def advance(self):
self.bng.step(self.steps, wait=False)
def observe(self):
sensors = self.bng.poll_sensors(self.vehicle)
new_observation = self._make_observation(sensors)
return new_observation, sensors
def step(self, action):
action = [*np.clip(action, -1, 1), action[0], action[1]]
action = [float(v) for v in action]
self.episode_steps += 1
self._make_commands(action)
self.advance()
new_observation, sensors = self.observe()
if self.observation is not None:
self.last_observation = self.observation
self.observation = new_observation
score, done = self._compute_reward(sensors)
print((' A: {:5.2f} B: {:5.2f} '
' S: {:5.2f} T: {:5.2f} R: {:5.2f}').format(
action[2], action[3], action[0], action[1], score))
# if done:
# self.bng.step(WCARaceGeometry.sps * 1)
return self.observation, score, done, {}
def collect_data(beamNG_path):
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')
electrics = Electrics()
vehicle1.attach_sensor('electrics', electrics)
pos1 = (-4.270055770874023, -115.30198669433594, 0.20322345197200775)
rot1 = (0.872300386428833, -0.48885437846183777, 0.01065115723758936)
scenario.add_vehicle(vehicle1, pos=pos1, rot=rot1)
# Place files defining our scenario for the simulator to read
scenario.make(bng)
# Launch BeamNG.research
bng.open(launch=True)
#SIZE = 50
try:
bng.load_scenario(scenario)
bng.start_scenario()
#vehicle1.ai_set_speed(10.452066507339481,mode = 'set')
vehicle1.ai_set_mode('span')
#collect data
print("\n Position and rotation of car \n ")
# for _ in range(200):
for _ in range(200):
time.sleep(0.1)
vehicle1.update_vehicle(
) # Synchs the vehicle's "state" variable with the simulator
sensors = bng.poll_sensors(
vehicle1
) # Polls the data of all sensors attached to the vehicle
positions.append(vehicle1.state['pos'])
directions.append(vehicle1.state['dir'])
print([vehicle1.state['pos'], vehicle1.state['dir']])
#write data into file
# print ("position :",positions)
# print ("position :",directions)
sys_output.print_title(
"\n The position and rotation of the car is saved in \"position.txt and \"roation.txt\" \""
)
write_data(FILE_POS, positions)
write_data(FILE_ROT, directions)
bng.stop_scenario()
bng.close()
time.sleep(2)
finally:
bng.close()
return (positions, directions)
def main() -> None:
from beamngpy import BeamNGpy, Scenario, Vehicle, beamngcommon
from beamngpy.sensors import Damage, Camera, Electrics
from time import sleep
bng = BeamNGpy("localhost", 64523)
ego_vehicle = Vehicle('ego_vehicle',
model='etk800',
licence='EGO',
color='Red')
scenario = Scenario(
"west_coast_usa",
"DamageSensorTest",
authors="Stefan Huber",
description="Test usage and check output of the damage sensor")
direction = (0, 1, 0)
fov = 120
resolution = (320, 160)
y, z = (1.7, 1.0)
cam_center = Camera((-0.3, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
cam_left = Camera((-1.3, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
cam_right = Camera((0.4, y, z),
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True)
#cameras_array = [camera_center, camera_left, camera_right]
ego_vehicle.attach_sensor('cam_center', cam_center)
ego_vehicle.attach_sensor('cam_left', cam_left)
ego_vehicle.attach_sensor('cam_right', cam_right)
ego_vehicle.attach_sensor("electrics", Electrics())
#electrics_data = Electrics.encode_vehicle_request(Electrics)
#print(electrics_data)
#scenario.add_vehicle(ego_vehicle,pos=(-717.121, 101, 118.675), rot=(0, 0, 45))
scenario.add_vehicle(ego_vehicle,
pos=(-725.7100219726563, 554.3270263671875,
121.0999984741211),
rot=(0, 0, 45))
scenario.make(bng)
bng.open(launch=True)
def save_image(data, ind, cam_name):
img = data[cam_name]['colour'].convert('RGB')
file_name = str(ind) + "_" + cam_name + ".jpg"
filepath = os.path.join('C:/Users/HP/Documents/Data_Log/10_lap_log',
file_name)
img.save(filepath)
return file_name
def save(data, ind):
cam_left_file_name = save_image(data, ind, 'cam_left')
cam_right_file_name = save_image(data, ind, 'cam_right')
cam_center_file_name = save_image(data, ind, 'cam_center')
steering_in_value = data['electrics']['values']['steering_input']
steering_value = data['electrics']['values']['steering']
throttle_in_value = data['electrics']['values']['throttle_input']
throttle_value = data['electrics']['values']['throttle']
clutch_value = data['electrics']['values']['clutch']
clutch_in_value = data['electrics']['values']['clutch_input']
wheel_speed_value = data['electrics']['values']['wheelspeed']
rpmspin_value = data['electrics']['values']['rpmspin']
#add here
print(cam_left_file_name, cam_right_file_name, cam_center_file_name,
steering_in_value, steering_value, throttle_in_value,
throttle_value, clutch_in_value, clutch_value, rpmspin_value,
wheel_speed_value)
print()
temp_df = temp_dataframe()
temp_df.loc[0] = [
cam_left_file_name, cam_right_file_name, cam_center_file_name,
steering_in_value, steering_value, throttle_in_value,
throttle_value, clutch_in_value, clutch_value, wheel_speed_value,
rpmspin_value
] # modify
#append with existing and save
df_orig = pd.read_csv('my_data_lap3.csv')
df_orig = pd.concat([df_orig, temp_df])
df_orig.to_csv('my_data_lap3.esc', index=False)
del [[df_orig, temp_df]]
def temp_dataframe():
df1 = pd.DataFrame(columns=[
'cam_left', 'cam_right', 'cam_centre', 'steering_in_value',
'steering_value', 'throttle_in_value', 'throttle_value',
'clutch_in_value', 'clutch_value', 'wheelspeed_value',
'rpmspin_value'
]) # modify
return df1
try:
bng.load_scenario(scenario)
bng.set_steps_per_second(60)
bng.set_deterministic()
bng.start_scenario()
bng.hide_hud()
ego_vehicle.ai_drive_in_lane(lane=True)
ego_vehicle.ai_set_mode('span')
ego_vehicle.ai_set_speed(10)
ind = 0
while True:
sensor_data = bng.poll_sensors(ego_vehicle)
save(sensor_data, ind)
ind += 1
finally:
bng.close()
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 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 ImpactGenerator:
parts = [
'etk800_bumper_F',
'etk800_bumperbar_F',
'etk800_bumper_R',
'etk800_fender_L',
'etk800_fender_R',
'etk800_hood',
'etk800_towhitch',
'etk800_steer',
'etk800_radiator',
'etk800_roof_wagon',
'wheel_F_5',
]
emptyable = {
'etk800_bumperbar_F',
'etk800_towhitch',
}
wca = {
'level': 'west_coast_usa',
'a_spawn': (-270.75, 678, 74.9),
'b_spawn': (-260.25, 678, 74.9),
'pole_pos': (-677.15, 848, 75.1),
'linear_pos_a': (-630, 65, 103.4),
'linear_pos_b': (-619, 77, 102.65),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 75.1),
't_pos_b': (-453, 700, 75.1),
't_rot_b': (0, 0, 315),
'ref_pos': (-18, 610, 75),
}
smallgrid = {
'level': 'smallgrid',
'a_spawn': (-270.75, 678, 0.1),
'b_spawn': (-260.25, 678, 0.1),
'pole_pos': (-677.15, 848, 0.1),
'pole': (-682, 842, 0),
'linear_pos_a': (-630, 65, 0.1),
'linear_pos_b': (-619, 77, 0.1),
'linear_rot_b': (0, 0, 45.5),
't_pos_a': (-440, 688, 0.1),
't_pos_b': (-453, 700, 0.1),
't_rot_b': (0, 0, 315),
'ref_pos': (321, 321, 0.1),
}
def __init__(self,
bng_home,
output,
config,
poolsize=2,
smallgrid=False,
sim_mtx=None,
similarity=0.5,
random_select=False,
single=False):
self.bng_home = bng_home
self.output = output
self.config = config
self.smallgrid = smallgrid
self.single = single
self.impactgen_mats = None
if smallgrid:
mats = materialmngr.get_impactgen_materials(bng_home)
self.impactgen_mats = sorted(list(mats))
self.poolsize = poolsize
self.similarity = similarity
self.sim_mtx = sim_mtx
self.random_select = random_select
self.pole_space = None
self.t_bone_space = None
self.linear_space = None
self.nocrash_space = None
self.post_space = None
self.total_possibilities = 0
self.bng = BeamNGpy('localhost', 64256, home=bng_home)
self.scenario = None
scenario_props = ImpactGenerator.wca
if smallgrid:
scenario_props = ImpactGenerator.smallgrid
self.vehicle_a = Vehicle('vehicle_a', model='etk800')
self.vehicle_b = Vehicle('vehicle_b', model='etk800')
self.scenario = Scenario(scenario_props['level'], 'impactgen')
self.scenario.add_vehicle(self.vehicle_a,
pos=scenario_props['a_spawn'],
rot=(0, 0, 0))
self.scenario.add_vehicle(self.vehicle_b,
pos=scenario_props['b_spawn'],
rot=(0, 0, 0))
self.vehicle_a_parts = defaultdict(set)
self.vehicle_a_config = None
self.vehicle_b_config = None
def generate_colors(self):
return copy.deepcopy(self.config['colors'])
def generate_nocrash_space(self, props):
nocrash_options = []
for part in ImpactGenerator.parts: # Vary each configurable part
nocrash_options.append(self.vehicle_a_parts[part])
self.nocrash_space = OptionSpace(nocrash_options)
def generate_pole_space(self, props):
pole_options = [(False, True)] # Vehicle facing forward/backward
pole_options.append(np.linspace(-0.75, 0.75, 5)) # Position offset
pole_options.append(np.linspace(0.15, 0.5, 4)) # Throttle intensity
for part in ImpactGenerator.parts: # Vary each configurable part
pole_options.append(self.vehicle_a_parts[part])
self.pole_space = OptionSpace(pole_options)
def generate_t_bone_space(self, props):
t_options = [(False, True)] # Vehicle hit left/right
t_options.append(np.linspace(-30, 30, 11)) # A rotation offset
t_options.append(np.linspace(-1.5, 1.5, 5)) # B pos. offset
t_options.append(np.linspace(0.2, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
t_options.append(self.vehicle_a_parts[part])
self.t_bone_space = OptionSpace(t_options)
def generate_linear_space(self, props):
linear_options = [(False, True)] # Vehicle hit front/back
linear_options.append(np.linspace(-15, 15, 5)) # A rot. offset
linear_options.append(np.linspace(-1.33, 1.33, 5)) # B pos. offset
linear_options.append(np.linspace(0.25, 0.5, 4)) # B throttle
for part in ImpactGenerator.parts:
linear_options.append(self.vehicle_a_parts[part])
self.linear_space = OptionSpace(linear_options)
def get_material_options(self):
if not self.random_select:
selected = materialmngr.pick_materials(self.impactgen_mats,
self.sim_mtx,
poolsize=self.poolsize,
similarity=self.similarity)
if selected is None:
log.info('Could not find material pool through similarity. '
'Falling back to random select.')
else:
selected = random.sample(self.impactgen_mats, self.poolsize)
return selected
def generate_post_space(self):
colors = self.generate_colors()
post_options = []
post_options.append(self.config['times'])
if self.smallgrid:
post_options.append([0])
post_options.append([0])
else:
post_options.append(self.config['clouds'])
post_options.append(self.config['fogs'])
post_options.append(colors)
if self.smallgrid:
mats = self.get_material_options()
if mats is not None:
post_options.append(list(mats))
post_options.append(list(mats))
return OptionSpace(post_options)
def generate_spaces(self):
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
self.generate_nocrash_space(props)
self.generate_t_bone_space(props)
self.generate_linear_space(props)
self.generate_pole_space(props)
def scan_parts(self, parts, known=set()):
with open('out.json', 'w') as outfile:
outfile.write(json.dumps(parts, indent=4, sort_keys=True))
for part_type in ImpactGenerator.parts:
options = parts[part_type]
self.vehicle_a_parts[part_type].update(options)
def init_parts(self):
self.vehicle_a_config = self.vehicle_a.get_part_config()
self.vehicle_b_config = self.vehicle_b.get_part_config()
b_parts = self.vehicle_b_config['parts']
b_parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
b_parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
b_parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
options = self.vehicle_a.get_part_options()
self.scan_parts(options)
for k in self.vehicle_a_parts.keys():
self.vehicle_a_parts[k] = list(self.vehicle_a_parts[k])
if k in ImpactGenerator.emptyable:
self.vehicle_a_parts[k].append('')
def init_settings(self):
self.bng.set_particles_enabled(False)
self.generate_spaces()
log.info('%s pole crash possibilities.', self.pole_space.count)
log.info('%s T-Bone crash possibilities.', self.t_bone_space.count)
log.info('%s parallel crash possibilities.', self.linear_space.count)
log.info('%s no crash possibilities.', self.nocrash_space.count)
self.total_possibilities = \
self.pole_space.count + \
self.t_bone_space.count + \
self.linear_space.count + \
self.nocrash_space.count
log.info('%s total incidents possible.', self.total_possibilities)
def get_vehicle_config(self, setting):
parts = dict()
for idx, part in enumerate(ImpactGenerator.parts):
parts[part] = setting[idx]
refwheel = parts['wheel_F_5']
parts['wheel_R_5'] = refwheel.replace('_F', '_R')
# Force licence plate to always be German
parts['etk800_licenseplate_R'] = 'etk800_licenseplate_R_EU'
parts['etk800_licenseplate_F'] = 'etk800_licenseplate_F_EU'
parts['licenseplate_design_2_1'] = 'license_plate_germany_2_1'
config = copy.deepcopy(self.vehicle_a_config)
config['parts'] = parts
return config
def set_annotation_paths(self):
part_path = os.path.join(self.bng_home, PART_ANNOTATIONS)
part_path = os.path.abspath(part_path)
obj_path = os.path.join(self.bng_home, OBJ_ANNOTATIONS)
obj_path = os.path.abspath(obj_path)
req = dict(type='ImpactGenSetAnnotationPaths')
req['partPath'] = part_path
req['objPath'] = obj_path
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenAnnotationPathsSet'
def set_image_properties(self):
req = dict(type='ImpactGenSetImageProperties')
req['imageWidth'] = self.config['imageWidth']
req['imageHeight'] = self.config['imageHeight']
req['colorFmt'] = self.config['colorFormat']
req['annotFmt'] = self.config['annotFormat']
req['radius'] = self.config['cameraRadius']
req['height'] = self.config['cameraHeight']
req['fov'] = self.config['fov']
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenImagePropertiesSet'
def setup(self):
self.scenario.make(self.bng)
log.debug('Loading scenario...')
self.bng.load_scenario(self.scenario)
log.debug('Setting steps per second...')
self.bng.set_steps_per_second(50)
log.debug('Enabling deterministic mode...')
self.bng.set_deterministic()
log.debug('Starting scenario...')
self.bng.start_scenario()
log.debug('Scenario started. Sleeping 20s.')
time.sleep(20)
self.init_parts()
self.init_settings()
log.debug('Setting annotation properties.')
self.set_annotation_paths()
self.set_image_properties()
def settings_exhausted(self):
return self.t_bone_space.exhausted() and \
self.linear_space.exhausted() and \
self.pole_space.exhausted() and \
self.nocrash_space.exhausted()
def set_post_settings(self, vid, settings):
req = dict(type='ImpactGenPostSettings')
req['ego'] = vid
req['time'] = settings[0]
req['clouds'] = settings[1]
req['fog'] = settings[2]
req['color'] = settings[3]
if len(settings) > 4:
req['skybox'] = settings[4]
if len(settings) > 5:
req['ground'] = settings[5]
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPostSet'
def finished_producing(self):
req = dict(type='ImpactGenOutputGenerated')
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipGenerated'
return resp['state']
def produce_output(self, color_name, annot_name):
while not self.finished_producing():
time.sleep(0.2)
req = dict(type='ImpactGenGenerateOutput')
req['colorName'] = color_name
req['annotName'] = annot_name
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenZipStarted'
'ImpactGenZipStarted'
def capture_post(self, crash_setting):
log.info('Enumerating post-crash settings and capturing output.')
self.bng.switch_vehicle(self.vehicle_a)
ref_pos = ImpactGenerator.wca['ref_pos']
if self.smallgrid:
ref_pos = ImpactGenerator.smallgrid['ref_pos']
self.bng.teleport_vehicle(self.vehicle_a, ref_pos)
self.bng.teleport_vehicle(self.vehicle_b, (10000, 10000, 10000))
self.bng.step(50, wait=True)
self.bng.pause()
self.post_space = self.generate_post_space()
while not self.post_space.exhausted():
post_setting = self.post_space.sample_new()
scenario = [[str(s) for s in crash_setting]]
scenario.append([str(s) for s in post_setting])
key = str(scenario).encode('ascii')
key = hashlib.sha512(key).hexdigest()[:30]
t = int(time.time())
color_name = '{}_{}_0_image.zip'.format(t, key)
annot_name = '{}_{}_0_annotation.zip'.format(t, key)
color_name = os.path.join(self.output, color_name)
annot_name = os.path.join(self.output, annot_name)
log.info('Setting post settings.')
self.set_post_settings(self.vehicle_a.vid, post_setting)
log.info('Producing output.')
self.produce_output(color_name, annot_name)
if self.single:
break
self.bng.resume()
def run_t_bone_crash(self):
log.info('Running t-bone crash setting.')
if self.t_bone_space.exhausted():
log.debug('T-Bone crash setting exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.t_bone_space.sample_new()
side, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if side:
angle += 225
else:
angle += 45
pos_a = props['t_pos_a']
rot_b = props['t_rot_b']
pos_b = list(props['t_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunTBone')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending t-bone crash config.')
self.bng.send(req)
log.debug('T-Bone crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenTBoneRan'
return setting
def run_linear_crash(self):
log.info('Running linear crash setting.')
if self.linear_space.exhausted():
log.debug('Linear crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.linear_space.sample_new()
back, angle, offset, throttle = setting[:4]
config = setting[4:]
config = self.get_vehicle_config(config)
if back:
angle += 225
else:
offset += 1.3
angle += 45
pos_a = props['linear_pos_a']
rot_b = props['linear_rot_b']
pos_b = list(props['linear_pos_b'])
pos_b[0] += offset
req = dict(type='ImpactGenRunLinear')
req['ego'] = self.vehicle_a.vid
req['other'] = self.vehicle_b.vid
req['config'] = config
req['aPosition'] = pos_a
req['angle'] = angle
req['bPosition'] = pos_b
req['bRotation'] = rot_b
req['throttle'] = throttle
log.debug('Sending linear crash config.')
self.bng.send(req)
log.debug('Linear crash response received.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenLinearRan'
return setting
def run_pole_crash(self):
log.info('Running pole crash setting.')
if self.pole_space.exhausted():
log.debug('Pole crash settings exhausted.')
return None
props = ImpactGenerator.wca
if self.smallgrid:
props = ImpactGenerator.smallgrid
setting = self.pole_space.sample_new()
back, offset, throttle = setting[:3]
config = setting[3:]
config = self.get_vehicle_config(config)
angle = 45
if back:
angle = 225
offset += 0.85
throttle = -throttle
pos = list(props['pole_pos'])
pos[0] += offset
req = dict(type='ImpactGenRunPole')
req['ego'] = self.vehicle_a.vid
req['config'] = config
req['position'] = pos
req['angle'] = angle
req['throttle'] = throttle
if self.smallgrid:
req['polePosition'] = props['pole']
log.debug('Sending pole crash config.')
self.bng.send(req)
log.debug('Got pole crash response.')
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenPoleRan'
return setting
def run_no_crash(self):
log.info('Running non-crash scenario.')
if self.nocrash_space.exhausted():
return None
setting = self.nocrash_space.sample_new()
log.info('Got new setting: %s', setting)
vehicle_config = self.get_vehicle_config(setting)
log.info('Got new vehicle config: %s', vehicle_config)
req = dict(type='ImpactGenRunNonCrash')
req['ego'] = self.vehicle_a.vid
req['config'] = vehicle_config
log.info('Sending Non-Crash request: %s', req)
self.bng.send(req)
resp = self.bng.recv()
assert resp['type'] == 'ImpactGenNonCrashRan'
log.info('Non-crash finished.')
return setting
def run_incident(self, incident):
log.info('Setting up next incident.')
self.bng.display_gui_message('Setting up next incident...')
setting = incident()
self.capture_post(setting)
return setting
def run_incidents(self):
log.info('Enumerating possible incidents.')
count = 1
incidents = [
self.run_t_bone_crash,
self.run_linear_crash,
self.run_pole_crash,
self.run_no_crash,
]
while not self.settings_exhausted():
log.info('Running incident %s of %s...', count,
self.total_possibilities)
self.bng.restart_scenario()
log.info('Scenario restarted.')
time.sleep(5.0)
self.vehicle_b.set_part_config(self.vehicle_b_config)
log.info('Vehicle B config set.')
incident = incidents[count % len(incidents)]
if self.run_incident(incident) is None:
log.info('Ran out of options for: %s', incident)
incidents.remove(incident) # Possibility space exhausted
count += 1
def run(self):
log.info('Starting up BeamNG instance.')
self.bng.open(['impactgen/crashOutput'])
self.bng.skt.settimeout(1000)
try:
log.info('Setting up BeamNG instance.')
self.setup()
self.run_incidents()
finally:
log.info('Closing BeamNG instance.')
self.bng.close()
class 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 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()
