def upload_geo_data(scf, geometries):
for bs, data in geometries.items():
geo = LighthouseBsGeometry()
geo.rotation_matrix = data[0]
geo.origin = data[1]
geo.valid = data[2]
WriteMem(scf, bs, geo)
def upload_geo_data(scf, geometries):
bs1 = LighthouseBsGeometry()
bs1.rotation_matrix = geometries[0][0]
bs1.origin = geometries[0][1]
bs1.valid = geometries[0][2]
bs2 = LighthouseBsGeometry()
bs2.rotation_matrix = geometries[1][0]
bs2.origin = geometries[1][1]
bs2.valid = geometries[1][2]
WriteMem(scf, bs1, bs2)
def _sweep_angles_received_and_averaged_cb(self, averaged_angles):
self._averaged_angles = averaged_angles
estimator = LighthouseBsGeoEstimator()
self._newly_estimated_geometry = {}
for id, average_data in averaged_angles.items():
sensor_data = average_data[1]
rotation_bs_matrix, position_bs_vector = estimator.estimate_geometry(
sensor_data)
geo = LighthouseBsGeometry()
geo.rotation_matrix = rotation_bs_matrix
geo.origin = position_bs_vector
geo.valid = True
self._newly_estimated_geometry[id] = geo
self._update_ui()
def read(file_name):
file = open(file_name, 'r')
with file:
data = yaml.safe_load(file)
if LighthouseConfigFileManager.TYPE_ID not in data:
raise Exception('Type field missing')
if data[LighthouseConfigFileManager.TYPE_ID] != LighthouseConfigFileManager.TYPE:
raise Exception('Unsupported file type')
if LighthouseConfigFileManager.VERSION_ID not in data:
raise Exception('Version field missing')
if data[LighthouseConfigFileManager.VERSION_ID] != LighthouseConfigFileManager.VERSION:
raise Exception('Unsupported file version')
result_geos = {}
result_calibs = {}
if LighthouseConfigFileManager.GEOS_ID in data:
for id, geo in data[LighthouseConfigFileManager.GEOS_ID].items():
result_geos[id] = LighthouseBsGeometry.from_file_object(geo)
if LighthouseConfigFileManager.CALIBS_ID in data:
for id, calib in data[LighthouseConfigFileManager.CALIBS_ID].items():
result_calibs[id] = LighthouseBsCalibration.from_file_object(calib)
return result_geos, result_calibs
def _prepare_geos(self, geos):
result = None
if geos is not None:
result = dict(geos)
# Pad for base stations without data
empty_geo = LighthouseBsGeometry()
for id in range(self._nr_of_base_stations):
if id not in result:
result[id] = empty_geo
return result
def estimate(self, uri, do_write):
cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(uri, cf=cf) as scf:
print("Reading sensor data...")
sweep_angle_reader = LighthouseSweepAngleAverageReader(
scf.cf, self.angles_collected_cb)
sweep_angle_reader.start_angle_collection()
self.collection_event.wait()
print("Estimating position of base stations...")
geometries = {}
estimator = LighthouseBsGeoEstimator()
for id in sorted(self.sensor_vectors_all.keys()):
average_data = self.sensor_vectors_all[id]
sensor_data = average_data[1]
rotation_bs_matrix, position_bs_vector = estimator.estimate_geometry(
sensor_data)
is_valid = estimator.sanity_check_result(position_bs_vector)
if is_valid:
geo = LighthouseBsGeometry()
geo.rotation_matrix = rotation_bs_matrix
geo.origin = position_bs_vector
geo.valid = True
geometries[id] = geo
self.print_geo(rotation_bs_matrix, position_bs_vector,
is_valid)
else:
print("Warning: could not find valid solution for " + id +
1)
print()
if do_write:
print("Uploading geo data to CF")
helper = LighthouseMemHelper(scf.cf)
helper.write_geos(geometries, self.write_done_cb)
self.write_event.wait()
def get_geometry():
geometry = []
for i in range(openvr.k_unMaxTrackedDeviceCount):
device_class = vr.getTrackedDeviceClass(i)
if device_class == openvr.TrackedDeviceClass_TrackingReference:
poses = vr.getDeviceToAbsoluteTrackingPose(
openvr.TrackingUniverseStanding, 0, i)
if not poses or len(poses) < i + 1:
raise Exception(
"Could not get pose for OpenVR device with index: " +
str(i))
pose = poses[i]
if pose and pose.bPoseIsValid:
poseMatrix = pose.mDeviceToAbsoluteTracking
positionGeometry = LighthouseBsGeometry()
positionGeometry.origin = getPosition(poseMatrix)
positionGeometry.rotation_matrix = getRotation(poseMatrix)
positionGeometry.valid = True
geometry.append(positionGeometry)
else:
raise Exception(
"Got invalid pose for OpenVR device with index: " + str(i))
return geometry
mem.dump()
self.got_data = True
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
with open(os.path.join(sys.path[0], "bs_position.txt"),
encoding="utf-8") as f:
text = f.read()
match = re.findall(r'-?\d.\d+', text)
numberList = [float(i) for i in match]
bs1 = LighthouseBsGeometry()
bs1.origin = numberList[0:3]
bs1.rotation_matrix = [numberList[3:6], numberList[6:9], numberList[9:12]]
bs2 = LighthouseBsGeometry()
bs2.origin = numberList[12:15]
bs2.rotation_matrix = [
numberList[15:18], numberList[18:21], numberList[21:24]
]
# URIs to the drones that should be updated
uris = [
'radio://0/80/2M/E7E7E7E7E0'
# 'radio://0/80/2M/E7E7E7E7E1',
# 'radio://0/80/2M/E7E7E7E7E2',
# 'radio://0/80/2M/E7E7E7E7E3',
# 'radio://0/80/2M/E7E7E7E7E4',
import time
import signal
import sys
import openvr
import math
import numpy as np
uri = None
vr = None
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
baseOne = LighthouseBsGeometry()
baseTwo = LighthouseBsGeometry()
baseStationIndexes = []
baseStations = [baseOne, baseTwo]
dataWrittenEvent = Event()
START_Z = 0.0
# Rotation matrixes to convert to the CF coordinate system
openvr_to_cf = np.array([
[0.0, 0.0, -1.0],
[-1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
])
cf_to_openvr = np.array([
if __name__ == "__main__":
print("Let's go!")
print("First, let's get the positions of the base stations")
print("___________________________________________________")
bsPoses, offset = getBsPos()
print("___________________________________________________")
if len(bsPoses) is not 2 or bsPoses[0] is None or bsPoses[1] is None:
print("Well, something went wrong, sorry")
print("Number of returned base station positions: %d" % len(bsPoses))
for i in range(len(bsPoses)):
print("Type of pose %d is: %s" % (i, type(bsPoses[i])))
print("Is SteamVR running? If not, that might just be the problem")
exit(1)
# formatting the positions
bs1 = LighthouseBsGeometry()
pose1 = bsPoses[0]
bs1.origin = [
pose1[0][3] - offset[0], pose1[1][3] - offset[1],
pose1[2][3] - offset[2]
]
bs1.rotation_matrix = [pose1[0][:3], pose1[1][:3], pose1[2][:3]]
bs2 = LighthouseBsGeometry()
pose2 = bsPoses[1]
bs2.origin = [
pose2[0][3] - offset[0], pose2[1][3] - offset[1],
pose2[2][3] - offset[2]
]
bs2.rotation_matrix = [pose2[0][:3], pose2[1][:3], pose2[2][:3]]
def _data_written(self, mem, addr):
self.data_written = True
def _data_failed(self, mem, addr):
raise Exception('Write to RAM failed')
def _data_persisted(self, data):
if (data.data):
print('Data persisted')
else:
raise Exception("Write to storage failed")
self.result_received = True
geo0 = LighthouseBsGeometry()
geo0.origin = [-1.01977998, -0.19424433, 1.97086964]
geo0.rotation_matrix = [
[0.66385385, -0.26347329, 0.6999142],
[0.18206993, 0.96466617, 0.19044612],
[-0.72536102, 0.00100494, 0.68836792],
]
geo0.valid = True
geo1 = LighthouseBsGeometry()
geo1.origin = [0, 0, 0]
geo1.rotation_matrix = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
geo1.valid = False
calib0 = LighthouseBsCalibration()
calib0.sweeps[0].tilt = -0.047353
if success:
print('Data written')
else:
print('Write failed')
self._event.set()
if __name__ == '__main__':
# URI to the Crazyflie to connect to
uri = 'radio://0/80'
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
bs1geo = LighthouseBsGeometry()
bs1geo.origin = [1.0, 2.0, 3.0]
bs1geo.rotation_matrix = [
[4.0, 5.0, 6.0],
[7.0, 8.0, 9.0],
[10.0, 11.0, 12.0],
]
bs1geo.valid = True
bs2geo = LighthouseBsGeometry()
bs2geo.origin = [21.0, 22.0, 23.0]
bs2geo.rotation_matrix = [
[24.0, 25.0, 26.0],
[27.0, 28.0, 29.0],
[30.0, 31.0, 32.0],
]
while not self.data_written:
time.sleep(1)
def _data_written(self, mem, addr):
self.data_written = True
print('Data written')
if __name__ == '__main__':
# URI to the Crazyflie to connect to
uri = 'radio://0/80/2M'
# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)
bs1 = LighthouseBsGeometry()
bs1.origin = [1.0, 2.0, 3.0]
bs1.rotation_matrix = [
[4.0, 5.0, 6.0],
[7.0, 8.0, 9.0],
[10.0, 11.0, 12.0],
]
bs2 = LighthouseBsGeometry()
bs2.origin = [21.0, 22.0, 23.0]
bs2.rotation_matrix = [
[24.0, 25.0, 26.0],
[27.0, 28.0, 29.0],
[30.0, 31.0, 32.0],
]
def __init__(self, index, vrSystem):
self.index = index
self.vrSystem = vrSystem
self.positionGeometry = LighthouseBsGeometry()
self.initialized = False
self.initialize()
