def __init__(self):
self.history = []
self.active = False
self._marker_map = {}
self._temporary_physical_marker_raw_data = {}
self._raw_data = {}
self._physical_markers_seen = set()
self._initial_positions = defaultdict(list)
self._callbacks = deque([self.get_marker_map, self.register_raw_data])
self._tracker = PolhemusLibertyLatus("polhemus", 2)
print()
self._tracker.set_handler("position", self.handle_input)
log.info("waiting to see all markers")
class TrackerJoint(EventDispatcher):
# Lower number tracker should be placed on pivot point.
def __init__(self):
self.history = []
self.active = False
self._marker_map = {}
self._temporary_physical_marker_raw_data = {}
self._raw_data = {}
self._physical_markers_seen = set()
self._initial_positions = defaultdict(list)
self._callbacks = deque([self.get_marker_map, self.register_raw_data])
self._tracker = PolhemusLibertyLatus("polhemus", 2)
print()
self._tracker.set_handler("position", self.handle_input)
log.info("waiting to see all markers")
@property
def angle(self):
return self.history[-1]["angle"]
@property
def radius(self):
return self.history[-1]["radius"]
def handle_input(self, data, _):
if self._callbacks:
result = self._callbacks[0](data, _)
if result == "COMPLETE":
self._callbacks.popleft()
@contextmanager
def running(self):
with VrpnServer(self._tracker, wait=20):
self.active = True
yield
self.active = False
def get_marker_map(self, data, _):
physical_marker = data.pop("sensor", 0)
self._temporary_physical_marker_raw_data[physical_marker] = data
if physical_marker not in self._physical_markers_seen:
log.info("marker {} detected".format(physical_marker))
self._physical_markers_seen.add(physical_marker)
if len(self._physical_markers_seen) == self._tracker.n_sensors:
log.info("all markers seen")
self._marker_map = {
physical_number: logical_number
for logical_number, physical_number in enumerate(sorted(self._physical_markers_seen))
}
inverse_marker_map = {logical: physical for physical, logical in self._marker_map.items()}
self._raw_data = {
logical_marker: self._temporary_physical_marker_raw_data[inverse_marker_map[logical_marker]]
for logical_marker in self._marker_map.values()
}
return "COMPLETE"
def append_data(self, angle, radius):
try:
last_angle = self.angle
except IndexError:
last_angle = angle
self.dispatch_event("on_joint_move", angle - last_angle)
self.history.append({"time": time.time(), "angle": angle, "radius": radius})
@extract_logical_marker
def register_raw_data(self, marker, data):
self._raw_data[marker] = data
self.append_data(
*angle_and_radius(np.array(self._raw_data[0]["position"]), np.array(self._raw_data[1]["position"]))
)
