Download as
Browse Code »

Commit 839d45c1a425b83bb66e770c07172b5acf43befe

Authored by Renan
1 parent b6ae3404
Exists in master

ENH: variable names refractored

Showing 12 changed files with 132 additions and 124 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @839d45c
... ... @@ -830,7 +830,6 @@ TREKKER_CONFIG = {'seed_max': 1, 'step_size': 0.1, 'min_fod': 0.1, 'probe_qualit
830 830  
831 831 MARKER_FILE_MAGICK_STRING = "INVESALIUS3_MARKER_FILE_"
832 832 CURRENT_MARKER_FILE_VERSION = 0
833   -
834 833 WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss")
835 834  
836 835 # Serial port
... ... @@ -840,8 +839,8 @@ BAUD_RATE_DEFAULT_SELECTION = 4
840 839 #Robot
841 840 ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1']
842 841 ROBOT_ElFIN_PORT = 10003
843   -
844 842 ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
845   -
846 843 ROBOT_HEAD_VELOCITY_THRESHOLD = 10
847 844 ROBOT_ARC_THRESHOLD_DISTANCE = 100
  845 +ROBOT_VERSOR_SCALE_FACTOR = 70
  846 +
... ...
invesalius/data/bases.py
  Diff comments   View file @839d45c
... ... @@ -245,10 +245,10 @@ def object_registration(fiducials, orients, coord_raw, m_change):
245 245  
246 246 return t_obj_raw, s0_raw, r_s0_raw, s0_dyn, m_obj_raw, r_obj_img
247 247  
248   -class transform_tracker_2_robot(object):
249   - M_tracker_2_robot = None
250   - def transformation_tracker_2_robot(self, tracker_coord):
251   - if transform_tracker_2_robot.M_tracker_2_robot is None:
  248 +class transform_tracker_to_robot(object):
  249 + M_tracker_to_robot = np.array([])
  250 + def transformation_tracker_to_robot(self, tracker_coord):
  251 + if not transform_tracker_to_robot.M_tracker_to_robot.any():
252 252 #print("matrix tracker2robot is not define")
253 253 return None
254 254 else:
... ... @@ -256,10 +256,10 @@ class transform_tracker_2_robot(object):
256 256 a, b, g = np.radians(tracker_coord[3:6])
257 257 rot = tr.euler_matrix(a, b, g, 'rzyx')
258 258 M_tracker = tr.concatenate_matrices(trans, rot)
259   - M_tracker_in_robot = transform_tracker_2_robot.M_tracker_2_robot @ M_tracker
  259 + M_tracker_in_robot = transform_tracker_to_robot.M_tracker_to_robot @ M_tracker
260 260  
261 261 _, _, angles, translate, _ = tr.decompose_matrix(M_tracker_in_robot)
262 262 tracker_in_robot = [translate[0], translate[1], translate[2], \
263 263 np.degrees(angles[2]), np.degrees(angles[1]), np.degrees(angles[0])]
264 264  
265   - return tracker_in_robot
266 265 \ No newline at end of file
  266 + return tracker_in_robot
... ...
invesalius/data/coordinates.py
  Diff comments   View file @839d45c
... ... @@ -488,9 +488,9 @@ def RobotCoord(trk_init, trck_id, ref_mode):
488 488 coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
489 489 coord_robot, _ = ElfinCoord(trk_init[1:])
490 490  
491   - probe_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[0])
492   - ref_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[1])
493   - obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2])
  491 + probe_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[0])
  492 + ref_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[1])
  493 + obj_tracker_in_robot = db.transform_tracker_to_robot().transformation_tracker_to_robot(coord_tracker[2])
494 494  
495 495 if probe_tracker_in_robot is None:
496 496 probe_tracker_in_robot = coord_tracker[0]
... ...
invesalius/data/coregistration.py
  Diff comments   View file @839d45c
... ... @@ -190,11 +190,10 @@ class CoordinateCorregistrate(threading.Thread):
190 190 self.event = event
191 191 self.sle = sle
192 192 self.icp_queue = queues[2]
193   - self.objattarget_queue = queues[3]
  193 + self.object_at_target_queue = queues[3]
194 194 self.icp = None
195 195 self.m_icp = None
196 196 self.robot_tracker_flag = None
197   - self.m_change_robot2ref = None
198 197 self.last_coord = None
199 198 self.tracker_id = tracker_id
200 199 self.target = target
... ... @@ -223,10 +222,10 @@ class CoordinateCorregistrate(threading.Thread):
223 222 else:
224 223 self.icp, self.m_icp = self.icp_queue.get_nowait()
225 224  
226   - if self.objattarget_queue.empty():
  225 + if self.object_at_target_queue.empty():
227 226 None
228 227 else:
229   - self.target_flag = self.objattarget_queue.get_nowait()
  228 + self.target_flag = self.object_at_target_queue.get_nowait()
230 229  
231 230 # print(f"Set the coordinate")
232 231 coord_raw, markers_flag = self.tracker.TrackerCoordinates.GetCoordinates()
... ...
invesalius/data/elfin.py
  Diff comments   View file @839d45c
... ... @@ -11,10 +11,10 @@ class elfin_server():
11 11 self.port_number = port_number
12 12  
13 13 def Initialize(self):
14   - SIZE = 1024
15   - rbtID = 0
  14 + message_size = 1024
  15 + robot_id = 0
16 16 self.cobot = elfin()
17   - self.cobot.connect(self.server_ip, self.port_number, SIZE, rbtID)
  17 + self.cobot.connect(self.server_ip, self.port_number, message_size, robot_id)
18 18 print("conected!")
19 19  
20 20 def Run(self):
... ... @@ -40,17 +40,17 @@ class elfin:
40 40 def __init__(self):
41 41 self.end_msg = ",;"
42 42  
43   - def connect(self, SERVER_IP, PORT_NUMBER, SIZE, rbtID):
  43 + def connect(self, server_ip, port_number, message_size, robot_id):
44 44 mySocket = socket(AF_INET, SOCK_STREAM)
45   - mySocket.connect((SERVER_IP, PORT_NUMBER))
  45 + mySocket.connect((server_ip, port_number))
46 46  
47   - self.size = SIZE
48   - self.rbtID = str(rbtID)
  47 + self.message_size = message_size
  48 + self.robot_id = str(robot_id)
49 49 self.mySocket = mySocket
50 50  
51 51 def send(self, message):
52 52 self.mySocket.sendall(message.encode('utf-8'))
53   - data = self.mySocket.recv(self.size).decode('utf-8').split(',')
  53 + data = self.mySocket.recv(self.message_size).decode('utf-8').split(',')
54 54 status = self.check_status(data)
55 55 if status and type(data) != bool:
56 56 if len(data) > 3:
... ... @@ -124,7 +124,7 @@ class elfin:
124 124 if Error Return False
125 125 if not Error Return True
126 126 """
127   - message = "GrpPowerOn," + self.rbtID + self.end_msg
  127 + message = "GrpPowerOn," + self.robot_id + self.end_msg
128 128 status = self.send(message)
129 129 return status
130 130  
... ... @@ -135,7 +135,7 @@ class elfin:
135 135 if Error Return False
136 136 if not Error Return True
137 137 """
138   - message = "GrpPowerOff," + self.rbtID + self.end_msg
  138 + message = "GrpPowerOff," + self.robot_id + self.end_msg
139 139 status = self.send(message)
140 140 return status
141 141  
... ... @@ -146,7 +146,7 @@ class elfin:
146 146 if Error Return False
147 147 if not Error Return True
148 148 """
149   - message = "GrpStop," + self.rbtID + self.end_msg
  149 + message = "GrpStop," + self.robot_id + self.end_msg
150 150 status = self.send(message)
151 151 return status
152 152  
... ... @@ -160,7 +160,7 @@ class elfin:
160 160 if not Error Return True
161 161 """
162 162  
163   - message = "SetOverride," + self.rbtID + ',' + str(override) + self.end_msg
  163 + message = "SetOverride," + self.robot_id + ',' + str(override) + self.end_msg
164 164 status = self.send(message)
165 165 return status
166 166  
... ... @@ -170,7 +170,7 @@ class elfin:
170 170 if True Return x,y,z,a,b,c
171 171 if Error Return False
172 172 """
173   - message = "ReadPcsActualPos," + self.rbtID + self.end_msg
  173 + message = "ReadPcsActualPos," + self.robot_id + self.end_msg
174 174 coord = self.send(message)
175 175 if coord:
176 176 return [float(s) for s in coord]
... ... @@ -185,7 +185,7 @@ class elfin:
185 185 """
186 186 target = [str(s) for s in target]
187 187 target = (",".join(target))
188   - message = "MoveL," + self.rbtID + ',' + target + self.end_msg
  188 + message = "MoveL," + self.robot_id + ',' + target + self.end_msg
189 189 return self.send(message)
190 190  
191 191 def SetToolCoordinateMotion(self, status):
... ... @@ -196,7 +196,7 @@ class elfin:
196 196 if Error Return False
197 197 if not Error Return True
198 198 """
199   - message = "SetToolCoordinateMotion," + self.rbtID + ',' + str(status) + self.end_msg
  199 + message = "SetToolCoordinateMotion," + self.robot_id + ',' + str(status) + self.end_msg
200 200 status = self.send(message)
201 201 return status
202 202  
... ... @@ -210,7 +210,7 @@ class elfin:
210 210 1013=waiting for execution;
211 211 1025 =Error reporting
212 212 """
213   - message = "ReadMoveState," + self.rbtID + self.end_msg
  213 + message = "ReadMoveState," + self.robot_id + self.end_msg
214 214 status = int(self.send(message)[0])
215 215 return status
216 216  
... ... @@ -221,7 +221,7 @@ class elfin:
221 221 if Error Return False
222 222 if not Error Return True
223 223 """
224   - message = "MoveHoming," + self.rbtID + self.end_msg
  224 + message = "MoveHoming," + self.robot_id + self.end_msg
225 225 status = self.send(message)
226 226 return status
227 227  
... ... @@ -233,5 +233,5 @@ class elfin:
233 233 """
234 234 target = [str(s) for s in target]
235 235 target = (",".join(target))
236   - message = "MoveC," + self.rbtID + ',' + target + self.end_msg
  236 + message = "MoveC," + self.robot_id + ',' + target + self.end_msg
237 237 return self.send(message)
... ...
invesalius/data/elfin_processing.py
  Diff comments   View file @839d45c
... ... @@ -89,9 +89,9 @@ class TrackerProcessing:
89 89  
90 90 def estimate_head_velocity(self, coord_vel, timestamp):
91 91 coord_vel = np.vstack(np.array(coord_vel))
92   - coord_init = coord_vel[:int(len(coord_vel)/2)].mean(axis=0)
93   - coord_final = coord_vel[int(len(coord_vel)/2):].mean(axis=0)
94   - velocity = (coord_final - coord_init)/(timestamp[-1]-timestamp[0])
  92 + coord_init = coord_vel[:int(len(coord_vel) / 2)].mean(axis=0)
  93 + coord_final = coord_vel[int(len(coord_vel) / 2):].mean(axis=0)
  94 + velocity = (coord_final - coord_init)/(timestamp[-1] - timestamp[0])
95 95 distance = (coord_final - coord_init)
96 96  
97 97 return velocity, distance
... ... @@ -99,32 +99,40 @@ class TrackerProcessing:
99 99 def versors(self, init_point, final_point):
100 100 init_point = np.array(init_point)
101 101 final_point = np.array(final_point)
102   - norm = (sum((final_point-init_point)**2))**0.5
103   - versorfactor = (((final_point-init_point)/norm)*70).tolist()
  102 + norm = (sum((final_point - init_point) ** 2)) ** 0.5
  103 + versorfactor = (((final_point-init_point) / norm) * const.ROBOT_VERSOR_SCALE_FACTOR).tolist()
104 104  
105 105 return versorfactor
106 106  
107   - def arcmotion(self, actual_point, coord_head, coord_inv):
  107 + def compute_arc_motion(self, actual_point, coord_head, coord_inv):
108 108 p1 = coord_inv
109 109  
110 110 pc = coord_head[0], coord_head[1], coord_head[2], coord_inv[3], coord_inv[4], coord_inv[5]
111 111  
112 112 versorfactor1 = self.versors(pc, actual_point)
113   - init_ext_point = actual_point[0]+versorfactor1[0],\
114   - actual_point[1]+versorfactor1[1],\
115   - actual_point[2]+versorfactor1[2], \
  113 + init_ext_point = actual_point[0] + versorfactor1[0], \
  114 + actual_point[1] + versorfactor1[1], \
  115 + actual_point[2] + versorfactor1[2], \
116 116 actual_point[3], actual_point[4], actual_point[5]
117 117  
118   - middle_point = ((p1[0]+actual_point[0])/2, (p1[1]+actual_point[1])/2, (p1[2]+actual_point[2])/2, 0, 0, 0)
  118 + middle_point = ((p1[0] + actual_point[0]) / 2,
  119 + (p1[1] + actual_point[1]) / 2,
  120 + (p1[2] + actual_point[2]) / 2,
  121 + 0, 0, 0)
119 122  
120   - newarr = (np.array(self.versors(pc, middle_point)))*2
121   - middle_arc_point = middle_point[0]+newarr[0], middle_point[1]+newarr[1], middle_point[2]+newarr[2]
  123 + newarr = (np.array(self.versors(pc, middle_point))) * 2
  124 + middle_arc_point = middle_point[0] + newarr[0], \
  125 + middle_point[1] + newarr[1], \
  126 + middle_point[2] + newarr[2]
122 127  
123 128 versorfactor3 = self.versors(pc, p1)
124 129  
125   - final_ext_arc_point = p1[0]+versorfactor3[0], p1[1]+versorfactor3[1], p1[2]+versorfactor3[2], \
126   - coord_inv[3], coord_inv[4], coord_inv[5], 0
127   - target_arc = middle_arc_point+final_ext_arc_point
  130 + final_ext_arc_point = p1[0] + versorfactor3[0], \
  131 + p1[1] + versorfactor3[1], \
  132 + p1[2] + versorfactor3[2], \
  133 + coord_inv[3], coord_inv[4], coord_inv[5], 0
  134 +
  135 + target_arc = middle_arc_point + final_ext_arc_point
128 136  
129 137 return init_ext_point, target_arc
130 138  
... ... @@ -150,13 +158,13 @@ class TrackerProcessing:
150 158  
151 159 return False
152 160  
153   - def head_move_compensation(self, current_ref, m_change_robot2ref):
  161 + def head_move_compensation(self, current_ref, m_change_robot_to_head):
154 162 trans = tr.translation_matrix(current_ref[:3])
155 163 a, b, g = np.radians(current_ref[3:6])
156 164 rot = tr.euler_matrix(a, b, g, 'rzyx')
157 165 M_current_ref = tr.concatenate_matrices(trans, rot)
158 166  
159   - m_robot_new = M_current_ref @ m_change_robot2ref
  167 + m_robot_new = M_current_ref @ m_change_robot_to_head
160 168 _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new)
161 169 angles = np.degrees(angles)
162 170  
... ... @@ -172,9 +180,9 @@ class TrackerProcessing:
172 180 return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
173 181  
174 182 def correction_distance_calculation_target(self, coord_inv, actual_point):
175   - sum = (coord_inv[0]-actual_point[0])**2\
176   - + (coord_inv[1]-actual_point[1])**2\
177   - + (coord_inv[2]-actual_point[2])**2
  183 + sum = (coord_inv[0]-actual_point[0]) ** 2\
  184 + + (coord_inv[1]-actual_point[1]) ** 2\
  185 + + (coord_inv[2]-actual_point[2]) ** 2
178 186 correction_distance_compensation = pow(sum, 0.5)
179 187  
180 188 return correction_distance_compensation
... ...
invesalius/data/trackers.py
  Diff comments   View file @839d45c
... ... @@ -242,7 +242,7 @@ def RobotTracker(tracker_id):
242 242 trck_init = None
243 243 trck_init_robot = None
244 244 tracker_id = None
245   - dlg_device = dlg.SetTrackerDevice2Robot()
  245 + dlg_device = dlg.SetTrackerDeviceToRobot()
246 246 if dlg_device.ShowModal() == ID_OK:
247 247 tracker_id = dlg_device.GetValue()
248 248 if tracker_id:
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @839d45c
... ... @@ -4323,7 +4323,7 @@ class SetOptitrackconfigs(wx.Dialog):
4323 4323  
4324 4324 return fn_cal, fn_userprofile
4325 4325  
4326   -class SetTrackerDevice2Robot(wx.Dialog):
  4326 +class SetTrackerDeviceToRobot(wx.Dialog):
4327 4327 def __init__(self, title=_("Setting tracker device:")):
4328 4328 wx.Dialog.__init__(self, wx.GetApp().GetTopWindow(), -1, title, size=wx.Size(1000, 200),
4329 4329 style=wx.DEFAULT_DIALOG_STYLE|wx.FRAME_FLOAT_ON_PARENT|wx.STAY_ON_TOP|wx.RESIZE_BORDER)
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @839d45c
... ... @@ -742,7 +742,7 @@ class NeuronavigationPanel(wx.Panel):
742 742 self.navigation.StopNavigation()
743 743 if self.tracker.tracker_id == const.ROBOT:
744 744 Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
745   - m_change_robot2ref=None)
  745 + m_change_robot_to_head=None)
746 746  
747 747 # Enable all navigation buttons
748 748 choice_ref.Enable(True)
... ... @@ -1348,7 +1348,7 @@ class MarkersPanel(wx.Panel):
1348 1348 Publisher.subscribe(self.UpdateNavigationStatus, 'Navigation status')
1349 1349 Publisher.subscribe(self.UpdateSeedCoordinates, 'Update tracts')
1350 1350 Publisher.subscribe(self.OnChangeCurrentSession, 'Current session changed')
1351   - Publisher.subscribe(self.UpdateRobotCoord, 'Update raw coord')
  1351 + Publisher.subscribe(self.UpdateRobotCoordinates, 'Update raw coordinates')
1352 1352  
1353 1353 def __find_target_marker(self):
1354 1354 """Return the index of the marker currently selected as target (there
... ... @@ -1429,9 +1429,9 @@ class MarkersPanel(wx.Panel):
1429 1429 def UpdateSeedCoordinates(self, root=None, affine_vtk=None, coord_offset=(0, 0, 0)):
1430 1430 self.current_seed = coord_offset
1431 1431  
1432   - def UpdateRobotCoord(self, coord_raw, markers_flag):
1433   - self.current_head = coord_raw[1]
1434   - self.current_robot = coord_raw[2]
  1432 + def UpdateRobotCoordinates(self, coordinates_raw, markers_flag):
  1433 + self.current_head = coordinates_raw[1]
  1434 + self.current_robot = coordinates_raw[2]
1435 1435  
1436 1436 def OnMouseRightDown(self, evt):
1437 1437 # TODO: Enable the "Set as target" only when target is created with registered object
... ... @@ -1444,12 +1444,12 @@ class MarkersPanel(wx.Panel):
1444 1444 target_menu = menu_id.Append(1, _('Set as target'))
1445 1445 menu_id.Bind(wx.EVT_MENU, self.OnMenuSetTarget, target_menu)
1446 1446 menu_id.AppendSeparator()
1447   - send_target_2_robot = menu_id.Append(3, _('Send target to robot'))
1448   - menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTarget2Robot, send_target_2_robot)
  1447 + send_target_to_robot = menu_id.Append(3, _('Send target to robot'))
  1448 + menu_id.Bind(wx.EVT_MENU, self.OnMenuSendTargetToRobot, send_target_to_robot)
1449 1449 if self.tracker.tracker_id == const.ROBOT:
1450   - send_target_2_robot.Enable(True)
  1450 + send_target_to_robot.Enable(True)
1451 1451 else:
1452   - send_target_2_robot.Enable(False)
  1452 + send_target_to_robot.Enable(False)
1453 1453 # TODO: Create the remove target option so the user can disable the target without removing the marker
1454 1454 # target_menu_rem = menu_id.Append(3, _('Remove target'))
1455 1455 # menu_id.Bind(wx.EVT_MENU, self.OnMenuRemoveTarget, target_menu_rem)
... ... @@ -1500,7 +1500,7 @@ class MarkersPanel(wx.Panel):
1500 1500  
1501 1501 Publisher.sendMessage('Set new color', index=index, color=color_new)
1502 1502  
1503   - def OnMenuSendTarget2Robot(self, evt):
  1503 + def OnMenuSendTargetToRobot(self, evt):
1504 1504 if isinstance(evt, int):
1505 1505 self.lc.Focus(evt)
1506 1506  
... ... @@ -1517,9 +1517,9 @@ class MarkersPanel(wx.Panel):
1517 1517 rot = tr.euler_matrix(a, b, g, 'rzyx')
1518 1518 m_ref_target = tr.concatenate_matrices(trans, rot)
1519 1519  
1520   - m_change_robot2ref = np.linalg.inv(m_ref_target) @ m_robot_target
  1520 + m_change_robot_to_head = np.linalg.inv(m_ref_target) @ m_robot_target
1521 1521  
1522   - Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot2ref=m_change_robot2ref)
  1522 + Publisher.sendMessage('Robot target matrix', robot_tracker_flag=True, m_change_robot_to_head=m_change_robot_to_head)
1523 1523  
1524 1524 def OnDeleteAllMarkers(self, evt=None):
1525 1525 if evt is None:
... ... @@ -1561,7 +1561,7 @@ class MarkersPanel(wx.Panel):
1561 1561 if self.__find_target_marker() in index:
1562 1562 Publisher.sendMessage('Disable or enable coil tracker', status=False)
1563 1563 Publisher.sendMessage('Robot target matrix', robot_tracker_flag=False,
1564   - m_change_robot2ref=None)
  1564 + m_change_robot_to_head=None)
1565 1565 wx.MessageBox(_("Target deleted."), _("InVesalius 3"))
1566 1566  
1567 1567 self.__delete_multiple_markers(index)
... ...
invesalius/navigation/navigation.py
  Diff comments   View file @839d45c
... ... @@ -82,7 +82,7 @@ class UpdateNavigationScene(threading.Thread):
82 82  
83 83 threading.Thread.__init__(self, name='UpdateScene')
84 84 self.serial_port_enabled, self.view_tracts, self.peel_loaded = vis_components
85   - self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue = vis_queues
  85 + self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue = vis_queues
86 86 self.sle = sle
87 87 self.event = event
88 88  
... ... @@ -91,7 +91,7 @@ class UpdateNavigationScene(threading.Thread):
91 91 while not self.event.is_set():
92 92 got_coords = False
93 93 try:
94   - coord, [coord_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
  94 + coord, [coordinates_raw, markers_flag], m_img, view_obj = self.coord_queue.get_nowait()
95 95 got_coords = True
96 96  
97 97 # print('UpdateScene: get {}'.format(count))
... ... @@ -116,7 +116,7 @@ class UpdateNavigationScene(threading.Thread):
116 116 # see the red cross in the position of the offset marker
117 117 wx.CallAfter(Publisher.sendMessage, 'Update slices position', position=coord[:3])
118 118 wx.CallAfter(Publisher.sendMessage, 'Set cross focal point', position=coord)
119   - wx.CallAfter(Publisher.sendMessage, 'Update raw coord', coord_raw=coord_raw, markers_flag=markers_flag)
  119 + wx.CallAfter(Publisher.sendMessage, 'Update raw coordinates', coordinates_raw=coordinates_raw, markers_flag=markers_flag)
120 120 wx.CallAfter(Publisher.sendMessage, 'Update slice viewer')
121 121  
122 122 if view_obj:
... ... @@ -147,8 +147,8 @@ class Navigation():
147 147 self.event = threading.Event()
148 148 self.coord_queue = QueueCustom(maxsize=1)
149 149 self.icp_queue = QueueCustom(maxsize=1)
150   - self.objattarget_queue = QueueCustom(maxsize=1)
151   - self.robottarget_queue = QueueCustom(maxsize=1)
  150 + self.object_at_target_queue = QueueCustom(maxsize=1)
  151 + self.robot_target_queue = QueueCustom(maxsize=1)
152 152 # self.visualization_queue = QueueCustom(maxsize=1)
153 153 self.serial_port_queue = QueueCustom(maxsize=1)
154 154 self.coord_tracts_queue = QueueCustom(maxsize=1)
... ... @@ -236,7 +236,7 @@ class Navigation():
236 236 self.event.clear()
237 237  
238 238 vis_components = [self.serial_port_in_use, self.view_tracts, self.peel_loaded]
239   - vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robottarget_queue]
  239 + vis_queues = [self.coord_queue, self.serial_port_queue, self.tracts_queue, self.icp_queue, self.robot_target_queue]
240 240  
241 241 Publisher.sendMessage("Navigation status", nav_status=True, vis_status=vis_components)
242 242  
... ... @@ -270,7 +270,7 @@ class Navigation():
270 270 obj_data = db.object_registration(obj_fiducials, obj_orients, coord_raw, m_change)
271 271 coreg_data.extend(obj_data)
272 272  
273   - queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.objattarget_queue]
  273 + queues = [self.coord_queue, self.coord_tracts_queue, self.icp_queue, self.object_at_target_queue]
274 274 jobs_list.append(dcr.CoordinateCorregistrate(self.ref_mode_id, tracker, coreg_data,
275 275 self.view_tracts, queues,
276 276 self.event, self.sleep_nav, tracker.tracker_id,
... ...
invesalius/navigation/robot.py
  Diff comments   View file @839d45c
... ... @@ -37,8 +37,8 @@ class Robot():
37 37 def __init__(self, tracker):
38 38 self.tracker = tracker
39 39 self.trk_init = None
40   - self.robottarget_queue = None
41   - self.objattarget_queue = None
  40 + self.robot_target_queue = None
  41 + self.object_at_target_queue = None
42 42 self.process_tracker = None
43 43  
44 44 self.thread_robot = None
... ... @@ -62,8 +62,8 @@ class Robot():
62 62 self.process_tracker = elfin_process.TrackerProcessing()
63 63 dlg_correg_robot = dlg.CreateTransformationMatrixRobot(self.tracker)
64 64 if dlg_correg_robot.ShowModal() == wx.ID_OK:
65   - M_tracker_2_robot = dlg_correg_robot.GetValue()
66   - db.transform_tracker_2_robot.M_tracker_2_robot = M_tracker_2_robot
  65 + M_tracker_to_robot = dlg_correg_robot.GetValue()
  66 + db.transform_tracker_to_robot.M_tracker_to_robot = M_tracker_to_robot
67 67 self.robot_server = self.tracker.trk_init[1][0]
68 68 self.trk_init = self.tracker.trk_init
69 69 else:
... ... @@ -76,31 +76,33 @@ class Robot():
76 76 if tracker.event_robot.is_set():
77 77 tracker.event_robot.clear()
78 78 self.thread_robot = ControlRobot(self.trk_init, tracker, self.robotcoordinates,
79   - [coord_queue, self.robottarget_queue, self.objattarget_queue],
80   - self.process_tracker, tracker.event_robot)
  79 + [coord_queue, self.robot_target_queue, self.object_at_target_queue],
  80 + self.process_tracker, tracker.event_robot)
81 81 self.thread_robot.start()
82 82  
83 83 def StopRobotThreadNavigation(self):
84 84 self.thread_robot.join()
  85 + #TODO: initialize process_tracker every time a "send coordinates to robot" is requested
  86 + self.process_tracker.__init__()
85 87  
86 88 def OnSendCoordinates(self, coord):
87 89 self.robot_server.SendCoordinates(coord)
88 90  
89   - def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot2ref):
  91 + def OnUpdateRobotTargetMatrix(self, robot_tracker_flag, m_change_robot_to_head):
90 92 try:
91   - self.robottarget_queue.put_nowait([robot_tracker_flag, m_change_robot2ref])
  93 + self.robot_target_queue.put_nowait([robot_tracker_flag, m_change_robot_to_head])
92 94 except queue.Full:
93 95 pass
94 96  
95 97 def OnObjectTarget(self, state):
96 98 try:
97   - if self.objattarget_queue:
98   - self.objattarget_queue.put_nowait(state)
  99 + if self.object_at_target_queue:
  100 + self.object_at_target_queue.put_nowait(state)
99 101 except queue.Full:
100 102 pass
101 103  
102 104 def SetRobotQueues(self, queues):
103   - self.robottarget_queue, self.objattarget_queue = queues
  105 + self.robot_target_queue, self.object_at_target_queue = queues
104 106  
105 107  
106 108 class RobotCoordinates():
... ... @@ -115,7 +117,7 @@ class RobotCoordinates():
115 117  
116 118  
117 119 class ControlRobot(threading.Thread):
118   - def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event):
  120 + def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event_robot):
119 121 threading.Thread.__init__(self, name='ControlRobot')
120 122  
121 123 self.trck_init_robot = trck_init[1][0]
... ... @@ -126,20 +128,20 @@ class ControlRobot(threading.Thread):
126 128 self.robot_tracker_flag = False
127 129 self.objattarget_flag = False
128 130 self.target_flag = False
129   - self.m_change_robot2ref = None
  131 + self.m_change_robot_to_head = None
130 132 self.coord_inv_old = None
131 133 self.coord_queue = queues[0]
132   - self.robottarget_queue = queues[1]
133   - self.objattarget_queue = queues[2]
  134 + self.robot_target_queue = queues[1]
  135 + self.object_at_target_queue = queues[2]
134 136 self.process_tracker = process_tracker
135   - self.event = event
136   - self.arcmotion_flag = False
137   - self.arcmotion_step_flag = None
138   - self.target_linearout = None
139   - self.target_linearin = None
  137 + self.event_robot = event_robot
  138 + self.arc_motion_flag = False
  139 + self.arc_motion_step_flag = None
  140 + self.target_linear_out = None
  141 + self.target_linear_in = None
140 142 self.target_arc = None
141 143  
142   - def getcoordsfromdevices(self):
  144 + def get_coordinates_from_tracker_devices(self):
143 145 coord_robot_raw = self.trck_init_robot.Run()
144 146 coord_robot = np.array(coord_robot_raw)
145 147 coord_robot[3], coord_robot[5] = coord_robot[5], coord_robot[3]
... ... @@ -150,32 +152,32 @@ class ControlRobot(threading.Thread):
150 152 return coord_raw, coord_robot_raw, markers_flag
151 153  
152 154 def robot_move_decision(self, distance_target, coord_inv, coord_robot_raw, current_ref_filtered):
153   - if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arcmotion_flag:
  155 + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arc_motion_flag:
154 156 self.trck_init_robot.SendCoordinates(coord_inv, const.ROBOT_MOTIONS["normal"])
155 157  
156   - elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arcmotion_flag:
  158 + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arc_motion_flag:
157 159 actual_point = coord_robot_raw
158   - if not self.arcmotion_flag:
  160 + if not self.arc_motion_flag:
159 161 coord_head = self.process_tracker.estimate_head_center(self.tracker,
160 162 current_ref_filtered).tolist()
161 163  
162   - self.target_linearout, self.target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
163   - coord_inv)
164   - self.arcmotion_flag = True
165   - self.arcmotion_step_flag = const.ROBOT_MOTIONS["linear out"]
  164 + self.target_linear_out, self.target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head,
  165 + coord_inv)
  166 + self.arc_motion_flag = True
  167 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["linear out"]
166 168  
167   - if self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["linear out"]:
168   - coord = self.target_linearout
169   - if np.allclose(np.array(actual_point), np.array(self.target_linearout), 0, 1):
170   - self.arcmotion_step_flag = const.ROBOT_MOTIONS["arc"]
  169 + if self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["linear out"]:
  170 + coord = self.target_linear_out
  171 + if np.allclose(np.array(actual_point), np.array(self.target_linear_out), 0, 1):
  172 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["arc"]
171 173 coord = self.target_arc
172 174  
173   - elif self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["arc"]:
  175 + elif self.arc_motion_flag and self.arc_motion_step_flag == const.ROBOT_MOTIONS["arc"]:
174 176 coord_head = self.process_tracker.estimate_head_center(self.tracker,
175 177 current_ref_filtered).tolist()
176 178  
177   - _, new_target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
178   - coord_inv)
  179 + _, new_target_arc = self.process_tracker.compute_arc_motion(coord_robot_raw, coord_head,
  180 + coord_inv)
179 181 if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
180 182 None
181 183 else:
... ... @@ -186,11 +188,11 @@ class ControlRobot(threading.Thread):
186 188 coord = self.target_arc
187 189  
188 190 if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
189   - self.arcmotion_flag = False
190   - self.arcmotion_step_flag = const.ROBOT_MOTIONS["normal"]
  191 + self.arc_motion_flag = False
  192 + self.arc_motion_step_flag = const.ROBOT_MOTIONS["normal"]
191 193 coord = coord_inv
192 194  
193   - self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag)
  195 + self.trck_init_robot.SendCoordinates(coord, self.arc_motion_step_flag)
194 196  
195 197 def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag):
196 198 coord_ref_tracker_in_robot = coords_tracker_in_robot[1]
... ... @@ -202,7 +204,7 @@ class ControlRobot(threading.Thread):
202 204 current_ref_filtered = self.process_tracker.kalman_filter(current_ref)
203 205 if self.process_tracker.head_move_threshold(current_ref_filtered):
204 206 coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered,
205   - self.m_change_robot2ref)
  207 + self.m_change_robot_to_head)
206 208 if self.coord_inv_old is None:
207 209 self.coord_inv_old = coord_inv
208 210  
... ... @@ -220,19 +222,19 @@ class ControlRobot(threading.Thread):
220 222 self.trck_init_robot.StopRobot()
221 223  
222 224 def run(self):
223   - while not self.event.is_set():
224   - coords_tracker_in_robot, coord_robot_raw, markers_flag = self.getcoordsfromdevices()
  225 + while not self.event_robot.is_set():
  226 + coords_tracker_in_robot, coord_robot_raw, markers_flag = self.get_coordinates_from_tracker_devices()
225 227  
226   - if self.robottarget_queue.empty():
  228 + if self.robot_target_queue.empty():
227 229 None
228 230 else:
229   - self.robot_tracker_flag, self.m_change_robot2ref = self.robottarget_queue.get_nowait()
230   - self.robottarget_queue.task_done()
  231 + self.robot_tracker_flag, self.m_change_robot_to_head = self.robot_target_queue.get_nowait()
  232 + self.robot_target_queue.task_done()
231 233  
232   - if self.objattarget_queue.empty():
  234 + if self.object_at_target_queue.empty():
233 235 None
234 236 else:
235   - self.target_flag = self.objattarget_queue.get_nowait()
236   - self.objattarget_queue.task_done()
  237 + self.target_flag = self.object_at_target_queue.get_nowait()
  238 + self.object_at_target_queue.task_done()
237 239  
238 240 self.control(coords_tracker_in_robot, coord_robot_raw, markers_flag)
... ...
invesalius/navigation/tracker.py
  Diff comments   View file @839d45c
... ... @@ -92,7 +92,7 @@ class Tracker():
92 92 print("Tracker still connected!")
93 93  
94 94 def ConnectToRobot(self, navigation, tracker, robot):
95   - robot.SetRobotQueues([navigation.robottarget_queue, navigation.objattarget_queue])
  95 + robot.SetRobotQueues([navigation.robot_target_queue, navigation.object_at_target_queue])
96 96 robot.OnRobotConnection()
97 97 trk_init_robot = self.trk_init[1][0]
98 98 if trk_init_robot:
... ...