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"])) )