Commit 329c784e2d4af2a6c7f78200d349975b6a5d9d95

Authored by Renan
1 parent acc75075
Exists in master

CLEANING

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