Source code for pypot.sensor.optitrack

import time
import numpy
import datetime
import threading

from collections import namedtuple


TrackedObject = namedtuple('TrackedObject', ('position', 'quaternion', 'orientation', 'timestamp'))


[docs]def quat2euler(q): qx, qy, qz, qw = q sqx, sqy, sqz, sqw = q ** 2 invs = 1.0 / (sqx + sqy + sqz + sqw) yaw = numpy.arctan2(2.0 * (qx * qz + qy * qw) * invs, (sqx - sqy - sqz + sqw) * invs) pitch = -numpy.arcsin(2.0 * (qx * qy - qz * qw) * invs) roll = numpy.arctan2(2.0 * (qy * qz + qx * qw) * invs, (-sqx + sqy - sqz + sqw) * invs) return numpy.array((yaw, pitch, roll))
try: import vrpn class OptiTrackClient(threading.Thread): """ Retrieves position, orientation, and timestamp of each tracked object. The position is expressed in meters (X is left, Y is up, and Z is depth). The orientation is expressed in radians (yaw, pitch, roll). """ def __init__(self, addr, port, obj_names): threading.Thread.__init__(self) self.daemon = True self.trackers = [] for obj in obj_names: t = vrpn.receiver.Tracker('{}@{}:{}'.format(obj, addr, port)) t.register_change_handler(obj, self.handler, 'position') self.trackers.append(t) self._tracked_objects = {} @property def tracked_objects(self): return self._tracked_objects @property def recent_tracked_objects(self): """ Only returns the objects that have been tracked less than 20ms ago. """ dt = 0.02 f = lambda name: (datetime.datetime.now() - self.tracked_objects[name].timestamp).total_seconds() return dict([(k, v) for k, v in self.tracked_objects.iteritems() if f(k) < dt]) def handler(self, obj, data): self.tracked_objects[obj] = TrackedObject(numpy.array(*data['position']), numpy.array(data['quaternion']), quat2euler(numpy.array(data['quaternion'])), datetime.datetime.now()) def serve_forever(self): self.start() while True: try: self.join(timeout=1.0) except KeyboardInterrupt: break def run(self): while True: for t in self.trackers: t.mainloop() time.sleep(1.0 / 120) except ImportError: pass if __name__ == '__main__': c = OptiTrackClient('193.50.110.176', 3883, ('obj_1', )) c.serve_forever()