def osc_bundle(contents):
builder = osc_bundle_builder.OscBundleBuilder(osc_bundle_builder.IMMEDIATELY)
if not isinstance(contents, Iterable):
contents = [contents]
for content in contents:
builder.add_content(content)
return builder.build()
def play_one(cursor):
messages = []
for (name, track) in state.tracks.items():
if track[cursor]:
mute = state.tracks_tmp_mute.get(name, 0)
if mute > 0:
state.tracks_tmp_mute[name] = mute - 1
continue
msg_builder = osc_message_builder.OscMessageBuilder(address=name)
for value in state.tracks_params[name]:
if callable(value):
value = value()
msg_builder.add_arg(
value, msg_builder.ARG_TYPE_FLOAT if type(value) == float
else msg_builder.ARG_TYPE_INT) # todo: improve this shit
messages.append(msg_builder.build())
if len(messages) > 0:
bundle_builder = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for msg in messages:
bundle_builder.add_content(msg)
client.send(bundle_builder.build())
def parse_bundle(bnd_id):
success = True
bnd_items = config_dict["BUNDLES"][bnd_id]
if bnd_items is not None:
if bnd_id in messages_and_bundles:
print(
"ERROR: Bundle parsing failed. Name clashes with the message \""
+ bnd_id + "\".")
success = False
else:
osc_bnd = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for item in bnd_items:
if item in messages_and_bundles:
osc_bnd.add_content(messages_and_bundles[item])
elif parse_bundle(item):
osc_bnd.add_content(messages_and_bundles[item])
else:
print(
"ERROR: Bundle parsing failed. No message or bundle found with the name \""
+ item + "\".")
success = False
break
if success:
messages_and_bundles[bnd_id] = osc_bnd.build()
config_dict["BUNDLES"][bnd_id] = None
return success
def build_bundle(timetag, msg_addr, msg_args):
"""Builds pythonsosc OSC bundle
Parameters
----------
timetag : int
Time at which bundle content should be executed.
msg_addr : str
SuperCollider address.
msg_args : list
List of arguments to add to message.
Returns
-------
OscBundle
Bundle ready to be sent.
"""
if msg_args is None:
msg_args = []
if timetag < 1e6:
timetag = time.time() + timetag
bundle = osc_bundle_builder.OscBundleBuilder(timetag)
msg = build_message(msg_addr, msg_args)
bundle.add_content(msg)
bundle = bundle.build()
return bundle
def make_bundle(**kwargs):
bb = osc_bundle_builder.OscBundleBuilder(
timestamp=osc_bundle_builder.IMMEDIATELY)
for key, value in kwargs.items():
mb = osc_message_builder.OscMessageBuilder(address=f'/{key}')
mb.add_arg(value)
bb.add_content(mb.build())
return bb.build()
def start_tuio_bundle(self, dimension, source):
""" Start building a new TUIO bundle"""
self.current_tuio_frame_bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
frame_message = self.init_tuio_frame(dimension, source)
self.current_tuio_frame_bundle.add_content(frame_message.build())
def sendOSCData():
global OSC_STATUS_CODE
try:
# build all osc messages for the controller values
controller_bundle_builder = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for key in controller_values:
# differentiate between buttons and continuous inputs
if "i" in controller_data_types[key]:
address = "{}{}/{}".format(CONTROLLER_ROOT_ADDRESS,
BUTTONS_ADDRESS, key)
elif controller_data_types[key] == "f":
address = "{}{}/{}".format(CONTROLLER_ROOT_ADDRESS,
CONTINUOUS_INPUTS_ADDRESS, key)
msg = osc_message_builder.OscMessageBuilder(address)
if key == 'DPAD':
msg.add_arg(controller_values[key][0],
controller_data_types[key][0])
msg.add_arg(controller_values[key][1],
controller_data_types[key][1])
else:
msg.add_arg(controller_values[key], controller_data_types[key])
controller_bundle_builder.add_content(msg.build())
# build all osc messages for the analysis values
analysis_bundle_builder = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for key in analysis_values:
msg = osc_message_builder.OscMessageBuilder("{}/{}".format(
ANALYSIS_ROOT_ADDRESS, key))
msg.add_arg(analysis_values[key], "f")
analysis_bundle_builder.add_content(msg.build())
# build final bundle
osc_data = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
osc_data.add_content(controller_bundle_builder.build())
osc_data.add_content(analysis_bundle_builder.build())
# send bundles over network
client.send(osc_data.build())
except InterruptedError:
OSC_STATUS_CODE = 1
except OSError:
OSC_STATUS_CODE = 2
else:
OSC_STATUS_CODE = 0
def send_max_minus_to_master():
client = udp_client.SimpleUDPClient("192.168.4.1", 9001)
msg = osc_message_builder.OscMessageBuilder(address="/counter/max_minus")
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg.add_arg(0)
bundle.add_content(msg.build())
bundle = bundle.build()
client.send(bundle)
def send_bundle():
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for c in channels:
bundle.add_content(make_message(c, randint(0, 1024)))
sub_bundle = bundle.build()
bundle.add_content(sub_bundle)
bundle = bundle.build()
client.send(bundle)
def send_osc(self, dev_id, values):
bundle = osc_bundle_builder.OscBundleBuilder(osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address=output_address)
# Test bundle outputs: 1, 2, 3, ...
print(values[0])
for val in values:
msg.add_arg(val)
bundle.add_content(msg.build())
bundle = bundle.build()
client.send(bundle)
def send(self, val):
## osc stuff
# client.send_message("/hrm", val)
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address="/hrm")
msg.add_arg(val)
bundle.add_content(msg.build())
client.send(bundle.build())
def send_counter_anfrage():
client = udp_client.SimpleUDPClient("192.168.4.1", 9001)
msg = osc_message_builder.OscMessageBuilder(
address="/counter/counter_info")
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg.add_arg(0)
bundle.add_content(msg.build())
bundle = bundle.build()
client.send(bundle)
print("Counter Anfrage gesendet")
root.after(5000, send_counter_anfrage)
def output_individual(data, labels):
data = json.loads(data)
printJSON(data)
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for label in labels:
output_address += self.device_number + "/bitalino/" + label
msg = osc_message_builder.OscMessageBuilder(
address=output_address)
msg.add_arg(arg_to_add)
msg.build()
client.send(msg)
async def sendTestBundle(self, numOutputs):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(
address=self.output_address)
# Test bundle outputs: 1, 2, 3, ...
testVal = 1.0
for i in range(numOutputs):
msg.add_arg(testVal)
testVal = testVal + 1.0
bundle.add_content(msg.build())
bundle = bundle.build()
self.client.send(bundle)
def send_osc_bundle(self, address, args):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address="/array" + address)
for arg in args:
msg.add_arg(arg)
bundle.add_content(msg.build())
bundle = bundle.build()
for client in self.clients:
client.send(bundle)
def send_counter_info(adress_send_to):
global max_people_allowed, people_inside
client = udp_client.SimpleUDPClient(adress_send_to, 9001)
msg = osc_message_builder.OscMessageBuilder(address="/counter_info")
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg.add_arg(max_people_allowed)
msg.add_arg(people_inside)
bundle.add_content(msg.build())
bundle = bundle.build()
print("counter_info an {} gesendet mit max {} und inside {}".format(
adress_send_to, max_people_allowed, people_inside),
flush=True)
client.send(bundle)
def sendMessage(self, module_path, command_name, *args, **kwargs):
module_path = "/{0}".format(module_path.replace(".", "/"))
osc_bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for key, value in kwargs.items():
msg = osc_message_builder.OscMessageBuilder(
address="{0}/attributes/{1}".format(module_path, key))
msg.add_arg(value)
osc_bundle.add_content(msg.build())
msg = osc_message_builder.OscMessageBuilder(
address="{0}/{1}".format(module_path, "operation"))
msg.add_arg(command_name)
osc_bundle.add_content(msg.build())
osc_bundle = osc_bundle.build()
self._client.send(osc_bundle)
def sendMessage(self, mod_type, data, disp, ip=None):
"""Send OSC message with [data] to the given socket and ip address."""
ip = ip or self.__servervalidation.ipaddress
client = SimpleUDPClient(ip, self.__servervalidation.port + disp)
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(
address=self.__servervalidation.basePath + mod_type.value[1])
msg.add_arg(self.__servervalidation.id_lg)
msg.add_arg(mod_type.value[0])
msg.add_arg(data)
bundle.add_content(msg.build())
bundle = bundle.build()
client.send(bundle)
def send_counter_info_to_all():
global max_people_allowed, people_inside
print(checked_in_ips)
for i in range(len(checked_in_ips)):
client = udp_client.SimpleUDPClient(checked_in_ips[i], 9001)
msg = osc_message_builder.OscMessageBuilder(address="/counter_info")
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg.add_arg(max_people_allowed)
msg.add_arg(people_inside)
bundle.add_content(msg.build())
bundle = bundle.build()
print("counter_info an {} gesendet mit max {} und inside {}".format(
checked_in_ips[i], max_people_allowed, people_inside),
flush=True)
client.send(bundle)
def loop():
global slowPrint
runtime = (time.time() - START_TIME)
alpha = 255
POWER = 255
# client.send_message("/hrm", random.random() )
# time.sleep(.33)
bpm = p.BPM
if bpm > 0:
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address="/hrm")
beatDiff = abs(p2.lastBeatTime - p.lastBeatTime)
multiplier = (MAX_SYNC - beatDiff) / MAX_SYNC
if multiplier < 0:
multiplier = 0
msg.add_arg(p.normal)
bundle.add_content(msg.build())
msg.add_arg(p2.normal)
bundle.add_content(msg.build())
# msg.add_arg(p.led)
# bundle.add_content(msg.build())
# msg.add_arg(p.longAverage)
# bundle.add_content(msg.build())
client.send(bundle.build())
pixels.fill((int(((p.led * p2.led) * multiplier) * 255), 0, 0))
pixels.show()
print("diff: ", beatDiff)
if runtime % 1 <= 0.1 and slowPrint:
slowPrint = False
print("BPM: %d " % p2.BPM)
elif runtime % 1 > 0.1:
slowPrint = True
else:
if runtime % printInterval <= 0.1 and slowPrint:
slowPrint = False
print("No Heartbeat found")
elif runtime % printInterval > 0.1:
slowPrint = True
async def output_individual(self, all_data, whole_sequence=0):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for id in range(len(all_data)):
data = json.loads(all_data[id])
while len(json.loads(json.dumps(data))) is 0:
await asyncio.sleep(1.0)
for label, output_buffer in data.items():
self.output_address = self.generate_output_message(id) + "/" + label
msg = osc_message_builder.OscMessageBuilder(
address=self.output_address)
arg_to_add = data[label] if whole_sequence == 1 else data[label][0]
msg.add_arg(arg_to_add)
bundle.add_content(msg.build())
bundle = bundle.build()
self.client.send(bundle)
await asyncio.sleep(0.0)
def test_build_complex_bundle(self):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address="/SYNC")
msg.add_arg(4.0)
# Add 4 messages in the bundle, each with more arguments.
bundle.add_content(msg.build())
msg.add_arg(2)
bundle.add_content(msg.build())
msg.add_arg("value")
bundle.add_content(msg.build())
msg.add_arg(b"\x01\x02\x03")
bundle.add_content(msg.build())
sub_bundle = bundle.build()
# Now add the same bundle inside itself.
bundle.add_content(sub_bundle)
bundle = bundle.build()
self.assertEqual(5, bundle.num_contents)
def sendMessage(self, module_path, command_name, *args, **kwargs):
if command_name:
module_path = "/{0}/{1}".format(module_path.replace(
".",
"/",
), command_name)
else:
module_path = module_path
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address=module_path)
#self._addArgument(msg, "command_id", OSCMessageSender.getCommandId())
#self._addArgument(msg, "command_name", command_name)
for name, value in kwargs.items():
self._addArgument(msg, name, value)
for name in args:
msg.add_arg(name)
bundle.add_content(msg.build())
bundle = bundle.build()
self._client.send(bundle)
def send(self, event : Event):
''' Sends OSC message according to definition of event.
Note that event.as_osc_message() returns
2 arguments: address, and message.
'''
address, message = event.as_osc_message()
bundle = osc_bundle_builder.OscBundleBuilder(osc_bundle_builder.IMMEDIATELY)
msg = osc_message_builder.OscMessageBuilder(address=address)
types = ['i']*2 + ['f']*4
for item, type in zip(message, types):
if item is None:
item = 0
if type == 'i':
item = int(item)
elif type == 'f':
item = float(item)
msg.add_arg(item, type)
bundle.add_content(msg.build())
bundle = bundle.build()
self.client.send(bundle)
async def output_bundle(self, all_data, whole_sequence=0):
#printJSON(data)
# bundle = osc_bundle_builder.OscBundleBuilder(
# osc_bundle_builder.IMMEDIATELY)
for id in range(len(all_data)):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
data = json.loads(all_data[id])
while len(json.loads(json.dumps(data))) is 0:
await asyncio.sleep(1.0)
self.output_address = self.generate_output_message(id)
# print(self.output_address)
msg = osc_message_builder.OscMessageBuilder(
address=self.output_address)
for label, output_buffer in data.items():
# print (label + " " + str(output_buffer[0]))
arg_to_add = output_buffer if whole_sequence == 1 else output_buffer[0]
msg.add_arg(arg_to_add)
bundle.add_content(msg.build())
bundle = bundle.build()
self.client.send(bundle)
def update(self):
bundle = osc_bundle_builder.OscBundleBuilder(osc_bundle_builder.IMMEDIATELY)
i = 0
while (i < self.BEAD_COUNT):
#print("bead {}".format(i))
msg = osc_message_builder.OscMessageBuilder(address = "/beadf")
msg.add_arg(i)
msg.add_arg(float(self.beads[i].color.r))
msg.add_arg(float(self.beads[i].color.g))
msg.add_arg(float(self.beads[i].color.b))
msg = msg.build()
bundle.add_content(msg)
i = i + 1
msg = osc_message_builder.OscMessageBuilder(address = "/update")
msg = msg.build()
bundle.add_content(msg)
bundle = bundle.build()
self.osc_client.send(bundle)
def build_bundle(timetag, msg_addr, msg_args):
"""Builds pythonsosc OSC bundle
Arguments:
timetag {int} -- Time at which bundle content
should be executed, either
in absolute or relative time
msg_addr {str} -- SuperCollider address
E.g. '/s_new'
msg_args {list} -- List of arguments to add
to message
Returns:
OscBundle -- Bundle ready to be sent
"""
if timetag < 1e6:
timetag = time.time() + timetag
bundle = osc_bundle_builder.OscBundleBuilder(timetag)
msg = build_message(msg_addr, msg_args)
bundle.add_content(msg)
bundle = bundle.build()
return bundle
def main():
# arguments parsing
parser = argparse.ArgumentParser()
parser.add_argument(
"--listeners",
default="127.0.0.1:9001",
help=
"The ip:port of the OSC programs listening, different listeners must be separated with ';', if multiple ports are used on one ip, it possible to use this form ip1:port1-1;port1-2;port1-3;ip2:port2-1;port2-2..."
)
parser.add_argument(
"--origin_serial",
default=None,
help="The serial number of the tracker used for origin calibration")
parser.add_argument(
"--real_world_points",
default=None,
help=
"The JSON file containing list of points to use for origin calibration"
)
parser.add_argument(
"--framerate",
type=int,
default=60,
help=
"Expected framerate - used to slow down OSC messages if OSC listener can't handle the framerate"
)
parser.add_argument(
"--openvr_coordinates",
default=False,
action="store_true",
help=
"Use openVR native coordinate system if set, or Unity coordinate system if not set"
)
parser.add_argument("--steam",
default=False,
action="store_true",
help="Open SteamVR when the script is launched")
parser.add_argument(
"--oscpattern",
default="/iss/tracker",
help="The OSC pattern used to send messages, default is '/iss/tracker'"
)
parser.add_argument("--bundles",
default=False,
action="store_true",
help="Send single frame messages as a OSC bundle")
args = parser.parse_args()
# global program variables
listeners = [] # array of OSC clients
origin_serial = args.origin_serial
real_world_points = []
real_world_points_array = []
different_h = False # to check if all points are at the same height
current_point_index = -1
calib_points = []
calib_mat = np.identity(4)
# booleans for keystroke interactions
to_unity_world = True # can be modified by --openvr_coordinates option or 'u' key
set_origin = False # change state with 'o' key
calib_next_point = False
escape = False # change state with 'escape' key
# filter used to smooth framerate computation for display
t0_list = []
t0_list_max_size = args.framerate / 2
# array of ids of currently tracked trackers (used to identify newly tracked trackers)
trackers_list = []
print("======================================")
print("")
print("---------------------------------------")
print(" Coordinate system and calibration ")
print("---------------------------------------")
# if --openvr_coordinates option is used, compute coordinates for openvr coordinate system
# if not, compute for Unity coordinate system
to_unity_world = not args.openvr_coordinates
coordsys = "Unity"
if not to_unity_world:
coordsys = "openVR"
print("Coordinate system is set for {0}".format(coordsys))
# load last origin matrix from origin_mat.rtm file,
# the file must be located in the same folder as this script
origin_file_path = os.path.dirname(
os.path.abspath(__file__)) + "\origin_mat.rtm"
#print("try to open origin file at {0}".format(origin_file_path))
origin_file = Path(origin_file_path)
if origin_file.exists():
with open(origin_file_path, 'rb') as loaded_file:
depickler = pickle.Unpickler(loaded_file)
calib_mat = depickler.load()
print("origin loaded from file " + origin_file_path)
#print(calib_mat)
else:
print(
"origin file not found at " + origin_file_path +
", please place the reference and press 'o' key to set a new origin"
)
if origin_serial:
print("Calibration can be done with tracker serial " + origin_serial)
if args.real_world_points:
fp = open(args.real_world_points, 'r')
real_world_points = json.load(fp)
if len(real_world_points) < 4:
real_world_points = None
print("Calibration file must contain at least 4 points")
else:
print("Load real world points from JSON file for calibration")
real_world_points_array = np.zeros((len(real_world_points), 3),
np.float32)
different_h = False
last_h = None
for i, pt in enumerate(real_world_points):
if to_unity_world:
real_world_points_array[i][0] = -pt['x']
real_world_points_array[i][1] = pt['z']
real_world_points_array[i][2] = -pt['y']
else:
real_world_points_array[i][0] = pt['x']
real_world_points_array[i][1] = pt['y']
real_world_points_array[i][2] = pt['z']
# check if there is at least one point outside of floor plan
h = real_world_points_array[i][2]
if last_h is None:
last_h = h
if last_h != h:
different_h = True
last_h = h
print("[{0}] : {1:3.2f}, {2:3.2f}, {3:3.2f}".format(
pt['id'], real_world_points_array[i][0],
real_world_points_array[i][1],
real_world_points_array[i][2]))
# if all points are at the same height,
# create a virtual point and append it to the array
if not different_h:
print(
"All points are at the same height, creating a virtual point for calibration"
)
A = real_world_points_array[0]
B = real_world_points_array[1]
C = real_world_points_array[2]
if collinear(A, B, C):
print(
"ERROR : the 3 first points in real_world_points JSON file must not be aligned"
)
virtual_point = A + np.cross(np.subtract(B, A),
np.subtract(C, A))
real_world_points_array = np.vstack(
[real_world_points_array, virtual_point])
print("[{0}] : {1:3.2f}, {2:3.2f}, {3:3.2f}".format(
'virtual point', virtual_point[0], virtual_point[1],
virtual_point[2]))
print("")
print("---------------------------------------")
print(" OSC parameters ")
print("---------------------------------------")
# parse the ip and ports from listeners and create OSC clients
listeners_str = args.listeners.split(";")
overall_ip = "127.0.0.1"
for l in listeners_str:
listener = l.split(":")
port = -1
if len(listener) == 2:
overall_ip = listener[0]
port = int(listener[1])
elif len(listener) == 1:
port = int(listener[0])
ip = overall_ip
print("Add OSC listener at {0}:{1}".format(ip, port))
listeners.append(udp_client.SimpleUDPClient(ip, port))
print("OSC pattern : " + args.oscpattern)
send_bundles = "Send separate OSC messages for each tracker"
if args.bundles:
send_bundles = "Send simultaneous tracker OSC messages as bundles"
print(send_bundles)
print("")
print("---------------------------------------")
print(" OpenVR ")
print("---------------------------------------")
# init OpenVR
# VRApplication_Other will not open SteamVR
# VRApplication_Scene will open SteamVR
vrapplication_type = openvr.VRApplication_Other
if args.steam:
vrapplication_type = openvr.VRApplication_Scene
print("open SteamVR")
print("Initialize OpenVR ... ", end='')
try:
openvr.init(vrapplication_type)
except openvr.OpenVRError as e:
print("Impossible to intialize OpenVR")
print(e)
exit(0)
vrsystem = openvr.VRSystem()
print("OK")
print("======================================")
print("")
program_t0 = time.perf_counter()
while (not escape):
# keep track of loop time
loop_t0 = time.perf_counter()
t0_list.append(loop_t0)
if len(t0_list) >= t0_list_max_size:
del t0_list[0]
# handle keyboard inputs
key = kbfunc()
if key != 0:
if key == ord('o'):
if origin_serial:
# set up origin
set_origin = True
print("\nset new origin : ")
if real_world_points:
current_point_index = -1
print("Multi-points calibration")
calib_next_point = True
else:
print("Single point calibration")
elif key == ord('n'):
calib_next_point = True
elif key == ord('r'):
print("\nreset origin")
calib_mat = np.identity(4)
elif key == ord('u'):
to_unity_world = not to_unity_world
print("\nto unity world = {0}".format(to_unity_world))
elif key == 27: # escape
escape = True
openvr.shutdown()
# get all devices currently tracked, it include HMD, controllers, lighthouses and trackers
poses_t = openvr.TrackedDevicePose_t * openvr.k_unMaxTrackedDeviceCount
tracked_devices = poses_t()
tracked_devices = vrsystem.getDeviceToAbsoluteTrackingPose(
openvr.TrackingUniverseStanding, 0, len(tracked_devices))
# array to store content that must be sent over OSC
osc_content = []
current_loop_trackers_list = []
for _i, device in enumerate(tracked_devices):
# broswe the array and keey only correctly tracked trackers
if device.bPoseIsValid and vrsystem.getTrackedDeviceClass(
_i) == openvr.TrackedDeviceClass_GenericTracker:
tracker_id = vrsystem.getStringTrackedDeviceProperty(
_i, openvr.Prop_SerialNumber_String)
current_loop_trackers_list.append(tracker_id)
# add tracker_id to list if not already in it
if tracker_id not in trackers_list:
trackers_list.append(tracker_id)
print("\nNew tracker found : {}".format(trackers_list[-1]))
# compute relative position (vector3) and rotation(quaternion) from 3x4 openvr matrix
m_4x4 = vive_pose_to_numpy_matrix_4x4(
device.mDeviceToAbsoluteTracking)
m_corrected = np.matmul(calib_mat, m_4x4)
m_rot = np.identity(3)
for x in range(0, 3):
for y in range(0, 3):
m_rot[x][y] = m_corrected[x][y]
quat = quaternion.from_rotation_matrix(m_rot)
# append computed pos/rot to the list
content = [
tracker_id, m_corrected[0][3], m_corrected[1][3],
m_corrected[2][3], quat.x, quat.y, quat.z, quat.w
]
if to_unity_world:
# switch and invert coordinates if Unity coordinate system is required
content = [
tracker_id, -m_corrected[0][3], -m_corrected[2][3],
m_corrected[1][3], quat.x, quat.z, -quat.y, quat.w
]
osc_content.append(content)
# set new origin if requested
if vrsystem.getStringTrackedDeviceProperty(
_i, openvr.Prop_SerialNumber_String
) == origin_serial and set_origin:
# perform multi-points calibration
if len(real_world_points) >= 4:
if calib_next_point:
if current_point_index < len(
real_world_points) - 1:
print("Place your origin tracker on point")
print(real_world_points[current_point_index +
1])
print("and press 'n'")
if current_point_index < 0:
pass
else:
openvr_pt = m_4x4[0:3, 3]
calib_points.append(openvr_pt)
calib_next_point = False
current_point_index = current_point_index + 1
if current_point_index >= len(real_world_points):
print("Computing calibration with {} points".
format(len(real_world_points)))
calib_points = np.stack(calib_points)
if not different_h:
print(
"All real world points are at same height, creating virtual point"
)
A = calib_points[0]
B = calib_points[1]
C = calib_points[2]
virtual_calib_point = A + np.cross(
np.subtract(B, A), np.subtract(C, A))
calib_points = np.vstack(
[calib_points, virtual_calib_point])
retval, M, inliers = cv2.estimateAffine3D(
calib_points, real_world_points_array)
calib_mat = np.vstack([M, [0, 0, 0, 1]])
with open(origin_file_path,
'wb') as saved_file:
pickler = pickle.Pickler(saved_file)
pickler.dump(calib_mat)
print(calib_mat.round(3))
set_origin = False
# perform single point calibration (e.g, tracker position is origin, rotation matters)
else:
set_origin = False
calib_mat = np.linalg.inv(m_4x4)
with open(origin_file_path, 'wb') as saved_file:
pickler = pickle.Pickler(saved_file)
pickler.dump(calib_mat)
print(calib_mat.round(3))
# remove trackers that are not tracked any more
for t in trackers_list:
if t not in current_loop_trackers_list:
print("\n\tTracker lost : {}".format(t))
trackers_list.remove(t)
# send osc content to all listeners if needed
for l in listeners:
if args.bundles:
bundle_builder = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY)
for c in osc_content:
msg = osc_message_builder.OscMessageBuilder(
address=args.oscpattern)
for oscarg in c:
msg.add_arg(oscarg)
bundle_builder.add_content(msg.build())
l.send(bundle_builder.build())
else:
for c in osc_content:
l.send_message(args.oscpattern, c)
#calulate fps
fps = 0
if len(t0_list) > 1:
fps = len(t0_list) / (t0_list[-1] - t0_list[0])
# update state display
if (not set_origin):
print(
"\rtime : {0:8.1f}, fps : {1:4.1f}, nb trackers : {2} ".
format(loop_t0 - program_t0, fps, len(osc_content)),
end="")
# ensure fps is respected to avoid OSC messages queue overflow
while time.perf_counter() - loop_t0 <= 1.0 / args.framerate:
pass
def test_empty_bundle(self):
bundle = osc_bundle_builder.OscBundleBuilder(
osc_bundle_builder.IMMEDIATELY).build()
self.assertEqual(0, bundle.num_contents)
def test_raises_on_invalid_timestamp(self):
bundle = osc_bundle_builder.OscBundleBuilder("I am not a timestamp")
self.assertRaises(osc_bundle_builder.BuildError, bundle.build)
