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