Download as
Browse Code »

Commit 329c784e2d4af2a6c7f78200d349975b6a5d9d95

Authored by Renan
1 parent acc75075
Exists in master

CLEANING

Showing 8 changed files with 196 additions and 313 deletions   Show diff stats
invesalius/constants.py
  Diff comments   View file @329c784
... ... @@ -670,9 +670,9 @@ CAMERA = 5
670 670 POLARIS = 6
671 671 POLARISP4 = 7
672 672 OPTITRACK = 8
673   -DEBUGTRACKRANDOM = 9
674   -DEBUGTRACKAPPROACH = 10
675   -HYBRID = 11
  673 +ROBOT = 9
  674 +DEBUGTRACKRANDOM = 10
  675 +DEBUGTRACKAPPROACH = 11
676 676 DEFAULT_TRACKER = SELECT
677 677  
678 678 NDICOMPORT = b'COM1'
... ... @@ -681,8 +681,8 @@ TRACKERS = [_("Claron MicronTracker"),
681 681 _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"),
682 682 _("Polhemus PATRIOT"), _("Camera tracker"),
683 683 _("NDI Polaris"), _("NDI Polaris P4"),
684   - _("Optitrack"), _("Debug tracker (random)"),
685   - _("Debug tracker (approach)"), _("Hybrid tracker")]
  684 + _("Optitrack"), _("Robot tracker"),
  685 + _("Debug tracker (random)"), _("Debug tracker (approach)")]
686 686  
687 687 STATIC_REF = 0
688 688 DYNAMIC_REF = 1
... ... @@ -811,4 +811,7 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") + "|" +\
811 811 ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1']
812 812 ROBOT_ElFIN_PORT = 10003
813 813  
814   -MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
815 814 \ No newline at end of file
  815 +ROBOT_MOTIONS = {"normal": 0, "linear out": 1, "arc": 2}
  816 +
  817 +ROBOT_HEAD_VELOCITY_THRESHOLD = 10
  818 +ROBOT_ARC_THRESHOLD_DISTANCE = 100
... ...
invesalius/data/coordinates.py
  Diff comments   View file @329c784
... ... @@ -65,9 +65,9 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode):
65 65 const.POLARIS: PolarisCoord,
66 66 const.POLARISP4: PolarisP4Coord,
67 67 const.OPTITRACK: OptitrackCoord,
  68 + const.ROBOT: RobotCoord,
68 69 const.DEBUGTRACKRANDOM: DebugCoordRandom,
69   - const.DEBUGTRACKAPPROACH: DebugCoordRandom,
70   - const.HYBRID: HybridCoord}
  70 + const.DEBUGTRACKAPPROACH: DebugCoordRandom}
71 71 coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode)
72 72 else:
73 73 print("Select Tracker")
... ... @@ -450,7 +450,7 @@ def dynamic_reference_m(probe, reference):
450 450 return coord_rot
451 451  
452 452  
453   -def HybridCoord(trk_init, trck_id, ref_mode):
  453 +def RobotCoord(trk_init, trck_id, ref_mode):
454 454 coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode)
455 455 coord_robot, _ = ElfinCoord(trk_init[1:])
456 456  
... ... @@ -459,7 +459,6 @@ def HybridCoord(trk_init, trck_id, ref_mode):
459 459 obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2])
460 460  
461 461 if probe_tracker_in_robot is None:
462   - #print("Getting raw tracker")
463 462 probe_tracker_in_robot = coord_tracker[0]
464 463 ref_tracker_in_robot = coord_tracker[1]
465 464 obj_tracker_in_robot = coord_tracker[2]
... ...
invesalius/data/coregistration.py
  Diff comments   View file @329c784
... ... @@ -20,11 +20,9 @@
20 20 import numpy as np
21 21 import queue
22 22 import threading
23   -from time import time, sleep
24   -import csv
  23 +from time import sleep
25 24  
26 25 import invesalius.constants as const
27   -import invesalius.data.coordinates as dco
28 26 import invesalius.data.transformations as tr
29 27 import invesalius.data.bases as bases
30 28  
... ... @@ -202,13 +200,6 @@ class CoordinateCorregistrate(threading.Thread):
202 200 #
203 201 self.target[1] = -self.target[1]
204 202  
205   - # self.time_start = time()
206   - # self.fieldnames = ["time",
207   - # "x_nav", "y_nav", "z_nav", "a_nav", "b_nav", "c_nav", "target"]
208   - # with open('data_nav.csv', 'w', newline='') as csv_file:
209   - # csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames)
210   - # csv_writer.writeheader()
211   -
212 203 def run(self):
213 204 coreg_data = self.coreg_data
214 205 view_obj = 1
... ... @@ -268,22 +259,6 @@ class CoordinateCorregistrate(threading.Thread):
268 259  
269 260 if not self.icp_queue.empty():
270 261 self.icp_queue.task_done()
271   -
272   - # with open('data_nav.csv', 'a', newline='') as csv_file:
273   - # csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames)
274   - #
275   - # info = {
276   - # "time": time() - self.time_start,
277   - # "x_nav": coord[0],
278   - # "y_nav": coord[1],
279   - # "z_nav": coord[2],
280   - # "a_nav": coord[3],
281   - # "b_nav": coord[4],
282   - # "c_nav": coord[5],
283   - # "target": self.target_flag
284   - # }
285   - #
286   - # csv_writer.writerow(info)
287 262 # The sleep has to be in both threads
288 263 sleep(self.sle)
289 264 except queue.Full:
... ...
invesalius/data/elfin_robot.py
  Diff comments   View file @329c784
... ... @@ -2,7 +2,6 @@ import numpy as np
2 2 import cv2
3 3 from time import time, sleep
4 4 import threading
5   -import csv
6 5  
7 6 import invesalius.data.elfin as elfin
8 7 import invesalius.data.transformations as tr
... ... @@ -10,12 +9,10 @@ import invesalius.data.coregistration as dcr
10 9 import invesalius.constants as const
11 10  
12 11  
13   -
14 12 class elfin_server():
15 13 def __init__(self, server_ip, port_number):
16 14 self.server_ip = server_ip
17 15 self.port_number = port_number
18   - #print(cobot.ReadPcsActualPos())
19 16  
20 17 def Initialize(self):
21 18 SIZE = 1024
... ... @@ -25,16 +22,14 @@ class elfin_server():
25 22 print("conected!")
26 23  
27 24 def Run(self):
28   - #target = [540.0, -30.0, 850.0, 140.0, -81.0, -150.0]
29   - #print("starting move")
30 25 return self.cobot.ReadPcsActualPos()
31 26  
32   - def SendCoordinates(self, target, type=const.MOTIONS["normal"]):
  27 + def SendCoordinates(self, target, type=const.ROBOT_MOTIONS["normal"]):
33 28 status = self.cobot.ReadMoveState()
34   - if type == const.MOTIONS["normal"] or type == const.MOTIONS["linear out"]:
  29 + if type == const.ROBOT_MOTIONS["normal"] or type == const.ROBOT_MOTIONS["linear out"]:
35 30 if status != 1009:
36 31 self.cobot.MoveL(target)
37   - elif type == const.MOTIONS["arc"]:
  32 + elif type == const.ROBOT_MOTIONS["arc"]:
38 33 if status != 1009:
39 34 self.cobot.MoveC(target)
40 35  
... ... @@ -45,8 +40,8 @@ class elfin_server():
45 40 def Close(self):
46 41 self.cobot.close()
47 42  
48   -class KalmanTracker:
49 43  
  44 +class KalmanTracker:
50 45 def __init__(self,
51 46 state_num=2,
52 47 cov_process=0.001,
... ... @@ -77,6 +72,7 @@ class KalmanTracker:
77 72 self.filter.correct(self.measurement)
78 73 self.state = self.filter.statePost
79 74  
  75 +
80 76 class TrackerProcessing:
81 77 def __init__(self):
82 78 self.coord_vel = []
... ... @@ -90,7 +86,6 @@ class TrackerProcessing:
90 86 cov_process=0.001,
91 87 cov_measure=0.1) for _ in range(6)]
92 88  
93   -
94 89 def kalman_filter(self, coord_tracker):
95 90 kalman_array = []
96 91 pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten()
... ... @@ -108,7 +103,6 @@ class TrackerProcessing:
108 103  
109 104 return coord_kalman
110 105  
111   -
112 106 def estimate_head_velocity(self, coord_vel, timestamp):
113 107 coord_vel = np.vstack(np.array(coord_vel))
114 108 coord_init = coord_vel[:int(len(coord_vel)/2)].mean(axis=0)
... ... @@ -118,36 +112,35 @@ class TrackerProcessing:
118 112  
119 113 return velocity, distance
120 114  
121   - def Versores(self,init_point, final_point):
  115 + def versors(self, init_point, final_point):
122 116 init_point = np.array(init_point)
123 117 final_point = np.array(final_point)
124 118 norm = (sum((final_point-init_point)**2))**0.5
125   - versorfator = (((final_point-init_point)/norm)*70).tolist()
  119 + versorfactor = (((final_point-init_point)/norm)*70).tolist()
126 120  
127   - return versorfator
  121 + return versorfactor
128 122  
129 123 def arcmotion(self, actual_point, coord_head, coord_inv):
130   -
131 124 p1 = coord_inv
132 125  
133 126 pc = coord_head[0], coord_head[1], coord_head[2], coord_inv[3], coord_inv[4], coord_inv[5]
134 127  
135   - versorfator1_calculado = self.Versores(pc,actual_point)
136   - init_ext_point = actual_point[0]+versorfator1_calculado[0],\
137   - actual_point[1]+versorfator1_calculado[1],\
138   - actual_point[2]+versorfator1_calculado[2], \
  128 + versorfactor1 = self.versors(pc, actual_point)
  129 + init_ext_point = actual_point[0]+versorfactor1[0],\
  130 + actual_point[1]+versorfactor1[1],\
  131 + actual_point[2]+versorfactor1[2], \
139 132 actual_point[3], actual_point[4], actual_point[5]
140 133  
141   - pm = ((p1[0]+actual_point[0])/2, (p1[1]+actual_point[1])/2,(p1[2]+actual_point[2])/2,0,0,0 ) #ponto médio da trajetória
  134 + middle_point = ((p1[0]+actual_point[0])/2, (p1[1]+actual_point[1])/2, (p1[2]+actual_point[2])/2, 0, 0, 0)
142 135  
143   - newarr = (np.array(self.Versores(pc,pm)))*2
144   - pontointermediario = pm[0]+newarr[0],pm[1]+newarr[1],pm[2]+newarr[2]
  136 + newarr = (np.array(self.versors(pc, middle_point)))*2
  137 + middle_arc_point = middle_point[0]+newarr[0], middle_point[1]+newarr[1], middle_point[2]+newarr[2]
145 138  
146   - versorfator3 = self.Versores(pc,p1)
147   - final_ext_point = p1[0]+versorfator3[0],p1[1]+versorfator3[1],p1[2]+versorfator3[2], coord_inv[3], coord_inv[4], coord_inv[5]
148   - final_ext_point_arc = final_ext_point = p1[0]+versorfator3[0],p1[1]+versorfator3[1],p1[2]+versorfator3[2], coord_inv[3], coord_inv[4], coord_inv[5],0
149   - #type_arc = 0
150   - target_arc = pontointermediario+final_ext_point_arc
  139 + versorfactor3 = self.versors(pc, p1)
  140 +
  141 + final_ext_arc_point = p1[0]+versorfactor3[0], p1[1]+versorfactor3[1], p1[2]+versorfactor3[2], \
  142 + coord_inv[3], coord_inv[4], coord_inv[5], 0
  143 + target_arc = middle_arc_point+final_ext_arc_point
151 144  
152 145 return init_ext_point, target_arc
153 146  
... ... @@ -165,7 +158,7 @@ class TrackerProcessing:
165 158 self.velocity_std = np.std(self.velocity_vector)
166 159 del self.velocity_vector[0]
167 160  
168   - if self.velocity_std > 10:
  161 + if self.velocity_std > const.ROBOT_HEAD_VELOCITY_THRESHOLD:
169 162 print('Velocity threshold activated')
170 163 return False
171 164 else:
... ... @@ -173,7 +166,6 @@ class TrackerProcessing:
173 166  
174 167 return False
175 168  
176   -
177 169 def head_move_compensation(self, current_ref, m_change_robot2ref):
178 170 trans = tr.translation_matrix(current_ref[:3])
179 171 a, b, g = np.radians(current_ref[3:6])
... ... @@ -184,8 +176,7 @@ class TrackerProcessing:
184 176 _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new)
185 177 angles = np.degrees(angles)
186 178  
187   - return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], \
188   - angles[2]
  179 + return m_robot_new[0, -1], m_robot_new[1, -1], m_robot_new[2, -1], angles[0], angles[1], angles[2]
189 180  
190 181 def estimate_head_center(self, tracker, current_ref):
191 182 m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion = tracker.GetMatrixTrackerFiducials()
... ... @@ -196,12 +187,11 @@ class TrackerProcessing:
196 187  
197 188 return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2
198 189  
199   - def correction_distance_calculation_target(self, coord_inv,actual_point):
200   - #a posição atual do robo está embaixo disso
  190 + def correction_distance_calculation_target(self, coord_inv, actual_point):
201 191 sum = (coord_inv[0]-actual_point[0])**2\
202   - +(coord_inv[1]-actual_point[1])**2\
203   - +(coord_inv[2]-actual_point[2])**2
204   - correction_distance_compensation = pow(sum,0.5)
  192 + + (coord_inv[1]-actual_point[1])**2\
  193 + + (coord_inv[2]-actual_point[2])**2
  194 + correction_distance_compensation = pow(sum, 0.5)
205 195  
206 196 return correction_distance_compensation
207 197  
... ... @@ -216,6 +206,7 @@ class RobotCoordinates():
216 206 def GetRobotCoordinates(self):
217 207 return self.coord
218 208  
  209 +
219 210 class ControlRobot(threading.Thread):
220 211 def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event):
221 212 threading.Thread.__init__(self, name='ControlRobot')
... ... @@ -240,15 +231,6 @@ class ControlRobot(threading.Thread):
240 231 self.target_linearin = None
241 232 self.target_arc = None
242 233  
243   - self.time_start = time()
244   - self.fieldnames = ["time",
245   - "x_robot", "y_robot", "z_robot", "a_robot", "b_robot", "c_robot",
246   - "x_tracker_obj", "y_tracker_obj", "z_tracker_obj", "a_tracker_obj", "b_tracker_obj", "c_tracker_obj",
247   - "velocity_std", "target"]
248   - with open('data_robot_and_tracker.csv', 'w', newline='') as csv_file:
249   - csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames)
250   - csv_writer.writeheader()
251   -
252 234 def getcoordsfromdevices(self):
253 235 coord_robot_raw = self.trck_init_robot.Run()
254 236 coord_robot = np.array(coord_robot_raw)
... ... @@ -259,6 +241,49 @@ class ControlRobot(threading.Thread):
259 241  
260 242 return coord_raw, coord_robot_raw, markers_flag
261 243  
  244 + def robot_move_decision(self, distance_target, coord_inv, coord_robot_raw, current_ref_filtered):
  245 + if distance_target < const.ROBOT_ARC_THRESHOLD_DISTANCE and not self.arcmotion_flag:
  246 + self.trck_init_robot.SendCoordinates(coord_inv, const.ROBOT_MOTIONS["normal"])
  247 +
  248 + elif distance_target >= const.ROBOT_ARC_THRESHOLD_DISTANCE or self.arcmotion_flag:
  249 + actual_point = coord_robot_raw
  250 + if not self.arcmotion_flag:
  251 + coord_head = self.process_tracker.estimate_head_center(self.tracker,
  252 + current_ref_filtered).tolist()
  253 +
  254 + self.target_linearout, self.target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
  255 + coord_inv)
  256 + self.arcmotion_flag = True
  257 + self.arcmotion_step_flag = const.ROBOT_MOTIONS["linear out"]
  258 +
  259 + if self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["linear out"]:
  260 + coord = self.target_linearout
  261 + if np.allclose(np.array(actual_point), np.array(self.target_linearout), 0, 1):
  262 + self.arcmotion_step_flag = const.ROBOT_MOTIONS["arc"]
  263 + coord = self.target_arc
  264 +
  265 + elif self.arcmotion_flag and self.arcmotion_step_flag == const.ROBOT_MOTIONS["arc"]:
  266 + coord_head = self.process_tracker.estimate_head_center(self.tracker,
  267 + current_ref_filtered).tolist()
  268 +
  269 + _, new_target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
  270 + coord_inv)
  271 + if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
  272 + None
  273 + else:
  274 + if self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw) >= \
  275 + const.ROBOT_ARC_THRESHOLD_DISTANCE*0.8:
  276 + self.target_arc = new_target_arc
  277 +
  278 + coord = self.target_arc
  279 +
  280 + if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
  281 + self.arcmotion_flag = False
  282 + self.arcmotion_step_flag = const.ROBOT_MOTIONS["normal"]
  283 + coord = coord_inv
  284 +
  285 + self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag)
  286 +
262 287 def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag):
263 288 coord_ref_tracker_in_robot = coords_tracker_in_robot[1]
264 289 coord_obj_tracker_in_robot = coords_tracker_in_robot[2]
... ... @@ -266,8 +291,7 @@ class ControlRobot(threading.Thread):
266 291 if self.robot_tracker_flag:
267 292 current_ref = coord_ref_tracker_in_robot
268 293 if current_ref is not None and markers_flag[1]:
269   - #current_ref_filtered = self.process_tracker.kalman_filter(current_ref)
270   - current_ref_filtered = current_ref
  294 + current_ref_filtered = self.process_tracker.kalman_filter(current_ref)
271 295 if self.process_tracker.head_move_threshold(current_ref_filtered):
272 296 coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered,
273 297 self.m_change_robot2ref)
... ... @@ -275,62 +299,19 @@ class ControlRobot(threading.Thread):
275 299 self.coord_inv_old = coord_inv
276 300  
277 301 if np.allclose(np.array(coord_inv), np.array(coord_robot_raw), 0, 0.01):
278   - # print("At target within range 5")
  302 + # print("At target within range 1")
279 303 pass
280 304 elif not np.allclose(np.array(coord_inv), np.array(self.coord_inv_old), 0, 5):
281   - # print("stop")
282 305 self.trck_init_robot.StopRobot()
283 306 self.coord_inv_old = coord_inv
284 307 else:
285   - #self.trck_init_robot.SendCoordinates(coord_inv, const.MOTIONS["normal"])
286 308 distance_target = self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw)
287   - if distance_target < 100 and not self.arcmotion_flag:
288   - self.trck_init_robot.SendCoordinates(coord_inv, const.MOTIONS["normal"])
289   -
290   - elif distance_target >= 100 or self.arcmotion_flag:
291   - actual_point = coord_robot_raw
292   - if not self.arcmotion_flag:
293   - coord_head = self.process_tracker.estimate_head_center(self.tracker,
294   - current_ref_filtered).tolist()
295   -
296   - self.target_linearout, self.target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
297   - coord_inv)
298   - self.arcmotion_flag = True
299   - self.arcmotion_step_flag = const.MOTIONS["linear out"]
300   -
301   - if self.arcmotion_flag and self.arcmotion_step_flag == const.MOTIONS["linear out"]:
302   - coord = self.target_linearout
303   - if np.allclose(np.array(actual_point), np.array(self.target_linearout), 0, 1):
304   - self.arcmotion_step_flag = const.MOTIONS["arc"]
305   - coord = self.target_arc
306   -
307   - elif self.arcmotion_flag and self.arcmotion_step_flag == const.MOTIONS["arc"]:
308   - coord_head = self.process_tracker.estimate_head_center(self.tracker,
309   - current_ref_filtered).tolist()
310   -
311   - _, new_target_arc = self.process_tracker.arcmotion(coord_robot_raw, coord_head,
312   - coord_inv)
313   - if np.allclose(np.array(new_target_arc[3:-1]), np.array(self.target_arc[3:-1]), 0, 1):
314   - None
315   - else:
316   - if self.process_tracker.correction_distance_calculation_target(coord_inv,
317   - coord_robot_raw) >= 80:
318   - self.target_arc = new_target_arc
319   -
320   - coord = self.target_arc
321   -
322   - if np.allclose(np.array(actual_point), np.array(self.target_arc[3:-1]), 0, 10):
323   - self.arcmotion_flag = False
324   - self.arcmotion_step_flag = const.MOTIONS["normal"]
325   - coord = coord_inv
326   -
327   - self.trck_init_robot.SendCoordinates(coord, self.arcmotion_step_flag)
  309 + self.robot_move_decision(distance_target, coord_inv, coord_robot_raw, current_ref_filtered)
328 310 self.coord_inv_old = coord_inv
329 311 else:
330 312 self.trck_init_robot.StopRobot()
331 313  
332 314 def run(self):
333   -
334 315 while not self.event.is_set():
335 316 coords_tracker_in_robot, coord_robot_raw, markers_flag = self.getcoordsfromdevices()
336 317  
... ... @@ -348,33 +329,4 @@ class ControlRobot(threading.Thread):
348 329  
349 330 self.control(coords_tracker_in_robot, coord_robot_raw, markers_flag)
350 331  
351   - # if not self.robottarget_queue.empty():
352   - # self.robottarget_queue.task_done()
353   - # if not self.objattarget_queue.empty():
354   - # self.objattarget_queue.task_done()
355   -
356   -
357   - with open('data_robot_and_tracker.csv', 'a', newline='') as csv_file:
358   - csv_writer = csv.DictWriter(csv_file, fieldnames=self.fieldnames)
359   -
360   - info = {
361   - "time": time() - self.time_start,
362   - "x_robot": coord_robot_raw[0],
363   - "y_robot": coord_robot_raw[1],
364   - "z_robot": coord_robot_raw[2],
365   - "a_robot": coord_robot_raw[3],
366   - "b_robot": coord_robot_raw[4],
367   - "c_robot": coord_robot_raw[5],
368   - "x_tracker_obj": coords_tracker_in_robot[2][0],
369   - "y_tracker_obj": coords_tracker_in_robot[2][1],
370   - "z_tracker_obj": coords_tracker_in_robot[2][2],
371   - "a_tracker_obj": coords_tracker_in_robot[2][3],
372   - "b_tracker_obj": coords_tracker_in_robot[2][4],
373   - "c_tracker_obj": coords_tracker_in_robot[2][5],
374   - "velocity_std": self.process_tracker.velocity_std,
375   - "target": self.target_flag
376   - }
377   -
378   - csv_writer.writerow(info)
379   -
380   - sleep(0.02)
381 332 \ No newline at end of file
  333 + sleep(0.01)
... ...
invesalius/data/plot_realtime.py
View file @acc7507
... ... @@ -1,39 +0,0 @@
1   -import random
2   -from itertools import count
3   -import pandas as pd
4   -import matplotlib
5   -import matplotlib.pyplot as plt
6   -from matplotlib.animation import FuncAnimation
7   -
8   -#plt.style.use('fivethirtyeight')
9   -matplotlib.use('TkAgg')
10   -
11   -x_vals = []
12   -y_vals = []
13   -
14   -index = count()
15   -fig, (ax1, ax2) = plt.subplots(2)
16   -
17   -def animate(i):
18   - data = pd.read_csv('D:\\Repository\\invesalius3\\data.csv')
19   - x = data['time']
20   - y1 = data['x']
21   - y2 = data['xf']
22   - status_x = data['statusx']
23   -
24   - ax1.cla()
25   - ax2.cla()
26   -
27   - ax1.plot(x, y1, label='Coord x')
28   - ax1.plot(x, y2, label='Coord x after Kalman')
29   -
30   - ax2.plot(x, status_x, label='STD', color='red')
31   -
32   - ax1.legend(loc='upper left')
33   - ax2.legend(loc='upper left')
34   -
35   -
36   -ani = FuncAnimation(fig, animate, interval=0.1)
37   -
38   -plt.tight_layout()
39   -plt.show()
40 0 \ No newline at end of file
invesalius/data/trackers.py
  Diff comments   View file @329c784
... ... @@ -41,10 +41,10 @@ def TrackerConnection(tracker_id, trck_init, action):
41 41 const.CAMERA: CameraTracker, # CAMERA
42 42 const.POLARIS: PolarisTracker, # POLARIS
43 43 const.POLARISP4: PolarisP4Tracker, # POLARISP4
44   - const.OPTITRACK: OptitrackTracker, #Optitrack
  44 + const.OPTITRACK: OptitrackTracker, #Optitrack
  45 + const.ROBOT: RobotTracker, #Robot
45 46 const.DEBUGTRACKRANDOM: DebugTrackerRandom,
46   - const.DEBUGTRACKAPPROACH: DebugTrackerApproach,
47   - const.HYBRID: HybridTracker}
  47 + const.DEBUGTRACKAPPROACH: DebugTrackerApproach}
48 48  
49 49 trck_init = trck_fcn[tracker_id](tracker_id)
50 50  
... ... @@ -66,36 +66,68 @@ def DefaultTracker(tracker_id):
66 66 # return tracker initialization variable and type of connection
67 67 return trck_init, 'wrapper'
68 68  
69   -def OptitrackTracker(tracker_id):
70   - """
71   - Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
72   - (Rigid bodies information).
  69 +def ClaronTracker(tracker_id):
  70 + import invesalius.constants as const
  71 + from invesalius import inv_paths
73 72  
74   - Parameters
75   - ----------
76   - tracker_id : Optitrack ID
  73 + trck_init = None
  74 + try:
  75 + import pyclaron
77 76  
78   - Returns
79   - -------
80   - trck_init : local name for Optitrack module
81   - """
82   - from wx import ID_OK
  77 + lib_mode = 'wrapper'
  78 + trck_init = pyclaron.pyclaron()
  79 + trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
  80 + trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
  81 + trck_init.NumberFramesProcessed = 1
  82 + trck_init.FramesExtrapolated = 0
  83 + trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
  84 + trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
  85 + trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
  86 + trck_init.Initialize()
  87 +
  88 + if trck_init.GetIdentifyingCamera():
  89 + trck_init.Run()
  90 + print("MicronTracker camera identified.")
  91 + else:
  92 + trck_init = None
  93 +
  94 + except ImportError:
  95 + lib_mode = 'error'
  96 + print('The ClaronTracker library is not installed.')
  97 +
  98 + return trck_init, lib_mode
  99 +
  100 +def PolhemusTracker(tracker_id):
  101 + try:
  102 + trck_init = PlhWrapperConnection(tracker_id)
  103 + lib_mode = 'wrapper'
  104 + if not trck_init:
  105 + print('Could not connect with Polhemus wrapper, trying USB connection...')
  106 + trck_init = PlhUSBConnection(tracker_id)
  107 + lib_mode = 'usb'
  108 + if not trck_init:
  109 + print('Could not connect with Polhemus USB, trying serial connection...')
  110 + trck_init = PlhSerialConnection(tracker_id)
  111 + lib_mode = 'serial'
  112 + except:
  113 + trck_init = None
  114 + lib_mode = 'error'
  115 + print('Could not connect to Polhemus by any method.')
  116 +
  117 + return trck_init, lib_mode
  118 +
  119 +def CameraTracker(tracker_id):
83 120 trck_init = None
84   - dlg_port = dlg.SetOptitrackconfigs()
85   - if dlg_port.ShowModal() == ID_OK:
86   - Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
87   - try:
88   - import optitrack
89   - trck_init = optitrack.optr()
  121 + try:
  122 + import invesalius.data.camera_tracker as cam
  123 + trck_init = cam.camera()
  124 + trck_init.Initialize()
  125 + print('Connect to camera tracking device.')
90 126  
91   - if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
92   - trck_init.Run() #Runs once Run function, to update cameras.
93   - else:
94   - trck_init = None
95   - except ImportError:
96   - print('Error')
97   - else:
98   - print('#####')
  127 + except:
  128 + print('Could not connect to camera tracker.')
  129 +
  130 + # return tracker initialization variable and type of connection
99 131 return trck_init, 'wrapper'
100 132  
101 133 def PolarisTracker(tracker_id):
... ... @@ -127,7 +159,6 @@ def PolarisTracker(tracker_id):
127 159 # return tracker initialization variable and type of connection
128 160 return trck_init, lib_mode
129 161  
130   -
131 162 def PolarisP4Tracker(tracker_id):
132 163 from wx import ID_OK
133 164 trck_init = None
... ... @@ -156,19 +187,36 @@ def PolarisP4Tracker(tracker_id):
156 187 # return tracker initialization variable and type of connection
157 188 return trck_init, lib_mode
158 189  
  190 +def OptitrackTracker(tracker_id):
  191 + """
  192 + Imports optitrack wrapper from Motive 2.2. Initialize cameras, attach listener, loads Calibration, loads User Profile
  193 + (Rigid bodies information).
159 194  
160   -def CameraTracker(tracker_id):
161   - trck_init = None
162   - try:
163   - import invesalius.data.camera_tracker as cam
164   - trck_init = cam.camera()
165   - trck_init.Initialize()
166   - print('Connect to camera tracking device.')
  195 + Parameters
  196 + ----------
  197 + tracker_id : Optitrack ID
167 198  
168   - except:
169   - print('Could not connect to camera tracker.')
  199 + Returns
  200 + -------
  201 + trck_init : local name for Optitrack module
  202 + """
  203 + from wx import ID_OK
  204 + trck_init = None
  205 + dlg_port = dlg.SetOptitrackconfigs()
  206 + if dlg_port.ShowModal() == ID_OK:
  207 + Cal_optitrack, User_profile_optitrack = dlg_port.GetValue()
  208 + try:
  209 + import optitrack
  210 + trck_init = optitrack.optr()
170 211  
171   - # return tracker initialization variable and type of connection
  212 + if trck_init.Initialize(Cal_optitrack, User_profile_optitrack)==0:
  213 + trck_init.Run() #Runs once Run function, to update cameras.
  214 + else:
  215 + trck_init = None
  216 + except ImportError:
  217 + print('Error')
  218 + else:
  219 + print('#####')
172 220 return trck_init, 'wrapper'
173 221  
174 222 def ElfinRobot(robot_IP):
... ... @@ -189,57 +237,24 @@ def ElfinRobot(robot_IP):
189 237 # return tracker initialization variable and type of connection
190 238 return trck_init, lib_mode
191 239  
192   -def ClaronTracker(tracker_id):
193   - import invesalius.constants as const
194   - from invesalius import inv_paths
195   -
  240 +def RobotTracker(tracker_id):
  241 + from wx import ID_OK
196 242 trck_init = None
197   - try:
198   - import pyclaron
199   -
200   - lib_mode = 'wrapper'
201   - trck_init = pyclaron.pyclaron()
202   - trck_init.CalibrationDir = inv_paths.MTC_CAL_DIR.encode(const.FS_ENCODE)
203   - trck_init.MarkerDir = inv_paths.MTC_MAR_DIR.encode(const.FS_ENCODE)
204   - trck_init.NumberFramesProcessed = 1
205   - trck_init.FramesExtrapolated = 0
206   - trck_init.PROBE_NAME = const.MTC_PROBE_NAME.encode(const.FS_ENCODE)
207   - trck_init.REF_NAME = const.MTC_REF_NAME.encode(const.FS_ENCODE)
208   - trck_init.OBJ_NAME = const.MTC_OBJ_NAME.encode(const.FS_ENCODE)
209   - trck_init.Initialize()
210   -
211   - if trck_init.GetIdentifyingCamera():
212   - trck_init.Run()
213   - print("MicronTracker camera identified.")
214   - else:
215   - trck_init = None
216   -
217   - except ImportError:
218   - lib_mode = 'error'
219   - print('The ClaronTracker library is not installed.')
220   -
221   - return trck_init, lib_mode
222   -
223   -
224   -def PolhemusTracker(tracker_id):
225   - try:
226   - trck_init = PlhWrapperConnection(tracker_id)
227   - lib_mode = 'wrapper'
228   - if not trck_init:
229   - print('Could not connect with Polhemus wrapper, trying USB connection...')
230   - trck_init = PlhUSBConnection(tracker_id)
231   - lib_mode = 'usb'
232   - if not trck_init:
233   - print('Could not connect with Polhemus USB, trying serial connection...')
234   - trck_init = PlhSerialConnection(tracker_id)
235   - lib_mode = 'serial'
236   - except:
237   - trck_init = None
238   - lib_mode = 'error'
239   - print('Could not connect to Polhemus by any method.')
240   -
241   - return trck_init, lib_mode
  243 + trck_init_robot = None
  244 + tracker_id = None
  245 + dlg_device = dlg.SetTrackerDevice2Robot()
  246 + if dlg_device.ShowModal() == ID_OK:
  247 + tracker_id = dlg_device.GetValue()
  248 + if tracker_id:
  249 + trck_connection = TrackerConnection(tracker_id, None, 'connect')
  250 + if trck_connection[0]:
  251 + dlg_ip = dlg.SetRobotIP()
  252 + if dlg_ip.ShowModal() == ID_OK:
  253 + robot_IP = dlg_ip.GetValue()
  254 + trck_init = trck_connection
  255 + trck_init_robot = ElfinRobot(robot_IP)
242 256  
  257 + return [trck_init, trck_init_robot, tracker_id]
243 258  
244 259 def DebugTrackerRandom(tracker_id):
245 260 trck_init = True
... ... @@ -351,26 +366,6 @@ def PlhUSBConnection(tracker_id):
351 366  
352 367 return trck_init
353 368  
354   -def HybridTracker(tracker_id):
355   - from wx import ID_OK
356   - trck_init = None
357   - trck_init_robot = None
358   - tracker_id = None
359   - dlg_device = dlg.SetTrackerDevice2Robot()
360   - if dlg_device.ShowModal() == ID_OK:
361   - tracker_id = dlg_device.GetValue()
362   - if tracker_id:
363   - trck_connection = TrackerConnection(tracker_id, None, 'connect')
364   - if trck_connection[0]:
365   - dlg_ip = dlg.SetRobotIP()
366   - if dlg_ip.ShowModal() == ID_OK:
367   - robot_IP = dlg_ip.GetValue()
368   - trck_init = trck_connection
369   - trck_init_robot = ElfinRobot(robot_IP)
370   -
371   - return [trck_init, trck_init_robot, tracker_id]
372   -
373   -
374 369 def DisconnectTracker(tracker_id, trck_init):
375 370 """
376 371 Disconnect current spatial tracker
... ... @@ -390,7 +385,7 @@ def DisconnectTracker(tracker_id, trck_init):
390 385 trck_init = False
391 386 lib_mode = 'serial'
392 387 print('Tracker disconnected.')
393   - elif tracker_id == const.HYBRID:
  388 + elif tracker_id == const.ROBOT:
394 389 trck_init[0].Close()
395 390 trck_init = False
396 391 lib_mode = 'wrapper'
... ...
invesalius/gui/dialogs.py
  Diff comments   View file @329c784
... ... @@ -875,9 +875,9 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode):
875 875 const.POLARIS: 'NDI Polaris',
876 876 const.POLARISP4: 'NDI Polaris P4',
877 877 const.OPTITRACK: 'Optitrack',
  878 + const.ROBOT: 'Robotic navigation',
878 879 const.DEBUGTRACKRANDOM: 'Debug tracker device (random)',
879   - const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)',
880   - const.HYBRID: 'Hybrid tracker device'}
  880 + const.DEBUGTRACKAPPROACH: 'Debug tracker device (approach)'}
881 881  
882 882 if lib_mode == 'choose':
883 883 msg = _('No tracking device selected')
... ... @@ -3482,7 +3482,7 @@ class ObjectCalibrationDialog(wx.Dialog):
3482 3482 if self.trk_init and self.tracker_id:
3483 3483 coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates()
3484 3484 if self.obj_ref_id and btn_id == 4:
3485   - if self.tracker_id == const.HYBRID:
  3485 + if self.tracker_id == const.ROBOT:
3486 3486 trck_init_robot = self.trk_init[1][0]
3487 3487 coord = trck_init_robot.Run()
3488 3488 else:
... ...
invesalius/gui/task_navigator.py
  Diff comments   View file @329c784
... ... @@ -1045,7 +1045,7 @@ class NeuronavigationPanel(wx.Panel):
1045 1045 # self.tracker.TrackerCoordinates, self.navigation.event).start()
1046 1046 #sleep(5)
1047 1047  
1048   - if self.tracker.tracker_id == const.HYBRID:
  1048 + if self.tracker.tracker_id == const.ROBOT:
1049 1049 self.robot.SetRobotQueues([self.navigation.robottarget_queue,
1050 1050 self.navigation.objattarget_queue])
1051 1051 self.robot.OnRobotConnection(self.tracker, self.robotcoordinates)
... ... @@ -1143,8 +1143,6 @@ class NeuronavigationPanel(wx.Panel):
1143 1143 btn_c.Enable(False)
1144 1144  
1145 1145 self.navigation.StartNavigation(self.tracker)
1146   - #if self.tracker.tracker_id == const.HYBRID:
1147   - # self.robot.StartRobotNavigation(self.tracker, self.navigation.coord_queue, self.navigation.robot_event)
1148 1146  
1149 1147 if not self.CheckFiducialRegistrationError():
1150 1148 # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok
... ...