Commit 329c784e2d4af2a6c7f78200d349975b6a5d9d95
1 parent
acc75075
Exists in
master
CLEANING
Showing
8 changed files
with
196 additions
and
313 deletions
Show diff stats
invesalius/constants.py
@@ -670,9 +670,9 @@ CAMERA = 5 | @@ -670,9 +670,9 @@ CAMERA = 5 | ||
670 | POLARIS = 6 | 670 | POLARIS = 6 |
671 | POLARISP4 = 7 | 671 | POLARISP4 = 7 |
672 | OPTITRACK = 8 | 672 | OPTITRACK = 8 |
673 | -DEBUGTRACKRANDOM = 9 | ||
674 | -DEBUGTRACKAPPROACH = 10 | ||
675 | -HYBRID = 11 | 673 | +ROBOT = 9 |
674 | +DEBUGTRACKRANDOM = 10 | ||
675 | +DEBUGTRACKAPPROACH = 11 | ||
676 | DEFAULT_TRACKER = SELECT | 676 | DEFAULT_TRACKER = SELECT |
677 | 677 | ||
678 | NDICOMPORT = b'COM1' | 678 | NDICOMPORT = b'COM1' |
@@ -681,8 +681,8 @@ TRACKERS = [_("Claron MicronTracker"), | @@ -681,8 +681,8 @@ TRACKERS = [_("Claron MicronTracker"), | ||
681 | _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"), | 681 | _("Polhemus FASTRAK"), _("Polhemus ISOTRAK II"), |
682 | _("Polhemus PATRIOT"), _("Camera tracker"), | 682 | _("Polhemus PATRIOT"), _("Camera tracker"), |
683 | _("NDI Polaris"), _("NDI Polaris P4"), | 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 | STATIC_REF = 0 | 687 | STATIC_REF = 0 |
688 | DYNAMIC_REF = 1 | 688 | DYNAMIC_REF = 1 |
@@ -811,4 +811,7 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") + "|" +\ | @@ -811,4 +811,7 @@ WILDCARD_MARKER_FILES = _("Marker scanner coord files (*.mkss)|*.mkss") + "|" +\ | ||
811 | ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1'] | 811 | ROBOT_ElFIN_IP = ['Select robot IP:', '143.107.220.251', '169.254.153.251', '127.0.0.1'] |
812 | ROBOT_ElFIN_PORT = 10003 | 812 | ROBOT_ElFIN_PORT = 10003 |
813 | 813 | ||
814 | -MOTIONS = {"normal": 0, "linear out": 1, "arc": 2} | ||
815 | \ No newline at end of file | 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
@@ -65,9 +65,9 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode): | @@ -65,9 +65,9 @@ def GetCoordinatesForThread(trck_init, trck_id, ref_mode): | ||
65 | const.POLARIS: PolarisCoord, | 65 | const.POLARIS: PolarisCoord, |
66 | const.POLARISP4: PolarisP4Coord, | 66 | const.POLARISP4: PolarisP4Coord, |
67 | const.OPTITRACK: OptitrackCoord, | 67 | const.OPTITRACK: OptitrackCoord, |
68 | + const.ROBOT: RobotCoord, | ||
68 | const.DEBUGTRACKRANDOM: DebugCoordRandom, | 69 | const.DEBUGTRACKRANDOM: DebugCoordRandom, |
69 | - const.DEBUGTRACKAPPROACH: DebugCoordRandom, | ||
70 | - const.HYBRID: HybridCoord} | 70 | + const.DEBUGTRACKAPPROACH: DebugCoordRandom} |
71 | coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode) | 71 | coord, markers_flag = getcoord[trck_id](trck_init, trck_id, ref_mode) |
72 | else: | 72 | else: |
73 | print("Select Tracker") | 73 | print("Select Tracker") |
@@ -450,7 +450,7 @@ def dynamic_reference_m(probe, reference): | @@ -450,7 +450,7 @@ def dynamic_reference_m(probe, reference): | ||
450 | return coord_rot | 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 | coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode) | 454 | coord_tracker, markers_flag = GetCoordinatesForThread(trk_init[0], trk_init[2], ref_mode) |
455 | coord_robot, _ = ElfinCoord(trk_init[1:]) | 455 | coord_robot, _ = ElfinCoord(trk_init[1:]) |
456 | 456 | ||
@@ -459,7 +459,6 @@ def HybridCoord(trk_init, trck_id, ref_mode): | @@ -459,7 +459,6 @@ def HybridCoord(trk_init, trck_id, ref_mode): | ||
459 | obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2]) | 459 | obj_tracker_in_robot = db.transform_tracker_2_robot().transformation_tracker_2_robot(coord_tracker[2]) |
460 | 460 | ||
461 | if probe_tracker_in_robot is None: | 461 | if probe_tracker_in_robot is None: |
462 | - #print("Getting raw tracker") | ||
463 | probe_tracker_in_robot = coord_tracker[0] | 462 | probe_tracker_in_robot = coord_tracker[0] |
464 | ref_tracker_in_robot = coord_tracker[1] | 463 | ref_tracker_in_robot = coord_tracker[1] |
465 | obj_tracker_in_robot = coord_tracker[2] | 464 | obj_tracker_in_robot = coord_tracker[2] |
invesalius/data/coregistration.py
@@ -20,11 +20,9 @@ | @@ -20,11 +20,9 @@ | ||
20 | import numpy as np | 20 | import numpy as np |
21 | import queue | 21 | import queue |
22 | import threading | 22 | import threading |
23 | -from time import time, sleep | ||
24 | -import csv | 23 | +from time import sleep |
25 | 24 | ||
26 | import invesalius.constants as const | 25 | import invesalius.constants as const |
27 | -import invesalius.data.coordinates as dco | ||
28 | import invesalius.data.transformations as tr | 26 | import invesalius.data.transformations as tr |
29 | import invesalius.data.bases as bases | 27 | import invesalius.data.bases as bases |
30 | 28 | ||
@@ -202,13 +200,6 @@ class CoordinateCorregistrate(threading.Thread): | @@ -202,13 +200,6 @@ class CoordinateCorregistrate(threading.Thread): | ||
202 | # | 200 | # |
203 | self.target[1] = -self.target[1] | 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 | def run(self): | 203 | def run(self): |
213 | coreg_data = self.coreg_data | 204 | coreg_data = self.coreg_data |
214 | view_obj = 1 | 205 | view_obj = 1 |
@@ -268,22 +259,6 @@ class CoordinateCorregistrate(threading.Thread): | @@ -268,22 +259,6 @@ class CoordinateCorregistrate(threading.Thread): | ||
268 | 259 | ||
269 | if not self.icp_queue.empty(): | 260 | if not self.icp_queue.empty(): |
270 | self.icp_queue.task_done() | 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 | # The sleep has to be in both threads | 262 | # The sleep has to be in both threads |
288 | sleep(self.sle) | 263 | sleep(self.sle) |
289 | except queue.Full: | 264 | except queue.Full: |
invesalius/data/elfin_robot.py
@@ -2,7 +2,6 @@ import numpy as np | @@ -2,7 +2,6 @@ import numpy as np | ||
2 | import cv2 | 2 | import cv2 |
3 | from time import time, sleep | 3 | from time import time, sleep |
4 | import threading | 4 | import threading |
5 | -import csv | ||
6 | 5 | ||
7 | import invesalius.data.elfin as elfin | 6 | import invesalius.data.elfin as elfin |
8 | import invesalius.data.transformations as tr | 7 | import invesalius.data.transformations as tr |
@@ -10,12 +9,10 @@ import invesalius.data.coregistration as dcr | @@ -10,12 +9,10 @@ import invesalius.data.coregistration as dcr | ||
10 | import invesalius.constants as const | 9 | import invesalius.constants as const |
11 | 10 | ||
12 | 11 | ||
13 | - | ||
14 | class elfin_server(): | 12 | class elfin_server(): |
15 | def __init__(self, server_ip, port_number): | 13 | def __init__(self, server_ip, port_number): |
16 | self.server_ip = server_ip | 14 | self.server_ip = server_ip |
17 | self.port_number = port_number | 15 | self.port_number = port_number |
18 | - #print(cobot.ReadPcsActualPos()) | ||
19 | 16 | ||
20 | def Initialize(self): | 17 | def Initialize(self): |
21 | SIZE = 1024 | 18 | SIZE = 1024 |
@@ -25,16 +22,14 @@ class elfin_server(): | @@ -25,16 +22,14 @@ class elfin_server(): | ||
25 | print("conected!") | 22 | print("conected!") |
26 | 23 | ||
27 | def Run(self): | 24 | def Run(self): |
28 | - #target = [540.0, -30.0, 850.0, 140.0, -81.0, -150.0] | ||
29 | - #print("starting move") | ||
30 | return self.cobot.ReadPcsActualPos() | 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 | status = self.cobot.ReadMoveState() | 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 | if status != 1009: | 30 | if status != 1009: |
36 | self.cobot.MoveL(target) | 31 | self.cobot.MoveL(target) |
37 | - elif type == const.MOTIONS["arc"]: | 32 | + elif type == const.ROBOT_MOTIONS["arc"]: |
38 | if status != 1009: | 33 | if status != 1009: |
39 | self.cobot.MoveC(target) | 34 | self.cobot.MoveC(target) |
40 | 35 | ||
@@ -45,8 +40,8 @@ class elfin_server(): | @@ -45,8 +40,8 @@ class elfin_server(): | ||
45 | def Close(self): | 40 | def Close(self): |
46 | self.cobot.close() | 41 | self.cobot.close() |
47 | 42 | ||
48 | -class KalmanTracker: | ||
49 | 43 | ||
44 | +class KalmanTracker: | ||
50 | def __init__(self, | 45 | def __init__(self, |
51 | state_num=2, | 46 | state_num=2, |
52 | cov_process=0.001, | 47 | cov_process=0.001, |
@@ -77,6 +72,7 @@ class KalmanTracker: | @@ -77,6 +72,7 @@ class KalmanTracker: | ||
77 | self.filter.correct(self.measurement) | 72 | self.filter.correct(self.measurement) |
78 | self.state = self.filter.statePost | 73 | self.state = self.filter.statePost |
79 | 74 | ||
75 | + | ||
80 | class TrackerProcessing: | 76 | class TrackerProcessing: |
81 | def __init__(self): | 77 | def __init__(self): |
82 | self.coord_vel = [] | 78 | self.coord_vel = [] |
@@ -90,7 +86,6 @@ class TrackerProcessing: | @@ -90,7 +86,6 @@ class TrackerProcessing: | ||
90 | cov_process=0.001, | 86 | cov_process=0.001, |
91 | cov_measure=0.1) for _ in range(6)] | 87 | cov_measure=0.1) for _ in range(6)] |
92 | 88 | ||
93 | - | ||
94 | def kalman_filter(self, coord_tracker): | 89 | def kalman_filter(self, coord_tracker): |
95 | kalman_array = [] | 90 | kalman_array = [] |
96 | pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten() | 91 | pose_np = np.array((coord_tracker[:3], coord_tracker[3:])).flatten() |
@@ -108,7 +103,6 @@ class TrackerProcessing: | @@ -108,7 +103,6 @@ class TrackerProcessing: | ||
108 | 103 | ||
109 | return coord_kalman | 104 | return coord_kalman |
110 | 105 | ||
111 | - | ||
112 | def estimate_head_velocity(self, coord_vel, timestamp): | 106 | def estimate_head_velocity(self, coord_vel, timestamp): |
113 | coord_vel = np.vstack(np.array(coord_vel)) | 107 | coord_vel = np.vstack(np.array(coord_vel)) |
114 | coord_init = coord_vel[:int(len(coord_vel)/2)].mean(axis=0) | 108 | coord_init = coord_vel[:int(len(coord_vel)/2)].mean(axis=0) |
@@ -118,36 +112,35 @@ class TrackerProcessing: | @@ -118,36 +112,35 @@ class TrackerProcessing: | ||
118 | 112 | ||
119 | return velocity, distance | 113 | return velocity, distance |
120 | 114 | ||
121 | - def Versores(self,init_point, final_point): | 115 | + def versors(self, init_point, final_point): |
122 | init_point = np.array(init_point) | 116 | init_point = np.array(init_point) |
123 | final_point = np.array(final_point) | 117 | final_point = np.array(final_point) |
124 | norm = (sum((final_point-init_point)**2))**0.5 | 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 | def arcmotion(self, actual_point, coord_head, coord_inv): | 123 | def arcmotion(self, actual_point, coord_head, coord_inv): |
130 | - | ||
131 | p1 = coord_inv | 124 | p1 = coord_inv |
132 | 125 | ||
133 | pc = coord_head[0], coord_head[1], coord_head[2], coord_inv[3], coord_inv[4], coord_inv[5] | 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 | actual_point[3], actual_point[4], actual_point[5] | 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 | return init_ext_point, target_arc | 145 | return init_ext_point, target_arc |
153 | 146 | ||
@@ -165,7 +158,7 @@ class TrackerProcessing: | @@ -165,7 +158,7 @@ class TrackerProcessing: | ||
165 | self.velocity_std = np.std(self.velocity_vector) | 158 | self.velocity_std = np.std(self.velocity_vector) |
166 | del self.velocity_vector[0] | 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 | print('Velocity threshold activated') | 162 | print('Velocity threshold activated') |
170 | return False | 163 | return False |
171 | else: | 164 | else: |
@@ -173,7 +166,6 @@ class TrackerProcessing: | @@ -173,7 +166,6 @@ class TrackerProcessing: | ||
173 | 166 | ||
174 | return False | 167 | return False |
175 | 168 | ||
176 | - | ||
177 | def head_move_compensation(self, current_ref, m_change_robot2ref): | 169 | def head_move_compensation(self, current_ref, m_change_robot2ref): |
178 | trans = tr.translation_matrix(current_ref[:3]) | 170 | trans = tr.translation_matrix(current_ref[:3]) |
179 | a, b, g = np.radians(current_ref[3:6]) | 171 | a, b, g = np.radians(current_ref[3:6]) |
@@ -184,8 +176,7 @@ class TrackerProcessing: | @@ -184,8 +176,7 @@ class TrackerProcessing: | ||
184 | _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new) | 176 | _, _, angles, translate, _ = tr.decompose_matrix(m_robot_new) |
185 | angles = np.degrees(angles) | 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 | def estimate_head_center(self, tracker, current_ref): | 181 | def estimate_head_center(self, tracker, current_ref): |
191 | m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion = tracker.GetMatrixTrackerFiducials() | 182 | m_probe_ref_left, m_probe_ref_right, m_probe_ref_nasion = tracker.GetMatrixTrackerFiducials() |
@@ -196,12 +187,11 @@ class TrackerProcessing: | @@ -196,12 +187,11 @@ class TrackerProcessing: | ||
196 | 187 | ||
197 | return (m_ear_left_new[:3, -1] + m_ear_right_new[:3, -1])/2 | 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 | sum = (coord_inv[0]-actual_point[0])**2\ | 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 | return correction_distance_compensation | 196 | return correction_distance_compensation |
207 | 197 | ||
@@ -216,6 +206,7 @@ class RobotCoordinates(): | @@ -216,6 +206,7 @@ class RobotCoordinates(): | ||
216 | def GetRobotCoordinates(self): | 206 | def GetRobotCoordinates(self): |
217 | return self.coord | 207 | return self.coord |
218 | 208 | ||
209 | + | ||
219 | class ControlRobot(threading.Thread): | 210 | class ControlRobot(threading.Thread): |
220 | def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event): | 211 | def __init__(self, trck_init, tracker, robotcoordinates, queues, process_tracker, event): |
221 | threading.Thread.__init__(self, name='ControlRobot') | 212 | threading.Thread.__init__(self, name='ControlRobot') |
@@ -240,15 +231,6 @@ class ControlRobot(threading.Thread): | @@ -240,15 +231,6 @@ class ControlRobot(threading.Thread): | ||
240 | self.target_linearin = None | 231 | self.target_linearin = None |
241 | self.target_arc = None | 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 | def getcoordsfromdevices(self): | 234 | def getcoordsfromdevices(self): |
253 | coord_robot_raw = self.trck_init_robot.Run() | 235 | coord_robot_raw = self.trck_init_robot.Run() |
254 | coord_robot = np.array(coord_robot_raw) | 236 | coord_robot = np.array(coord_robot_raw) |
@@ -259,6 +241,49 @@ class ControlRobot(threading.Thread): | @@ -259,6 +241,49 @@ class ControlRobot(threading.Thread): | ||
259 | 241 | ||
260 | return coord_raw, coord_robot_raw, markers_flag | 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 | def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag): | 287 | def control(self, coords_tracker_in_robot, coord_robot_raw, markers_flag): |
263 | coord_ref_tracker_in_robot = coords_tracker_in_robot[1] | 288 | coord_ref_tracker_in_robot = coords_tracker_in_robot[1] |
264 | coord_obj_tracker_in_robot = coords_tracker_in_robot[2] | 289 | coord_obj_tracker_in_robot = coords_tracker_in_robot[2] |
@@ -266,8 +291,7 @@ class ControlRobot(threading.Thread): | @@ -266,8 +291,7 @@ class ControlRobot(threading.Thread): | ||
266 | if self.robot_tracker_flag: | 291 | if self.robot_tracker_flag: |
267 | current_ref = coord_ref_tracker_in_robot | 292 | current_ref = coord_ref_tracker_in_robot |
268 | if current_ref is not None and markers_flag[1]: | 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 | if self.process_tracker.head_move_threshold(current_ref_filtered): | 295 | if self.process_tracker.head_move_threshold(current_ref_filtered): |
272 | coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered, | 296 | coord_inv = self.process_tracker.head_move_compensation(current_ref_filtered, |
273 | self.m_change_robot2ref) | 297 | self.m_change_robot2ref) |
@@ -275,62 +299,19 @@ class ControlRobot(threading.Thread): | @@ -275,62 +299,19 @@ class ControlRobot(threading.Thread): | ||
275 | self.coord_inv_old = coord_inv | 299 | self.coord_inv_old = coord_inv |
276 | 300 | ||
277 | if np.allclose(np.array(coord_inv), np.array(coord_robot_raw), 0, 0.01): | 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 | pass | 303 | pass |
280 | elif not np.allclose(np.array(coord_inv), np.array(self.coord_inv_old), 0, 5): | 304 | elif not np.allclose(np.array(coord_inv), np.array(self.coord_inv_old), 0, 5): |
281 | - # print("stop") | ||
282 | self.trck_init_robot.StopRobot() | 305 | self.trck_init_robot.StopRobot() |
283 | self.coord_inv_old = coord_inv | 306 | self.coord_inv_old = coord_inv |
284 | else: | 307 | else: |
285 | - #self.trck_init_robot.SendCoordinates(coord_inv, const.MOTIONS["normal"]) | ||
286 | distance_target = self.process_tracker.correction_distance_calculation_target(coord_inv, coord_robot_raw) | 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 | self.coord_inv_old = coord_inv | 310 | self.coord_inv_old = coord_inv |
329 | else: | 311 | else: |
330 | self.trck_init_robot.StopRobot() | 312 | self.trck_init_robot.StopRobot() |
331 | 313 | ||
332 | def run(self): | 314 | def run(self): |
333 | - | ||
334 | while not self.event.is_set(): | 315 | while not self.event.is_set(): |
335 | coords_tracker_in_robot, coord_robot_raw, markers_flag = self.getcoordsfromdevices() | 316 | coords_tracker_in_robot, coord_robot_raw, markers_flag = self.getcoordsfromdevices() |
336 | 317 | ||
@@ -348,33 +329,4 @@ class ControlRobot(threading.Thread): | @@ -348,33 +329,4 @@ class ControlRobot(threading.Thread): | ||
348 | 329 | ||
349 | self.control(coords_tracker_in_robot, coord_robot_raw, markers_flag) | 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 | \ No newline at end of file | 332 | \ No newline at end of file |
333 | + sleep(0.01) |
invesalius/data/plot_realtime.py
@@ -1,39 +0,0 @@ | @@ -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 | \ No newline at end of file | 0 | \ No newline at end of file |
invesalius/data/trackers.py
@@ -41,10 +41,10 @@ def TrackerConnection(tracker_id, trck_init, action): | @@ -41,10 +41,10 @@ def TrackerConnection(tracker_id, trck_init, action): | ||
41 | const.CAMERA: CameraTracker, # CAMERA | 41 | const.CAMERA: CameraTracker, # CAMERA |
42 | const.POLARIS: PolarisTracker, # POLARIS | 42 | const.POLARIS: PolarisTracker, # POLARIS |
43 | const.POLARISP4: PolarisP4Tracker, # POLARISP4 | 43 | const.POLARISP4: PolarisP4Tracker, # POLARISP4 |
44 | - const.OPTITRACK: OptitrackTracker, #Optitrack | 44 | + const.OPTITRACK: OptitrackTracker, #Optitrack |
45 | + const.ROBOT: RobotTracker, #Robot | ||
45 | const.DEBUGTRACKRANDOM: DebugTrackerRandom, | 46 | const.DEBUGTRACKRANDOM: DebugTrackerRandom, |
46 | - const.DEBUGTRACKAPPROACH: DebugTrackerApproach, | ||
47 | - const.HYBRID: HybridTracker} | 47 | + const.DEBUGTRACKAPPROACH: DebugTrackerApproach} |
48 | 48 | ||
49 | trck_init = trck_fcn[tracker_id](tracker_id) | 49 | trck_init = trck_fcn[tracker_id](tracker_id) |
50 | 50 | ||
@@ -66,36 +66,68 @@ def DefaultTracker(tracker_id): | @@ -66,36 +66,68 @@ def DefaultTracker(tracker_id): | ||
66 | # return tracker initialization variable and type of connection | 66 | # return tracker initialization variable and type of connection |
67 | return trck_init, 'wrapper' | 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 | trck_init = None | 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 | return trck_init, 'wrapper' | 131 | return trck_init, 'wrapper' |
100 | 132 | ||
101 | def PolarisTracker(tracker_id): | 133 | def PolarisTracker(tracker_id): |
@@ -127,7 +159,6 @@ def PolarisTracker(tracker_id): | @@ -127,7 +159,6 @@ def PolarisTracker(tracker_id): | ||
127 | # return tracker initialization variable and type of connection | 159 | # return tracker initialization variable and type of connection |
128 | return trck_init, lib_mode | 160 | return trck_init, lib_mode |
129 | 161 | ||
130 | - | ||
131 | def PolarisP4Tracker(tracker_id): | 162 | def PolarisP4Tracker(tracker_id): |
132 | from wx import ID_OK | 163 | from wx import ID_OK |
133 | trck_init = None | 164 | trck_init = None |
@@ -156,19 +187,36 @@ def PolarisP4Tracker(tracker_id): | @@ -156,19 +187,36 @@ def PolarisP4Tracker(tracker_id): | ||
156 | # return tracker initialization variable and type of connection | 187 | # return tracker initialization variable and type of connection |
157 | return trck_init, lib_mode | 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 | return trck_init, 'wrapper' | 220 | return trck_init, 'wrapper' |
173 | 221 | ||
174 | def ElfinRobot(robot_IP): | 222 | def ElfinRobot(robot_IP): |
@@ -189,57 +237,24 @@ def ElfinRobot(robot_IP): | @@ -189,57 +237,24 @@ def ElfinRobot(robot_IP): | ||
189 | # return tracker initialization variable and type of connection | 237 | # return tracker initialization variable and type of connection |
190 | return trck_init, lib_mode | 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 | trck_init = None | 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 | def DebugTrackerRandom(tracker_id): | 259 | def DebugTrackerRandom(tracker_id): |
245 | trck_init = True | 260 | trck_init = True |
@@ -351,26 +366,6 @@ def PlhUSBConnection(tracker_id): | @@ -351,26 +366,6 @@ def PlhUSBConnection(tracker_id): | ||
351 | 366 | ||
352 | return trck_init | 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 | def DisconnectTracker(tracker_id, trck_init): | 369 | def DisconnectTracker(tracker_id, trck_init): |
375 | """ | 370 | """ |
376 | Disconnect current spatial tracker | 371 | Disconnect current spatial tracker |
@@ -390,7 +385,7 @@ def DisconnectTracker(tracker_id, trck_init): | @@ -390,7 +385,7 @@ def DisconnectTracker(tracker_id, trck_init): | ||
390 | trck_init = False | 385 | trck_init = False |
391 | lib_mode = 'serial' | 386 | lib_mode = 'serial' |
392 | print('Tracker disconnected.') | 387 | print('Tracker disconnected.') |
393 | - elif tracker_id == const.HYBRID: | 388 | + elif tracker_id == const.ROBOT: |
394 | trck_init[0].Close() | 389 | trck_init[0].Close() |
395 | trck_init = False | 390 | trck_init = False |
396 | lib_mode = 'wrapper' | 391 | lib_mode = 'wrapper' |
invesalius/gui/dialogs.py
@@ -875,9 +875,9 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode): | @@ -875,9 +875,9 @@ def ShowNavigationTrackerWarning(trck_id, lib_mode): | ||
875 | const.POLARIS: 'NDI Polaris', | 875 | const.POLARIS: 'NDI Polaris', |
876 | const.POLARISP4: 'NDI Polaris P4', | 876 | const.POLARISP4: 'NDI Polaris P4', |
877 | const.OPTITRACK: 'Optitrack', | 877 | const.OPTITRACK: 'Optitrack', |
878 | + const.ROBOT: 'Robotic navigation', | ||
878 | const.DEBUGTRACKRANDOM: 'Debug tracker device (random)', | 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 | if lib_mode == 'choose': | 882 | if lib_mode == 'choose': |
883 | msg = _('No tracking device selected') | 883 | msg = _('No tracking device selected') |
@@ -3482,7 +3482,7 @@ class ObjectCalibrationDialog(wx.Dialog): | @@ -3482,7 +3482,7 @@ class ObjectCalibrationDialog(wx.Dialog): | ||
3482 | if self.trk_init and self.tracker_id: | 3482 | if self.trk_init and self.tracker_id: |
3483 | coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates() | 3483 | coord_raw, markers_flag = self.TrackerCoordinates.GetCoordinates() |
3484 | if self.obj_ref_id and btn_id == 4: | 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 | trck_init_robot = self.trk_init[1][0] | 3486 | trck_init_robot = self.trk_init[1][0] |
3487 | coord = trck_init_robot.Run() | 3487 | coord = trck_init_robot.Run() |
3488 | else: | 3488 | else: |
invesalius/gui/task_navigator.py
@@ -1045,7 +1045,7 @@ class NeuronavigationPanel(wx.Panel): | @@ -1045,7 +1045,7 @@ class NeuronavigationPanel(wx.Panel): | ||
1045 | # self.tracker.TrackerCoordinates, self.navigation.event).start() | 1045 | # self.tracker.TrackerCoordinates, self.navigation.event).start() |
1046 | #sleep(5) | 1046 | #sleep(5) |
1047 | 1047 | ||
1048 | - if self.tracker.tracker_id == const.HYBRID: | 1048 | + if self.tracker.tracker_id == const.ROBOT: |
1049 | self.robot.SetRobotQueues([self.navigation.robottarget_queue, | 1049 | self.robot.SetRobotQueues([self.navigation.robottarget_queue, |
1050 | self.navigation.objattarget_queue]) | 1050 | self.navigation.objattarget_queue]) |
1051 | self.robot.OnRobotConnection(self.tracker, self.robotcoordinates) | 1051 | self.robot.OnRobotConnection(self.tracker, self.robotcoordinates) |
@@ -1143,8 +1143,6 @@ class NeuronavigationPanel(wx.Panel): | @@ -1143,8 +1143,6 @@ class NeuronavigationPanel(wx.Panel): | ||
1143 | btn_c.Enable(False) | 1143 | btn_c.Enable(False) |
1144 | 1144 | ||
1145 | self.navigation.StartNavigation(self.tracker) | 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 | if not self.CheckFiducialRegistrationError(): | 1147 | if not self.CheckFiducialRegistrationError(): |
1150 | # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok | 1148 | # TODO: Exhibit FRE in a warning dialog and only starts navigation after user clicks ok |