-
Notifications
You must be signed in to change notification settings - Fork 0
/
Head_check.py
182 lines (156 loc) · 7.22 KB
/
Head_check.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
import sys
import os
import logging
import time
import numpy as np
import wx
import threading
from argparse import ArgumentParser
from IPython.config.loader import PyFileConfigLoader
import PyDragonfly
from PyDragonfly import CMessage, MT_EXIT, copy_to_msg, copy_from_msg
from dragonfly_utils import respond_to_ping
import Dragonfly_config as rc
import quaternionarray as qa
import amcmorl_py_tools.vecgeom as vg
from amcmorl_py_tools.vecgeom import transformations as tf
from TrackedPoint import TrackedPoint as TP
class Head_check(object):
def __init__(self, config_file, server):
self.find_data()
self.load_config(config_file)
self.load_logging()
self.setup_dragonfly(server)
self.run()
def find_data(self):
app = wx.PySimpleApp()
dialog = wx.FileDialog(None, style=wx.FD_OPEN)
if dialog.ShowModal() == wx.ID_OK:
self.file_path = dialog.GetPath()
dialog.Destroy()
app = 0
#self.file_path = "C:\\Users\\amcmorl\\Dave25-07-16.txt"
self.open_file(self.file_path)
def open_file(self, file_name):
with open(file_name, 'r') as calib:
Right_Tragus = []
Left_Tragus = []
Nasion = []
Cz = []
lines = calib.readlines()
for i, line in enumerate(lines):
if 'Right Tragus' in line:
Right_Tragus.extend((lines[i+1], lines[i+2], lines[i+3]))
if 'Left Tragus' in line:
Left_Tragus.extend((lines[i+1], lines[i+2], lines[i+3]))
if 'Nasion' in line:
Nasion.extend((lines[i+1], lines[i+2], lines[i+3]))
if 'Cz' in line:
Cz.extend((lines[i+1], lines[i+2], lines[i+3]))
self.glasses_to_RT = TP(
self._make_array(Right_Tragus, 1),
self._make_array(Right_Tragus, 2),
self._make_array(Right_Tragus, 0)
)
self.glasses_to_LT = TP(
self._make_array(Left_Tragus, 1),
self._make_array(Left_Tragus, 2),
self._make_array(Left_Tragus, 0)
)
self.glasses_to_Nasion = TP(
self._make_array(Nasion, 1),
self._make_array(Nasion, 2),
self._make_array(Nasion, 0)
)
self.glasses_to_cz = TP(
self._make_array(Cz, 1),
self._make_array(Cz, 2),
self._make_array(Cz, 0)
)
def _make_array(self, arr, x): #x indexes array
pos = arr[x].split()[-1]
return np.array(pos.split(',')).astype(np.float)
def load_config(self, config_file):
cfg = PyFileConfigLoader(config_file)
cfg.load_config()
self.config = cfg.config
# special casing for SAMPLE_GENERATED
if (self.config.trigger == 'SAMPLE_GENERATED'):
self.config.trigger_msg = rc.MT_SAMPLE_GENERATED
self.config.trigger_mdf = rc.MDF_SAMPLE_GENERATED
else:
self.config.trigger_msg = \
eval('rc.MT_' + self.config.trigger)
self.config.trigger_mdf = \
eval('rc.MDF_' + self.config.trigger)
print "Triggering with", self.config.trigger
print "TMS Mapping Collection: loading config"
#self.ntools = len(self.config.tool_list)
self.plate = self.config.tools.index('CB609')
self.marker = self.config.tools.index('CT315')
self.glasses = self.config.tools.index('ST568')
self.pointer = self.config.tools.index('P717')
def setup_dragonfly(self, server):
self.mod = PyDragonfly.Dragonfly_Module(0, 0)
self.mod.ConnectToMMM(server)
self.mod.Subscribe(MT_EXIT)
self.mod.Subscribe(rc.MT_PING)
self.mod.Subscribe(rc.MT_POLARIS_POSITION)
self.mod.Subscribe(rc.MT_HOTSPOT_POSITION)
self.mod.Subscribe(rc.MT_TMS_TRIGGER)
self.mod.SendModuleReady()
print "TMS_Mapping_Collection: connected to dragonfly"
def load_logging(self):
log_file = os.path.normpath(os.path.join(self.config.config_dir, 'TMS_mapping_collection.log'))
print "log file: " + log_file
logging.basicConfig(filename=log_file, level=logging.DEBUG)
logging.info(' ')
logging.info(' ')
logging.debug("**** STARTING UP ****")
logging.info(" %s " % time.asctime())
logging.info("*********************")
def run(self):
while True:
msg = CMessage()
rcv = self.mod.ReadMessage(msg, 0.001)
if rcv == 1:
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
break;
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'Head_check')
else:
self.process_message(msg)
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
#print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing)))
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
if in_mdf.tool_id == (self.glasses + 1):
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
self.find_pos_to_glasses(positions, orientations)
def shuffle_q(self, q):
return np.roll(q, -1, axis=0)
def find_pos_to_glasses(self, glasses_position, glasses_orientation): #Saves 0, 0, 0 array to file when object called
cz_pos, cz_rot = self.glasses_to_cz.get_pos(glasses_orientation, glasses_position)
Nasion_pos, nasion_rot = self.glasses_to_Nasion.get_pos(glasses_orientation, glasses_position)
LT_pos, LT_rot = self.glasses_to_LT.get_pos(glasses_orientation, glasses_position)
RT_pos, RT_rot = self.glasses_to_RT.get_pos(glasses_orientation, glasses_position)
print 'CZ: ', cz_pos
print 'Nasion: ', Nasion_pos
print 'LT: ', LT_pos
print 'RT: ', RT_pos
if __name__=="__main__":
parser = ArgumentParser(description = 'Trial controller for TMS mapping')
parser.add_argument(type=str, dest='config')
parser.add_argument(type=str, dest='mm_ip', nargs='?', default='127.0.0.1:7111')
args = parser.parse_args()
frame = Head_check(args.config, args.mm_ip)