def test_gforces(beamng):
with beamng as bng:
scenario = Scenario('west_coast_usa', 'gforce_test')
vehicle = Vehicle('test_car', model='etk800')
gforces = GForces()
vehicle.attach_sensor('gforces', gforces)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=(0, 0, 45))
scenario.make(beamng)
gx = []
gy = []
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(120)
vehicle.ai_set_aggression(2)
vehicle.ai_set_mode('span')
for _ in range(64):
bng.step(30)
vehicle.poll_sensors()
gx.append(gforces.data['gx'])
gy.append(gforces.data['gy'])
assert np.var(gx) > 1 and np.var(gy) > 1
def test_ultrasonic(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'ultrasonic_test')
cube_dist = 4
cube = ProceduralCube(name='cube',
pos=(0, -cube_dist, 5),
rot=None,
rot_quat=(0, 0, 0, 1),
size=(1, 20, 10))
scenario.add_procedural_mesh(cube)
pos = (0, 1, 2)
rot = (0, 1, 0)
ultrasonic = Ultrasonic(pos, rot)
vehicle = Vehicle('test', model='pickup')
vehicle.attach_sensor('ultrasonic', ultrasonic)
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot_quat=(0, 0, 0, 1))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
vehicle.poll_sensors()
assert 0 < vehicle.sensors['ultrasonic'].data['distance'] < cube_dist
def test_noise(beamng):
with beamng as bng:
scenario = Scenario('west_coast_usa', 'camera_test')
vehicle = Vehicle('test_car', model='etk800')
pos = (-0.3, 1, 1.0)
direction = (0, 1, 0)
fov = 120
resolution = (64, 64)
cam = Camera(pos, direction, fov, resolution, colour=True, depth=True)
noise_cam = RandomImageNoise(cam)
vehicle.attach_sensor('noise_cam', noise_cam)
lidar = Lidar()
noise_lidar = RandomLIDARNoise(lidar, mean=0, var=.5)
vehicle.attach_sensor('noise_lidar', noise_lidar)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=(0, 0, 45))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.step(120)
time.sleep(20)
vehicle.poll_sensors()
reference_img = np.array(noise_cam._sensor.data['colour'])
noise_img = np.array(noise_cam.data['colour'])
assert (not (np.array_equal(noise_img, reference_img)))
ref_pc = lidar.data['points']
noise_pc = noise_lidar.data['points']
assert (not (np.array_equal(ref_pc, noise_pc)))
def test_noise(beamng):
scenario = Scenario('west_coast_usa', 'camera_test')
vehicle = Vehicle('test_car', model='etk800')
pos = (-0.3, 1, 1.0)
direction = (0, 1, 0)
fov = 120
resolution = (64, 64)
reference_camera = Camera(pos, direction, fov, resolution, colour=True)
vehicle.attach_sensor('reference_camera', reference_camera)
noise_camera = Camera(pos, direction, fov, resolution, colour=True)
noise_camera = WhiteGaussianRGBNoise(noise_camera, .5, 0)
vehicle.attach_sensor('noise_cam', noise_camera)
scenario.add_vehicle(vehicle, pos=(-717.121, 101, 118.675), rot=(0, 0, 45))
scenario.make(beamng)
with beamng as bng:
bng.load_scenario(scenario)
bng.step(120)
vehicle.poll_sensors()
noise_img = np.asarray(noise_camera.data['colour'])
reference_img = np.asarray(reference_camera.data['colour'])
# since this is based on two different renders this will always be different
assert (not (np.array_equal(noise_img, reference_img)))
def test_bboxes(beamng):
with beamng as bng:
scenario = Scenario('west_coast_usa', 'bbox_test')
ego = Vehicle('ego', model='etk800', color='White')
scenario.add_vehicle(ego,
pos=(-725.365, 92.4684, 118.437),
rot_quat=(-0.006, -0.0076, 0.921, -0.389))
camera = Camera((-0.3, 1, 1), (0, 1, 0),
75, (1024, 1024),
colour=True,
depth=True,
annotation=True,
instance=True)
ego.attach_sensor('camera', camera)
car1 = Vehicle('car1', model='etk800', color='Green')
scenario.add_vehicle(car1,
pos=(-710.76, 101.50, 118.56),
rot_quat=(-0.006, -0.0076, 0.921, -0.389))
car2 = Vehicle('car2', model='etk800', color='Red')
scenario.add_vehicle(car2,
pos=(-715.83, 96.69, 118.53),
rot_quat=(-0.006, -0.0076, 0.921, -0.389))
car3 = Vehicle('car3', model='etki', color='Blue')
scenario.add_vehicle(car3,
pos=(-696.96, 126.9, 118.44),
rot_quat=(0.0181, -0.0065, 0.3816, 0.924))
car4 = Vehicle('car4', model='miramar', color='Black')
scenario.add_vehicle(car4,
pos=(-708.58203, 115.326, 118.60),
rot_quat=(0.0181, -0.00645, 0.3818, 0.9240))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.step(120)
time.sleep(5)
ego.poll_sensors()
classes = bng.get_annotation_classes(bng.get_annotations())
annotation = ego.sensors['camera'].data['annotation']
instance = ego.sensors['camera'].data['instance']
bboxes = Camera.extract_bboxes(annotation, instance, classes)
bboxes = [b for b in bboxes if b['class'] == 'CAR']
assert len(bboxes) == 5
def test_vehicle_move(beamng):
with beamng as bng:
bng.set_steps_per_second(50)
bng.set_deterministic()
scenario = Scenario('smallgrid', 'move_test')
vehicle = Vehicle('test_car', model='etk800')
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
scenario.make(bng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.pause()
vehicle.control(throttle=1)
bng.step(120, wait=True)
vehicle.poll_sensors()
assert np.linalg.norm(vehicle.sensors['state'].data['pos']) > 1
scenario.delete(beamng)
def test_state(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'vehicle_state_test')
vehicle = Vehicle('test_car', model='pickup')
state = State()
vehicle.attach_sensor('newstate', state)
scenario.add_vehicle(vehicle, pos=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(20)
vehicle.poll_sensors()
assert state.data['pos'][0] < 0.1
def test_vehicle_spawn(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'spawn_test')
vehicle = Vehicle('irrelevant', model='pickup')
scenario.add_vehicle(vehicle, pos=(0, 0, 0), rot=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
other = Vehicle('relevant', model='etk800')
scenario.add_vehicle(other, pos=(10, 10, 0), rot=(0, 0, 0))
other.poll_sensors()
assert other.sensors['state'].connected
assert 'pos' in other.sensors['state'].data
bng.step(120, wait=True)
scenario.remove_vehicle(other)
bng.step(600, wait=True)
assert not other.sensors['state'].connected
def test_multicam(beamng):
with beamng as bng:
scenario = Scenario('west_coast_usa', 'camera_test')
vehicle = Vehicle('test_car', model='etk800')
pos = (-0.3, 1, 1.0)
direction = (0, 1, 0)
fov = 120
resolution = (64, 64)
front_cam1 = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True,
instance=True,
shmem=True)
vehicle.attach_sensor('front_cam1', front_cam1)
front_cam2 = Camera(pos,
direction,
fov,
resolution,
colour=True,
depth=True,
annotation=True,
instance=True,
shmem=True)
vehicle.attach_sensor('front_cam2', front_cam2)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=(0, 0, 45))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.pause()
bng.step(120)
time.sleep(20)
vehicle.poll_sensors()
def test_lidar(beamng, shmem):
with beamng as bng:
scenario = Scenario('west_coast_usa', 'lidar_test')
vehicle = Vehicle('test_car', model='etk800')
lidar = Lidar(shmem=shmem)
vehicle.attach_sensor('lidar', lidar)
scenario.add_vehicle(vehicle,
pos=(-717.121, 101, 118.675),
rot=(0, 0, 45))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.step(120)
time.sleep(20)
vehicle.poll_sensors()
arr = lidar.data['points']
ref = arr[0]
eq = arr[np.where(arr == ref)]
assert eq.size != arr.size
def test_damage(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'damage_test')
dummy = Vehicle('dummy', model='pickup')
scenario.add_vehicle(dummy, pos=(0, 0, 0))
scenario.make(beamng)
vehicle = Vehicle('test_car', model='etk800')
damage = Damage()
vehicle.attach_sensor('damage', damage)
bng.load_scenario(scenario)
bng.start_scenario()
scenario.add_vehicle(vehicle,
pos=(0, 0, 32),
rot=(-90, 0, 0),
cling=False)
bng.step(600)
vehicle.poll_sensors()
assert damage.data['damage'] > 100
def test_electrics(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'electrics_test')
vehicle = Vehicle('test_car', model='etk800')
electrics = Electrics()
vehicle.attach_sensor('electrics', electrics)
scenario.add_vehicle(vehicle, pos=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(120)
vehicle.control(throttle=1.0)
bng.step(360)
vehicle.poll_sensors()
assert electrics.data['airspeed'] > 0
assert electrics.data['wheelspeed'] > 0
assert electrics.data['throttle_input'] > 0
def test_imu(beamng):
with beamng as bng:
scenario = Scenario('smallgrid', 'vehicle_state_test')
vehicle = Vehicle('test_car', model='etk800')
imu_pos = IMU(pos=(0.73, 0.51, 0.8), debug=True)
imu_node = IMU(node=0, debug=True)
vehicle.attach_sensor('imu_pos', imu_pos)
vehicle.attach_sensor('imu_node', imu_node)
scenario.add_vehicle(vehicle, pos=(0, 0, 0))
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
bng.step(20)
pax, pay, paz, pgx, pgy, pgz = [], [], [], [], [], []
for _ in range(30):
# Stand still, sample IMU
bng.step(60)
vehicle.poll_sensors()
pax.append(imu_pos.data['aX'])
pay.append(imu_pos.data['aY'])
paz.append(imu_pos.data['aZ'])
pgx.append(imu_pos.data['gX'])
pgy.append(imu_pos.data['gY'])
pgz.append(imu_pos.data['gZ'])
# Some slight movement is bound to happen since the engine is one and
# the vehicle isn't perfectly stable; hence no check for == 0
assert np.mean(pax) < 1
assert np.mean(pay) < 1
assert np.mean(paz) < 1
assert np.mean(pgx) < 1
assert np.mean(pgy) < 1
assert np.mean(pgz) < 1
pax, pay, paz, pgx, pgy, pgz = [], [], [], [], [], []
nax, nay, naz, ngx, ngy, ngz = [], [], [], [], [], []
for _ in range(30):
# Drive randomly, sample IMU
t = random.random() * 2 - 1
s = random.random() * 2 - 1
vehicle.control(throttle=t, steering=s)
bng.step(60)
vehicle.poll_sensors()
pax.append(imu_pos.data['aX'])
pay.append(imu_pos.data['aY'])
paz.append(imu_pos.data['aZ'])
pgx.append(imu_pos.data['gX'])
pgy.append(imu_pos.data['gY'])
pgz.append(imu_pos.data['gZ'])
nax.append(imu_node.data['aX'])
nay.append(imu_node.data['aY'])
naz.append(imu_node.data['aZ'])
ngx.append(imu_node.data['gX'])
ngy.append(imu_node.data['gY'])
ngz.append(imu_node.data['gZ'])
for arr in [pax, pay, paz, pgx, pgy, pgz]:
assert np.max(arr) > 0.01
assert np.min(arr) < -0.01
# See if IMU at different position ended up with different measurements
for parr, narr in zip([pax, pay, paz, pgx, pgy, pgz],
[nax, nay, naz, ngx, ngy, ngz]):
assert np.mean(parr) != np.mean(narr)
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 test_vehicle_ai(beamng):
with beamng as bng:
bng.set_steps_per_second(50)
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()
bng.switch_vehicle(vehicle)
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.poll_sensors()
ref = [script[1]['x'], script[1]['y'], script[1]['z']]
pos = vehicle.sensors['state'].data['pos']
ref, pos = np.array(ref), np.array(pos)
assert np.linalg.norm(ref - pos) < 2.5
scenario.delete(beamng)
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")
def test_quats(beamng):
with beamng as bng:
setup_logging()
scenario = Scenario('smallgrid', 'test_quat')
blue_etk = Vehicle('ego_vehicle',
model='etk800',
color='Blue',
licence="angle")
scenario.add_vehicle(blue_etk, pos=(0, 0, 0), rot=(0, 0, 0))
blue_etk = Vehicle('ego_vehicle2',
model='etk800',
color='Green',
license="quat")
rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)
scenario.add_vehicle(blue_etk, pos=(5, 0, 0), rot_quat=rot_quat)
rb = ScenarioObject(oid='roadblock',
name='sawhorse',
otype='BeamNGVehicle',
pos=(-10, -5, 0),
rot=(0, 0, 0),
scale=(1, 1, 1),
JBeam='sawhorse',
datablock="default_vehicle")
scenario.add_object(rb)
cn = ScenarioObject(oid='cones',
name='cones',
otype='BeamNGVehicle',
pos=(0, -5, 0),
rot=None,
rot_quat=(0, 0, 0, 1),
scale=(1, 1, 1),
JBeam='cones',
datablock="default_vehicle")
scenario.add_object(cn)
scenario.make(beamng)
bng.load_scenario(scenario)
bng.start_scenario()
white_etk = Vehicle('ego_vehicle3', model='etk800', color='White')
bng.spawn_vehicle(white_etk, (-10, 0, 0), (0, 0, 0))
pickup = Vehicle('ego_vehicle4', model='pickup')
pos = (-15, 0, 0)
bng.spawn_vehicle(pickup, pos, None, rot_quat=(0, 0, 0, 1))
resp = bng.get_current_vehicles()
assert len(resp) == 6
pickup.connect(bng)
pickup.poll_sensors()
pos_before = pickup.state['pos']
bng.teleport_vehicle(pickup.vid, pos, rot=(0, 45, 0))
pickup.poll_sensors()
pos_after = pickup.state['pos']
assert (pos_before != pos_after)
pickup.poll_sensors()
pos_before = pickup.state['pos']
rot_quat = (-0.00333699025, -0.00218820246, -0.689169466, 0.724589229)
bng.teleport_vehicle(pickup.vid, pos, rot_quat=rot_quat)
pickup.poll_sensors()
pos_after = pickup.state['pos']
assert (pos_before != pos_after)
try:
bng.teleport_scenario_object(rb, (-10, 5, 0), rot=(-45, 0, 0))
assert True
except socket.timeout:
assert False
try:
rot_quat = (-0.003337, -0.0021882, -0.6891695, 0.7245892)
bng.teleport_scenario_object(rb, (-10, 5, 0), rot_quat=rot_quat)
assert True
except socket.timeout:
assert False
